/******************************************************************
*******************************************************************
**
** This is proprietary unpublished source code, property
** of Genesis Software.  Use or disclosure without prior
** agreement is expressly prohibited.
**
** Copyright (c) 2025 Genesis Software, all rights reserved.
**
*******************************************************************
 ******************************************************************/

/*
** MODULATOR.C
**
** Create modulation shape
**
*/
#define WITH_TRIGGER

#include <stdio.h>
#include <string.h>

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wtype-limits"
#pragma GCC diagnostic ignored "-Wsign-compare"
#include "pico/stdlib.h"
#include "hardware/clocks.h"
#include "hardware/dma.h"
#include "hardware/interp.h"
#include "hardware/irq.h"
#include "hardware/pll.h"
#include "hardware/pio.h"
#include "hardware/pwm.h"
#include "hardware/structs/pll.h"
#include "hardware/structs/clocks.h"
#pragma GCC diagnostic pop

#include "dac.pio.h"
#include "ctrl.pio.h"
#include "trigger.pio.h"

// https://github.com/howerj/q
// Modified to be Q20.12 rather than Q16.16
#include "q/q.h"

#include "shaped-trap.h"

// Base of DAC pins
#define DACOUT_GPIO	7
// Base of ctrl pins
#define CTRLOUT_GPIO	16
// PWM output pin
#define TRIGOUT_GPIO	22
// PIO SM trigger input pin (connected to above for testing)
// Also outputs trigger on next pin
#define TRIGIN_GPIO	27

// Pulse control bits
#define PACTIVE		0x01
#define PHINV		0x02
#define SENSE1		0x04
#define SENSE2		0x08
#define GATE		0x10
#define TRSW		0x20

// Pulse shape data
uint8_t pulse_data[65536] __attribute__((aligned(4)));
// Pulse control data
uint8_t pulse_ctrl[65536] __attribute__((aligned(4)));
// PWM slice for PRF timer
unsigned slice_num = 0;

// PIO for pulse generation
PIO pulse_pio = pio0;

// DMA channel to feed DAC PIO
static int dac_dma_chan;
// DAC SM
uint dac_sm;
// Instruction offset for DAC PIO program
uint dac_pio_sm_offset;

// DMA channel to feed ctrl PIO
static int ctrl_dma_chan;
// Ctrl SM
uint ctrl_sm;
// Instruction offset for ctrl PIO program
uint ctrl_pio_sm_offset;

/*
 * Use a DMA channel to feed PIO0 SM0 with pulse data.
 * Each DMA transfer is a single pulse.
 *
 * The PIO state machine waits to be triggered before starting
 * so we can use another state machine to look for the trigger edge.
 *
 * When the DMA is done the IRQ handler will configure it for the next
 * pulse (or not if it should stop). ie reset the PIO state machine
 * back to waiting for an edge and re-arm the DMA.
 */
void
dma_handler(void) {
  // Clear the interrupt request.
  dma_hw->ints0 = 1u << dac_dma_chan;

#if 0
  printf("DAC transfers %lu\n", dma_channel_hw_addr(dac_dma_chan)->transfer_count);
  printf("Ctrl transfers %lu\n", dma_channel_hw_addr(ctrl_dma_chan)->transfer_count);
#endif

  // Reset DAQ & ctrl PIO SMs so they are  waiting for a trigger
  pio_sm_exec(pulse_pio, dac_sm, pio_encode_jmp(dac_pio_sm_offset));
  pio_sm_exec(pulse_pio, ctrl_sm, pio_encode_jmp(ctrl_pio_sm_offset));

  // Setup next pulse data & ctrl DMA addresses
  dma_channel_wait_for_finish_blocking(dac_dma_chan);
  dma_channel_set_read_addr(dac_dma_chan, pulse_data, true);
  dma_channel_wait_for_finish_blocking(ctrl_dma_chan);
  dma_channel_set_read_addr(ctrl_dma_chan, pulse_ctrl, true);
}

void
pwm_wrap(void) {
  pwm_clear_irq(slice_num);

#ifndef WITH_TRIGGER
  // Manually trigger DAQ SM (cleared by SM)
  pulse_pio->irq_force = 3;

  // 'scope trigger
  gpio_put(2, 1);
  gpio_put(2, 0);
#endif
}

// Calculate pulse shape data
// TODO: predistortion, proper sense, gate, phase, active, T/R switch
// Could encode them as bit stream like data but more compact would be
// (say) a list of counts to toggle pins at
// Need to add pre/postgate/sense/phase counters
unsigned
compute_pulse(uint8_t *data, uint8_t *ctrl, unsigned datalen, uint16_t plen, char *code, uint8_t ncode, const uint8_t *shape, uint8_t shapelen, uint8_t codegap, uint8_t slew1, uint8_t slew2, uint8_t dcofs) {
  uint32_t shapesamples, nsamples, idx, bit1startup, bit1stopup;
  q_t dcscale, stepsize;
  char tmps[20];
  interp_config cfg;

  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 / 2)));
    // Number of samples for everything
    // XXX: Need the +1 otherwise slew2 is truncated
    nsamples = shapesamples * 2 + slew1 + slew2 + 1;
  } else {
    shapesamples = plen / 2;
    nsamples = shapesamples * 2 * ncode + codegap * (ncode - 1) + slew1 + slew2 + 1;
  }

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

  // Check the requested pulse will not overflow given data
  if (nsamples > datalen) {
    printf("Pulse too long (%ld > %u)\n", nsamples, datalen);
    return 0;
  }
  // Check it is not too short
  if (shapesamples < 2) {
    printf("Pulse too short (%lu < %d)\n", shapesamples, 2);
    return 0;
  }
  // Or too long (will overflow for loop variable)
  if (qtoi(shapesamples) > 65535) {
    printf("Shape too long (%u > %d)\n", qtoi(shapesamples), 65535);
    return 0;
  }

  // Setup interp 0 lane 0 to generate index into shape table
  // Mask start is 0 because we use 8 bit samples
  cfg = interp_default_config();
  interp_config_set_shift(&cfg, QBITS);
  interp_config_set_mask(&cfg, 0, 32 - QBITS);
  interp_config_set_blend(&cfg, true);
  interp_set_config(interp0, 0, &cfg);

  // Setup interp 0 lane 1 to LERP each sample pair
  cfg = interp_default_config();
  interp_config_set_shift(&cfg, QBITS - 8);
  interp_config_set_signed(&cfg, false);
  interp_config_set_cross_input(&cfg, true); // unsigned blending
  interp_set_config(interp0, 1, &cfg);

  // Setup interp 1 lane 0 to clamp 0-255
  cfg = interp_default_config();
  interp_config_set_clamp(&cfg, true);
  interp_config_set_shift(&cfg, 0);
  interp_config_set_mask(&cfg, 0, 8);
  interp_config_set_signed(&cfg, false);
  interp_set_config(interp1, 0, &cfg);
  interp1->base[0] = 0;
  interp1->base[1] = 255;

  interp0->accum[0] = 0; // Initial offset into shape table
  interp0->base[2] = (uintptr_t)shape; // Start of shape table

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

  memset(pulse_data, 0, datalen);
  memset(pulse_ctrl, 0, datalen);
  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++) {
    if (c == 0)
      bit1startup = idx;

    uint ctrltmp = PACTIVE;
    if (code[c] == '0')
      ctrltmp |= PHINV;

    // Pulse up
    if (c == 0) {
      interp0->accum[0] = 0; // Initial offset into shape table
      interp0->base[2] = (uintptr_t)shape; // Start of shape table
    }
    for (uint16_t i = 0; i < shapesamples; i++) {
      ctrl[idx] = ctrltmp;
      if (c == 0) {
	// Get sample pair
	uint8_t *sample_pair = (uint8_t *) interp0->peek[2];
	// Ask lane 1 for a LERP, using the lane 0 accumulator
	interp0->base[0] = sample_pair[0];
	interp0->base[1] = sample_pair[1];
	uint8_t peek = interp0->peek[1];
	// Apply DC offset scaling & clamp
	interp1->accum[0] = dcofs + qtoi(qmul(qint(peek), dcscale));
	data[idx++] = interp1->peek[0];
	// Update interpolator for next point
	interp0->add_raw[0] = stepsize;
      } else
	// Already done it before, just copy the previous instance
	data[idx++] = data[bit1startup + i];
    }
    if (c == 0)
      bit1stopup = idx - 1;
    // Pulse down
    // Since the pulse is symmetrical just copy the up slope in reverse
    // XXX: if we had asymmetrical predistortion this wouldn't be true
    for (uint16_t i = 0; i < shapesamples; i++) {
      // Could replace this with a separate loop to poke it into place
      // Similarly for TR switch when implemented
      if (i == 0 && c == 0)
	ctrl[idx] = ctrltmp | SENSE1;
      else
	ctrl[idx] = ctrltmp;
      data[idx++] = data[bit1stopup - i];
    }

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

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

  data[idx++] = 0;
  ctrl[idx] = 0;

  return idx + 1;
}

int
main(void) {
    absolute_time_t then, now;

    // Set sysclk to 120MHz
    set_sys_clock_khz(120000, true);

    stdio_init_all();
    printf("\n\n\nIniting\n");

    // Needed otherwise timer related functions hang under debugging
    // https://github.com/raspberrypi/pico-sdk/issues/1152#issuecomment-1418248639
    timer_hw->dbgpause = 0;

    gpio_init(PICO_DEFAULT_LED_PIN);
    gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT);
    gpio_init(2);
    gpio_set_dir(2, GPIO_OUT);

#if 0
    // GPIO tester to check breadboard wiring
    for (unsigned i = 7; i < 7 + 9; i++) {
      printf("GPIO %d\n", i);
      gpio_init(i);
      gpio_set_dir(i, GPIO_OUT);
      printf("on\n");
      gpio_put(i, 1);
      __breakpoint();
      printf("off\n");
      gpio_put(i, 0);
      __breakpoint();
    }
#endif

    uint32_t idx;
    uint16_t plen;
    char *code;
    if (1) {
      plen = 8000;
      code = "1110010";
    } else {
      plen = 53000;
      code = "1";
    }

    uint8_t codegap = 4;
    uint8_t slew1 = 10;
    uint8_t slew2 = 10;
    uint8_t dcofs = 110;
    then = get_absolute_time();
    if ((idx = compute_pulse(pulse_data, pulse_ctrl, sizeof(pulse_data),
			     plen, code, strlen(code),
			     shaped_trap, sizeof(shaped_trap),
			     codegap, slew1, slew2, dcofs)) == 0) {
      printf("Failed to compute pulse\n");
      while (1)
	;
    }
    now = get_absolute_time();
    unsigned long long diff = absolute_time_diff_us(then, now);
    printf("Pulse computation took %lld usec and created %lu samples - %.1f nsec/sample\n",
	   diff, idx, (float)diff * 1000.0 / idx);
    unsigned transfers = (idx + 3) >> 2;
    printf("Using %u transfers\n", transfers);
    //__breakpoint();

    // Load the DAC program, and configure a free state machine
    // to run the program.
    dac_pio_sm_offset = pio_add_program(pulse_pio, &dac_program);
    if (dac_pio_sm_offset < 0) {
      printf("Unable to load DAC program\n");
      __breakpoint();
    }
    dac_sm = pio_claim_unused_sm(pulse_pio, true);
    // Data is GPIO7 to GPIO14, clock is GPIO15
    // Clock divisor of 2 so it runs at 60MHz and
    // generates a 30MHz clock
    dac_program_init(pulse_pio, dac_sm, dac_pio_sm_offset, DACOUT_GPIO, 2);

    // Configure a channel to write 32 bits at a time to PIO0
    // SM0's TX FIFO, paced by the data request signal from that peripheral.
    dac_dma_chan = dma_claim_unused_channel(true);
    dma_channel_config dac_dmac = dma_channel_get_default_config(dac_dma_chan);
    channel_config_set_transfer_data_size(&dac_dmac, DMA_SIZE_32);
    channel_config_set_read_increment(&dac_dmac, true);
    channel_config_set_dreq(&dac_dmac, PIO_DREQ_NUM(pulse_pio, dac_sm, true));

    dma_channel_configure(
        dac_dma_chan,
        &dac_dmac,
        &pulse_pio->txf[dac_sm],   // Write address
        pulse_data,                // Pulse data
        transfers,	           // Transfer count
        true			   // Start transfer
    );

    // Tell the DMA to raise IRQ line 0 when the channel finishes a block
    dma_channel_set_irq0_enabled(dac_dma_chan, true);

    // Configure the processor to run dma_handler() when DMA IRQ 0 is asserted
    irq_set_exclusive_handler(DMA_IRQ_0, dma_handler);
    irq_set_enabled(DMA_IRQ_0, true);

    // Load the ctrl program, and configure a free state machine
    // to run the program.
    ctrl_pio_sm_offset = pio_add_program(pulse_pio, &ctrl_program);
    if (ctrl_pio_sm_offset < 0) {
      printf("Unable to load ctrl program\n");
      __breakpoint();
    }
    ctrl_sm = pio_claim_unused_sm(pulse_pio, true);
    ctrl_program_init(pulse_pio, ctrl_sm, ctrl_pio_sm_offset, CTRLOUT_GPIO, 2);

    // Configure a channel to write 32 bits at a time to PIO0
    // SM0's TX FIFO, paced by the data request signal from that peripheral.
    ctrl_dma_chan = dma_claim_unused_channel(true);
    dma_channel_config ctrl_dmac = dma_channel_get_default_config(ctrl_dma_chan);
    channel_config_set_transfer_data_size(&ctrl_dmac, DMA_SIZE_32);
    channel_config_set_read_increment(&ctrl_dmac, true);
    channel_config_set_dreq(&ctrl_dmac, PIO_DREQ_NUM(pulse_pio, ctrl_sm, true));

    dma_channel_configure(
        ctrl_dma_chan,
        &ctrl_dmac,
        &pulse_pio->txf[ctrl_sm],  // Write address
        pulse_ctrl,                // Ctrl data
        transfers,	           // Transfer count
        true			   // Start transfer
    );
    // No IRQ, piggyback on the data one

#ifdef WITH_TRIGGER
    // Load the trigger program, and configure a free state machine
    // to run the program.
    uint trigger_pio_sm_offset = pio_add_program(pulse_pio, &trigger_program);
    if (trigger_pio_sm_offset < 0) {
      printf("Unable to load trigger program\n");
      __breakpoint();
    }
    uint trigger_sm = pio_claim_unused_sm(pulse_pio, true);
    trigger_program_init(pulse_pio, trigger_sm, trigger_pio_sm_offset, TRIGIN_GPIO, 2);
#endif

    //
    // Setup PWM
    // Used here to output a trigger which gets fed back into the trigger SM
    //
    // 120MHz / 250 = 480kHz base
    // Maximum divisor is only 256 which limits the low end,
    // could further subdivide in the IRQ handler
    pwm_config c = pwm_get_default_config();
    pwm_config_set_clkdiv_int(&c, 250);
    // 8Hz
    pwm_config_set_wrap(&c, 60000 - 1);

    gpio_set_function(TRIGOUT_GPIO, GPIO_FUNC_PWM);

    slice_num = pwm_gpio_to_slice_num(TRIGOUT_GPIO);
    pwm_init(slice_num, &c, true);
    pwm_clear_irq(slice_num);
    pwm_set_chan_level(slice_num, PWM_CHAN_A, 1);
    pwm_set_enabled(slice_num, 1);
    pwm_set_irq_enabled(slice_num, true);
    irq_set_exclusive_handler(PWM_IRQ_WRAP, pwm_wrap);
    irq_set_enabled(PWM_IRQ_WRAP, true);

    // Everything else from this point is interrupt-driven. The processor has
    // time to sit and think about its early retirement -- maybe open a bakery?
    while (true) {
	gpio_put(PICO_DEFAULT_LED_PIN, 1);
	sleep_ms(100);
	gpio_put(PICO_DEFAULT_LED_PIN, 0);
	sleep_ms(100);
    }

    __breakpoint();
}