#include <sys/errno.h>
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>

#if 1
#include <q.h>
#else
// We want Q24.8 - Q16.16 is too small for sample calculations
#define FPT_WBITS 24
#include "fptc-lib/src/fptc.h"
#define q_t fpt
#define qmul fpt_mul
#define qdiv fpt_div
#define qtoi fpt2i
#define qint i2fpt
#define qsub(x, y) ((x) - (y))
#define qsprint(n, s, len) fpt_str(n, s, 3)
#endif

#include "shaped-trap.h"

unsigned
compute_pulse(uint8_t *data, unsigned datalen, uint16_t plen, char *code, uint8_t ncode, uint8_t shapelen, const uint8_t *shape, uint8_t codegap, uint8_t slew1, uint8_t slew2, uint8_t dcofs) {
  uint32_t shapesamples, nsamples, idx;
  q_t dcscale, stepsize;
  char tmps[20];

  dcscale = qdiv(qsub(qint(255), qint(dcofs)), qint(255));
  qsprint(dcscale, tmps, sizeof(tmps));
  printf("dcscale = %s\n", tmps);

  if (ncode == 1) {
    // Number of samples for half of the pulse
    // Do division first so we don't overflow Q16.16
    shapesamples = qtoi(qmul(qdiv(qint(plen), qint(100)), qint(shapelen)));
    // Number of samples for everything
    nsamples = shapesamples * 2 + slew1 + slew2;
  } else {
    shapesamples = plen;
    nsamples = shapesamples * 2 * ncode + codegap * (ncode - 1) + slew1 + slew2;
  }

  // Number of samples per step in the pulse shape
  stepsize = qdiv(qint(shapesamples), qint(shapelen));
  qsprint(stepsize, tmps, sizeof(tmps));
  printf("shapelen = %d shapesamples = %u nsamples = %u stepsize = %s\n", shapelen, shapesamples, nsamples, tmps);

  if (nsamples > datalen) {
    printf("Pulse too long (%d > %u)\n", nsamples, datalen);
    return 0;
  }
  if (shapesamples < 2) {
    printf("Pulse too short (%u < %d)\n", shapesamples, 2);
    return 0;
  }
  if (qtoi(shapesamples) > 65535) {
    printf("Shape too long (%u > %d)\n", qtoi(shapesamples), 65535);
    return 0;
  }
  idx = 0;

  // Up slew
  for (uint16_t i = 0; i < slew1; i++)
    data[idx++] = qtoi(qdiv(qmul(qint(dcofs), qint(i)), qint(slew1)));

  for (uint16_t c = 0; c < ncode; c++) {
    // Pulse up
    for (uint16_t i = 0; i < shapesamples; i++)
      data[idx++] = dcofs + qtoi(qmul(qint(shape[qtoi(qdiv(qint(i), stepsize))]), dcscale));

    // Pulse down
    for (uint16_t i = 0; i < shapesamples; i++)
      data[idx++] = dcofs + qtoi(qmul(qint(shape[qtoi(qdiv(qint(shapesamples - i - 1), stepsize))]), dcscale));

    // Code gap
    if (c < ncode - 1)
      for (uint16_t i = 0; i < codegap; i++)
	data[idx++] = dcofs;
  }

  // Down slew
  for (uint16_t i = 0; i < slew2; i++)
    data[idx++] = qtoi(qdiv(qmul(qint(dcofs), qint(slew2 - i)), qint(slew2)));

  return idx - 1;
}

int
main(int argc, char **argv) {
  uint8_t data[65536], codegap, slew1, slew2, dcofs;
  uint16_t plen;
  uint32_t idx;
  char *code;

  if (argc != 7) {
    printf("Bad usage\n");
    exit(1);
  }
  plen = atoi(argv[1]);
  code = argv[2];
  codegap = atoi(argv[3]);
  slew1 = atoi(argv[4]);
  slew2 = atoi(argv[5]);
  dcofs = atoi(argv[6]);

  if ((idx = compute_pulse(data, sizeof(data), plen, code, strlen(code), sizeof(shaped_trap), shaped_trap, codegap, slew1, slew2, dcofs)) == 0) {
    printf("Failed to build pulse\n");
    exit(1);
  }

  FILE *fh;
  if ((fh = fopen("/tmp/out.bin", "w")) == NULL) {
    printf("Unable to open file: %s\n", strerror(errno));
    exit(1);
  }
  fwrite(data, idx, 1, fh);
  fclose(fh);

  exit(0);
}