/**
 * Bruce Land and Hunter Adams (vha3@cornell.edu)
 * 
 * PDE string
 * F = Fs*sqrt(rho)/(2*(string_size-2)). 
 * so rho= [F/Fs * 2 * (string_size-2)]^2
 *
 * bowing
 * If (dui)/dt is the string velocity at some point on the string, i, 
 * and vbow is the bow speed set by the user, then define
Vrel = (dui)/dt - vbow
and the input delta position added to the PDE solution at point i is
Fbow = Abow * sign(Vrel) * (eps + exp(-abs(Vrel)/v_width))
Where Abow is a drive force, v_width is a speed scale, and eps is a small constant, all set by the user.
In this implementation the exponential is approximated by a simple square pulse
Fbow = current_bow_amp * sign(Vrel) * (bow_eps +(abs(Vrel)<bow_v_width)?1.0f :0.0f) ;

Settable parameters:
pluck: amp position type width
bow:   amp position eps velocity_width  Velocity_bow 
string: damping output_position

 * Protothreads v1.1.1
 * Serial console on GPIO 0 and 1 for debugging
 * and for synth conrol
 * 
 * DAC :
 * GPIO 5 (pin 7) Chip select
   GPIO 6 (pin 9) SCK/spi0_sclk
   GPIO 7 (pin 10) MOSI/spi0_tx
   3.3v (pin 36) -> VCC on DAC 
   GND (pin 3)  -> GND on DAC
 * 
 * =====================================================
 * 
 * There are two threads running on each core, plus the synthesis ISR running on core1
 * The PDE synthesis woks nicely with s15x16 fixed point, but fails with s1x14, probably
 * due to excessive runcation eror.
 * 
 * ==========
 * Support routines:
 * -- ADC setup
 * -- Define two different fixed point schemes
 *      One for DDS which requires bigger numbers, the other for FFT which needs fast execution.
 * -- Set up a low-level timer alarm which truggers the synthesis ISR
 * 
 * ==========
 * Core0:
 * 
 * -- serial thread which gets parameter
 * 
 * ==========
 * Core1:
 * 
 * -- synthesis ISR triggered by a timer alarm
 * ---- runs at 40KHz (25 uSec interval) for audio synthesis.
 * ---- computes fulll string PDE
 * ----  max length of sring is >100 at 40 KHz.
 * ---- more useful lengths are 20 to 50 because of
 *      a max rho of 1.0, which limits the frequency of longer strings
 * ---- Pushes output to an SPI channel
 * ---- Fills an array with DDS results for the FFT thread
 * 
 * -- A play thread which
 * ---- just repeats a note.
 * 
 * PIO: There are three PIO state machines that generate the video.
 * See: https://vanhunteradams.com/Pico/VGA/VGA.html
 */
// ==========================================
// === VGA graphics library
// ==========================================
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "pico/stdlib.h"
#include "hardware/pio.h"
#include "hardware/adc.h"
#include "hardware/irq.h"
#include "hardware/spi.h"

// ==========================================
// === hardware and protothreads globals
// ==========================================
#include "hardware/sync.h"
#include "hardware/timer.h"
#include "pico/multicore.h"
#include "string.h"
// protothreads header
#include "pt_cornell_rp2040_v1_3.h"

#define sign(a) (((a)>0)?1.0f:-1.0f) 
#define abs(a)  (((a)>0)?a:-a)


// ==========================================
// === set up SPI DAC
// ==========================================
// All SPI DAC setup was gotten from HUnter Adams
// https://vanhunteradams.com/Pico/TimerIRQ/SPI_DDS.html
// DAC parameters
// A-channel, 1x, active
#define DAC_config_chan_A 0b0011000000000000
// B-channel, 1x, active
#define DAC_config_chan_B 0b1011000000000000

//SPI configurations
#define PIN_CS   5
#define PIN_SCK  6
#define PIN_MOSI 7
#define SPI_PORT spi0

// data for the spi port
uint16_t DAC_data ; 

// ==========================================
// === set up DDS and timer ISR
// ==========================================
// 1/Fs in microseconds
// 40 KSamples/sec
volatile int alarm_period = 25 ;

// inputs
float Fs = 40000 ;
//   Hz          C3     D3     E3     F3     G3     A3     B3     C4
float notes[8]={130.8, 146.8, 164.8, 174.6, 196.0, 220.0, 246.9, 261.6};
//
volatile int note_start = true ;
int note_index ;
#define string_size 50
#define copy_len 4*string_size

float string_n[string_size], string_n_1[string_size], new_string[string_size], new_string_temp;
float init_string[4][string_size];

// pluck parameters at t=0
float pluck_amp = 1.0f ;
int pluck_type = 0 ;
int pluck_pos = 10, pluck_width = 5;

// locaqtion to take output
 int output_pos = 15 ;

// bowing parameters
int bow_pos = 10 ;
int note_time = 0 ;
// see inital pgm comment for explaination of these
float bow_amp =0.05f, bow_eps = 0.01f, bow_v_width = 0.01f, Vbow = 0.05f ;  //0.05
float bow_attack = 0.2f, bow_decay = 0.5f ;
int bow_attack_int = 8000, bow_decay_int = 20000 ;
float Fbow ;
float current_bow_amp ;

// speed of sound 9<=rho <= 1.0
float rho = 0.5f ;
// damping: useul range of 0.99 to  0.999999
// larger number is less damped
float eta = 0.9999f;
float note_dur = 1.5 ;

// flag for serial thread to tell ISR to play
int play_scale = true ;

// =========================================
// === serial formatting macros
// to make terminal input nicer
// =========================================
#define new_screen printf("\14")
#define cursor_pos(line,col) printf("\033[%d;%dH", line, col)
#define Boldface printf("\33[1m")
#define Normaltext printf("\33[0m") // (undoes all text highlighting and color)
#define Greentext printf("\33[32m")
#define BoldGreentext printf("\33[1;32m")
// 256 MODE COLOR see 
// https://gist.github.com/fnky/458719343aabd01cfb17a3a4f7296797
#define SetTextColor(c) printf("\33[38;5;%dm", c);
#define SetBkgdColor(c) printf("\33[48;5;%dm", c);

// ==================================================
// ===  parameter printing core1
// ==================================================
// 

//int print_col[3] = {50, 250, 450};
//int print_row[5] = {60, 80, 100, 120, 140 } ;
//char print_str[30] ;
// shoot all the paramaters to the VGA display
void print_all(){
    new_screen ;
    cursor_pos(1,5); Boldface ;SetTextColor(220); SetBkgdColor(239);
    printf("PDE String Synth for rp2350, ece4760 Cornell University, 2025");

    cursor_pos(2,0); Normaltext ;
    printf( "pluck amp=%5.3f   ", pluck_amp);

    cursor_pos(3,0);
    printf( "pluck_type (0-2)=%d   ", pluck_type);

    cursor_pos(4,0);
    printf("pluck_width (1-15)=%d   ", pluck_width);

    cursor_pos(5,0);
    printf("pluck_pos (2-48)=%d   ", pluck_pos);


    cursor_pos(2,50); 
    printf( "damping=%5.7f   ", eta);

    cursor_pos(3,50); 
    printf("note duration=%5.2f   ", note_dur);

    cursor_pos(4,50); 
    printf( "output pos=%d   ", output_pos);

    cursor_pos(2,25); 
    printf( "bow attack=%5.3f   ", bow_attack);

    cursor_pos(3,25); 
    printf( "bow decay=%5.3f   ", bow_decay);

    cursor_pos(4,25); 
    printf( "bow amp=%7.5f   ", bow_amp);

    cursor_pos(5,25); 
    printf( "bow position=%d   ", bow_pos);

    //commands
    cursor_pos(6,0); Boldface ;SetTextColor(40); SetBkgdColor(239);
     printf("Commands\n\r");
    Normaltext;
    Greentext ;
      printf("play/stop scale\n\r"); 
      printf("-----\n\r");       
    //
    printf("pluckamp amp --  pluck amp (0-1.0)\n\r") ; 
    printf("pluckpos pos --  pluck position (2-48) for type 0 gaussian\n\r") ; 
    printf("plucktype type -- 0-2 -- gaussian, sawtooth, noise\n\r") ;  
    printf("pluckwidth  width -- 1 to 20 for type 0 gaussian \n\r") ;  
    printf("-----\n\r");
    printf("bowattack time -- rise time in seconds\n\r");
    printf("bowdecay time -- fall time in seconds\n\r"); 
    printf("bowamp amplitude -- 0 to 0.2 or so \n\r"); 
    printf("bowpos position  -- 2 to 48 \n\r"); 
    printf("-----\n\r");
    //
    printf("damping coeff -- 0.8 to 1.0\n\r");
    printf("notedur duration -- seconds  \n\r");
    printf("out position -- 2 to 48  \n\r");

    //
    printf("\n\r");
    Normaltext ;
}
// ==========================================
// === set up timer ISR  used in this pgm
// ==========================================
// === timer alarm ========================
// !! modifiying alarm zero trashes the cpu 
//        and causes LED  4 long - 4 short
// !! DO NOT USE alarm 0
// This low-level setup is ocnsiderably faster to execute
// than the hogh-level callback

#define ALARM_NUM 1
#define ALARM_IRQ timer_hardware_alarm_get_irq_num(timer_hw, ALARM_NUM)
// ISR interval will be 10 uSec
//
// the actual ISR
void compute_sample(void);
//
static void alarm_irq(void) {
    // mark ISR entry
    gpio_put(2,1);
    // Clear the alarm irq
    hw_clear_bits(&timer_hw->intr, 1u << ALARM_NUM);
    // arm the next interrupt
    // Write the lower 32 bits of the target time to the alarm to arm it
    timer_hw->alarm[ALARM_NUM] = timer_hw->timerawl + alarm_period ;
    //
    // the routine which actually does the PDE
    compute_sample();

    // mark ISR exit
    gpio_put(2,0);
}

// set up the timer alarm ISR
static void alarm_in_us(uint32_t delay_us) {
    // Enable the interrupt for our alarm (the timer outputs 4 alarm irqs)
    hw_set_bits(&timer_hw->inte, 1u << ALARM_NUM);
    // Set irq handler for alarm irq
    irq_set_exclusive_handler(ALARM_IRQ, alarm_irq);
    // Enable the alarm irq
    irq_set_enabled(ALARM_IRQ, true);
    // Enable interrupt in block and at processor
    // Alarm is only 32 bits 
    uint64_t target = timer_hw->timerawl + delay_us;
    // Write the lower 32 bits of the target time to the alarm which
    // will arm it
    timer_hw->alarm[ALARM_NUM] = (uint32_t) target;   
}

///==============================================
// === the actual user input
// ==============================================

static PT_THREAD (protothread_serial(struct pt *pt))
{
  PT_BEGIN(pt);
    
    static char serial_buffer[40];
    static char cmd[16], arg1[16], arg2[16], arg3[16] ;
    static char* token ;
    static int bad_cmd ;
    float farg ;
    int arg ;

    //
    print_all() ;
    
    //
    while(1) {

      // print prompt
        cursor_pos(23,0); 
        printf("                    ");
        cursor_pos(23,0); 
        printf("PDE cmd> ");
        // spawn a thread to do the non-blocking write
       // serial_write ;

        // spawn a thread to do the non-blocking serial read
         serial_read ;
        // tokenize
        token = strtok(pt_serial_in_buffer, "  ");
        strcpy(cmd, token) ;
        token = strtok(NULL, "  ");
        strcpy(arg1, token) ;
        token = strtok(NULL, "  ");
        strcpy(arg2, token) ;
        token = strtok(NULL, "  ");
        strcpy(arg3, token) ;
      // conversion to intrnal units
      // increment = Fout/Fs * 2^32
      // octave number is based on a C3 to C4 table
      // parse by command
        //
        if(strcmp(cmd,"play")==0){
            play_scale = true ;
        }

        else if(strcmp(cmd,"stop")==0){
            play_scale = false ;
        }

        else if(strcmp(cmd,"pluckamp")==0){
            sscanf(arg1,"%f", &pluck_amp) ;
        }

        else if(strcmp(cmd,"plucktype")==0){
            sscanf(arg1,"%d", &pluck_type) ;
        }

        else if(strcmp(cmd,"pluckpos")==0){
            sscanf(arg1,"%d", &pluck_pos) ;
            for (int i=1; i<string_size-2; i++){
              init_string[0][i] = (1000 *  exp(-(float)(i - pluck_pos)*(i - pluck_pos)/(pluck_width * pluck_width))); 
            }
        }

        else if(strcmp(cmd,"pluckwidth")==0){
            sscanf(arg1,"%d", &pluck_width) ;
            for (int i=1; i<string_size-2; i++){
              init_string[0][i] = (1000 *  exp(-(float)(i - pluck_pos)*(i - pluck_pos)/(pluck_width * pluck_width))); 
            }
        }

        else if(strcmp(cmd,"damping")==0){
            sscanf(arg1, "%f", &farg);
            eta = (farg) ;
        }
        
        else if(strcmp(cmd,"notedur")==0){
            sscanf(arg1, "%f", &farg);
            note_dur = (farg) ;
        }

         else if(strcmp(cmd,"out")==0){
            sscanf(arg1, "%d", &output_pos);
        }

        else if(strcmp(cmd,"bowattack")==0){
            sscanf(arg1, "%f", &bow_attack);
            bow_attack_int = (int)(bow_attack * Fs) ;
        }

        else if(strcmp(cmd,"bowdecay")==0){
            sscanf(arg1, "%f", &bow_decay);
            bow_decay_int = (int)(bow_decay * Fs) ;
        }

        else if(strcmp(cmd,"bowamp")==0){
            sscanf(arg1, "%f", &bow_amp);
        }

         else if(strcmp(cmd,"bowpos")==0){
            sscanf(arg1, "%d", &bow_pos);
        }

        else {
            cursor_pos(22,0); 
            printf("***huh? bad command*** ");
            bad_cmd = true ;
        }
        //
        if(!bad_cmd) print_all();
        bad_cmd = false;
      

      // NEVER exit while
    } // END WHILE(1)
  PT_END(pt);
} // serial thread

// ==================================================
// === play the scale thread on core 1
// ==================================================
static PT_THREAD (protothread_play(struct pt *pt))
{
    PT_BEGIN(pt);
    
     // data structure for interval timer
     PT_INTERVAL_INIT() ;

      while(1) {
        static int i ;
        PT_YIELD_usec(1000) ;
        if(play_scale){

            //mod_attack_inc = div(current_depth, mod_attack_time) ;
            //mod_decay_inc = div(current_depth, mod_decay_time) ;
            for( i=0; i<8; i++){
               // current_main_inc = main_inc[i] ;
                rho= (pow((notes[i]/Fs * 2.0f * (string_size-2)), 2.0f))/4.0f ; 
                //printf("rho %f\n", rho );
                note_start = true ;
                //PT_YIELD_usec(12000) ;
               // printf("amp %f %f\n", current_bow_amp, Fbow) ;
                // play until too small for DAC
               // PT_YIELD_UNTIL(pt, current_amp<onefix);
                PT_YIELD_usec(note_dur*1000000) ;
            }
        }
        //
        // NEVER exit while
      } // END WHILE(1)
  PT_END(pt);
} // play thread

// ==================================================
// === ISR routine -- RUNNING on core 1
// ==================================================
// 
void compute_sample(void)
{
    // === 
    
    // start a burst on new data
    if(note_start) {
      // reset the start flag
      note_start = false ;
      note_time = 0 ;
      // init the drive amplitude 
      current_bow_amp = 0.0f ;
      // init pde along string
			// pluck the string
			// this is a set up for zerro initial velocity with
			// the strike as a position
			for (int i=1; i<string_size-2; i++){
				// Gaussian pluck
				string_n[i] = pluck_amp * init_string[pluck_type][i];
				string_n_1[i] = string_n[i] ;      	
			}

			string_n[0] = 0.0f ;
			string_n_1[0] = 0.0f ;
			string_n[string_size-1] = 0.0f ;
			string_n_1[string_size-1] = 0.0f ;
    } // note start

    // play the PDE
    
    // bowing
    if (note_time < 20){
      current_bow_amp = bow_amp ;
    }
    else if(note_time==20){
      current_bow_amp = 0 ;
    }
    else if (note_time<bow_attack_int){
      current_bow_amp += 1/(float)bow_attack_int * bow_amp ;
    }
    else if(note_time < bow_attack_int+bow_decay_int){
      current_bow_amp -= 1/(float)bow_decay_int * bow_amp ;
    }
    else {
      current_bow_amp = 0.0f ;
    }
    //
    //current_bow_amp = bow_amp;
    float Vrel = (string_n[bow_pos] - string_n_1[bow_pos] - Vbow) ;
    Fbow = current_bow_amp * sign(Vrel) * (bow_eps +(abs(Vrel)<bow_v_width)?1.0f :0.0f) ;

    // the PDE
    for (int i=1; i<string_size-1; i++){          
        new_string_temp = (rho * (string_n[i-1] + string_n[i+1] - 2.0f *(string_n[i]))) ;
        new_string[i] = eta * (new_string_temp + 2.0f *(string_n[i]) - eta*(string_n_1[i])) ;       
    }
    new_string[bow_pos] += Fbow ;
    new_string[bow_pos-1] += 0.5f * Fbow ;
    new_string[bow_pos+1] += 0.5f * Fbow ;
    new_string[bow_pos-2] += 0.25f * Fbow ;
    new_string[bow_pos+2] += 0.25f * Fbow ;

    memcpy(string_n_1, string_n, copy_len);
    memcpy(string_n, new_string, copy_len);
    
    int temp = (int)(string_n[output_pos]);
    DAC_data = (DAC_config_chan_A | (( temp + 1048) & 0xfff))  ;

    // Write data to DAC
    // spi_write16_blocking(SPI_PORT, &DAC_data, 1) ;
    // nonblocking SPI write
    spi0_hw->dr = DAC_data ;
      // move time ahead
      //note_time += onefix ;
      // save in buffer for FFT

    note_time++ ;      
} // end ISR call


// ========================================
// === core 1 main -- started in main below
// ========================================
void core1_main(){ 

    // fire off synthesis interrupt
    alarm_in_us(alarm_period);
    //  === add threads  ====================
    // for core 1
    //pt_add_thread(protothread_FM) ;
   //pt_add_thread(protothread_fft) ;
    pt_add_thread(protothread_play) ;
    //
    // === initalize the scheduler ==========
    pt_schedule_start ;
    // NEVER exits
    // ======================================
}

// ========================================
// === core 0 main
// ========================================
int main(){

  // start the serial i/o
  stdio_init_all() ;
  // announce the threader version on system reset
  printf("\n\rProtothreads RP2040 v1.3 two-core, priority\n\r");

  // Initialize SPI channel (channel, baud rate set to 20MHz)
  // connected to spi DAC
spi_init(SPI_PORT, 20000000) ;
// Format (channel, data bits per transfer, polarity, phase, order)
spi_set_format(SPI_PORT, 16, 0, 0, 0);

// Map SPI signals to GPIO ports
//gpio_set_function(PIN_MISO, GPIO_FUNC_SPI);
gpio_set_function(PIN_SCK, GPIO_FUNC_SPI);
gpio_set_function(PIN_MOSI, GPIO_FUNC_SPI);
gpio_set_function(PIN_CS, GPIO_FUNC_SPI) ;
// monitor ISR
 gpio_init(2) ;	
 gpio_set_dir(2, GPIO_OUT) ;

  for (int i=1; i<string_size-2; i++){
				// Gaussian pluck
				init_string[0][i] = (1000*  exp(-(float)(i - pluck_pos)*(i - pluck_pos)/(pluck_width * pluck_width))); 
        // sawtooth
        init_string[1][i] =  ((i<11)?(float)(i * 100) : (i<22)? (float)(1000 - i * 100) : 0 );   
        // noise
        init_string[2][i] = (float)((rand() % 1024) - 515);	
			}
     
  // start core 1 threads
  multicore_reset_core1();
  multicore_launch_core1(&core1_main);

  // === config threads ========================
  // for core 0
  pt_add_thread(protothread_serial);
  //
  // === initalize the scheduler ===============
  pt_schedule_start ;
  // NEVER exits
  // ===========================================
} // end main