/**
 * Bruce Land and Hunter Adams (vha3@cornell.edu)
 * ======================================================
 * Karpus-Strong string synth
 * pluck and bow simulation
 * ======================================================
 * 
 * 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
 * 
 * ==========
 * Core0:
 * 
 * ==========
 * Core1:
 * 
 * -- synthesis ISR triggered by a timer alarm
 * ---- runs at 20KHz (50 uSec interval) for audio synthesis.
 * ---- Worst case execution time is about 5 uSec.
 * ---- Computes Karplus-Strong string sound
 * ---- Pushes output to an SPI channel
 * ---- Fills an array with DDS results for the FFT thread
 * 
 * ---- Handles the serial interface for setting parameters
 * ---- precomputes a bunch of fixed point constants to make the ISR faster
 * 
 * -- A play thread which
 * ---- sequences a C scale of 8 notes.
 * 
 * ===========================================
 * KS setup
 * see also 
 * Extensions of the Karplus-Strong Plucked-String Algorithm
   David A. Jaffe, Julius O. Smith
   Computer Music Journal, Vol. 7, No. 2 (Summer, 1983), pp. 56-69
   -----------
 * for one octave scale C4 to B4
 *   notes C4	   C#	    D	    Eb	    E	    F	   F#	   G	   G#	   A	  Bb	    B4
          261.6	277.2	293.7	311.1	329.6	349.2	370.0	392.0	415.3	440.0	466.2	493.9
now compuate the string lengths as string_length = 20000/note_freq
 so lengths are 76.4526   72.1501   68.0967   64.2880   60.6796   57.2738   54.0541   51.0204 48.1580   45.4545   42.9000   40.4940
but a shift reqister cannot have fractional stages, os shift register lengths are
 string_length[n_note] = {76,  72,  68,  64,  60,  57,  54,  51,  48,  45,  42, 40};
a fractional length is added by using a one-sample maximun phase shifter which adds time coresponding
to the following fractional sample values
  fine_tune[n_note] = {.45, .15, .10, .29, .68, .27, .05, .02, .26, .45, .90, .49};
 if eta = (1-fine_tune)/(1+fine_tune)
 then
 tuning_out =  (eta * (tuning_input - last_tune_out)) + last_tune_in ;
 the
 full algorithm then is shift, lowpass filter, tuning filter, feed back into shifter
 */
// ==========================================
// === VGA graphics library
// ==========================================
#include <stdio.h>
#include <stdlib.h>
#include <string.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"

float serial_value ;
char serial_value_input[32] ;
int play_scale = true ;

// =========================================
// === serial formatting macros
// =========================================
#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);

// =========================================
// === KS parameters
// =========================================
// sample rate
 float Fs = 40000 ;
// # of notes in one octave
#define n_note 12 
// index into the note tables
volatile int current_note ;
// max size ~100 Hz -- could be bigger
#define max_string_size 200
float string[max_string_size] ;
float init_string[4][max_string_size];

// simulationg input
int bow_type = 1 ;
float bow_table[4][256] ;
float bow_phase_accum ;
// length of table divided by length of string
// give the stride thru the table
float bow_phase_inc[n_note] ={(256/77.0), (256/73.0), (256/68.0), (256/64.0),
                             (256/60.0), (256/57.0), (256/55.00), (256/52.00),
                              (256/48.0), (256/45.0), (256/42.0), (256/40.0)} ;
// current sring pointers
int ptrin, ptrout ;

// set up the sring lengths for each note = Fs/(note requency)
//  notes C4	C#	    D	    Eb	    E	    F	   F#	   G	    G#	   A	    Bb	    B4
//        261.6	277.2	293.7	311.1	329.6	349.2	370.0	392.0	415.3	440.0	466.2	493.9
//  matlab length 76.4526   72.1501   68.0967   64.2880   60.6796   57.2738   54.0541   51.0204
//                48.1580   45.4545   42.9000   40.4940
// but a shift register has to have integer length, so:
volatile int   string_length[n_note] = {76,  72,  68,  64,  60,  57,  54,  51,  48,  45,  42, 40};
// leftover fractional length
//volatile fixAccum  fine_tune[n_note] = {.45, .15, .10, .29, .68, .27, .05, .02, .26, .45, .90, .49};
// so make a all-pass phase shfiter with a max shift equivalent to one shift-sregister slot.
// eta = (1-fine_tune)/(1+fine_tune)
// actual filter using the eta parameter is in the ISR
volatile float  eta [n_note] = {(0.3768), (0.739), (0.8237), 
                              (0.5528), (0.1908), (0.57), (0.8974), 
                              (0.96),(0.727), (0.375),( 0.0526), (0.3387) }; 

// chooses the waveform of energy put on the string when plucked
// 0 is random, 1 is lowpassed random;  2 is gaussian, 3 is sawtooth
int current_pluck = 0 ;
bool pluck_enable = true ;
// for the gaussian
int pluck_pos = 20, pluck_width = 10, output_pos = 15 ;

// various filter variables for string
volatile float tuning_out, last_tune_out, last_tune_in ;
volatile float lowpass_out  ;
// single pole IIR lowpass
volatile float  low_pass_coeff = (0.95) ; // close to classic Karplus Strong

// damping coefficient reduction in amp per synthesis sample
// mmust be <=1.0
volatile float  damping_out, damping_coeff = (0.995)  ;
float final_damping = (0.25);

int note_duration = 1000000 ; // 1 sec


// driving the string (as if bowing)
// time since pluck
int sample_number ;
float drive_amp = 0 ;
// rise/fall time of impulse in samples
float drive_rise=1e4, drive_fall=4000 ;
float drive_rise_inc, drive_fall_inc ;
float current_drive_amp, string_input ;
// bow frequency relative to string fundamental
float bow_freq = 2.3 ;


// ==========================================
// === 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 ;
// signals the ISR to pluck the string
volatile int note_start = true ;
 
// ==========================================
// === 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;   
}


// ==================================================
// ===  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("Karplus-Strong String Synth for rp2350, ece4760 Cornell University, 2025");

    cursor_pos(3,0); Normaltext ;
    printf( "pluck (1/0)=%d   ", pluck_enable);

    cursor_pos(3,25);
    printf( "plucktype (0-3)=%d   ", current_pluck);

    cursor_pos(3,50);
    printf("pluckwidth (1-15)=%d   ", pluck_width);

    cursor_pos(4,0); 
    printf( "damping=%5.3f   ", (damping_coeff));

    cursor_pos(4,25); 
    printf( "lowpass=%5.3f   ", (low_pass_coeff));

    cursor_pos(4,50); 
    printf("note duration=%5.2f   ", (float)note_duration/1e6);

    cursor_pos(5,0); 
    printf( "bow attack=%5.3f   ", (drive_rise)/Fs);

    cursor_pos(5,25); 
    printf( "bow decay=%5.3f   ", (drive_fall)/Fs);

    cursor_pos(5,50); 
    printf( "bow amp=%5.0f   ", (drive_amp));

    cursor_pos(6,0); 
    printf( "bow frequency=%5.3f   ", bow_freq);

    cursor_pos(6,25); 
    printf("bow type=%d   ", bow_type);

    cursor_pos(7,0); 
    //commands
    Greentext ;
    printf("\n\r***commands***\n\rplay -- play scale\n\r");          
    printf("stop -- playng\n\r");
    //
    printf("pluck 1/0 -- enable initial pluck\n\r") ; 
    printf("plucktype 0-3 -- noise, lp noise, gaussian, sawtooth\n\r") ;  
    printf("pluckwidth -- 1 to 20 for type 2 gaussian \n\r") ;  
    //
    printf("bowattack time -- rise time in seconds\n\r");
    printf("bowdecay time -- fall time in seconds\n\r"); 
    printf("bowamp amplitude -- integer 0 to 1000 \n\r"); 
    printf("bowtype 0-1 -- sine/noise \n\r"); 
    printf("bowfreq relative_freq -- relative to string fundamental\n\r");
    //
    printf("damping coeff -- 0.8 to 1.0\n\r");
    printf("lowpass coeff -- 0.5 to 1.0  \n\r");
    printf("tempo time -- seconds  \n\r");
    //
    printf("\n\r");
    Normaltext ;
}

///==============================================
// === 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("KS 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,"pluck")==0){
            sscanf(arg1,"%d", &pluck_enable) ;
        }

        else if(strcmp(cmd,"plucktype")==0){
            sscanf(arg1,"%d", &current_pluck) ;
        }

        else if(strcmp(cmd,"pluckwidth")==0){
            sscanf(arg1,"%d", &pluck_width) ;
            for (int i=0; i<max_string_size; i++){
            // Gaussian pluck
              init_string[2][i] = (2000 * exp(-(float)(i - pluck_pos)*(i - pluck_pos)/(pluck_width * pluck_width)));  
            }
        }

        else if(strcmp(cmd,"bowattack")==0){
            sscanf(arg1, "%f", &farg);
            drive_rise = (farg * Fs) ;
            drive_rise_inc = (drive_amp / drive_rise) ;
        }

        else if(strcmp(cmd,"bowdecay")==0){
            sscanf(arg1, "%f", &farg);
             drive_fall = (farg * Fs) ;
            drive_fall_inc = (drive_amp / drive_fall) ;
        }

         else if(strcmp(cmd,"bowamp")==0){
            sscanf(arg1, "%f", &farg);
            drive_amp = (farg) ;
            drive_fall_inc = (drive_amp / drive_fall) ;
            drive_rise_inc = (drive_amp / drive_rise) ;
        }

         else if(strcmp(cmd,"bowfreq")==0){
            sscanf(arg1, "%f", &farg);
            bow_freq = (farg) ;
            // bowing tables for DDS
            for (int i=0; i<256; i++){
              bow_table[0][i] = (1000 * sin(bow_freq*6.28*(float)i/256.)) ;
              //bow_table[1][i] = float_to_s15x16(1000 * sin(4.2*3.14149*(float)i/256.)) ;
              bow_table[1][i] = (float) ((rand() % 2048) - 1024)  ;
            }
        }

        else if(strcmp(cmd,"bowtype")==0){
            sscanf(arg1,"%d", &bow_type) ;
        }

         else if(strcmp(cmd,"damping")==0){
            sscanf(arg1, "%f", &farg);
            damping_coeff = (farg) ;
        }

         else if(strcmp(cmd,"lowpass")==0){
            sscanf(arg1, "%f", &farg);
            low_pass_coeff = (farg) ;
        }

         else if(strcmp(cmd,"tempo")==0){
            sscanf(arg1, "%f", &farg);
            note_duration = (int)(farg * 1e6) ;
        }

        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);
} // timer 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() ;
     // compute rise/fall of drive
     drive_rise_inc = (drive_amp / drive_rise) ;
     drive_fall_inc = (drive_amp / drive_fall) ;

      while(1) {
        static int i ;
        PT_YIELD_usec(1000) ;
        if(play_scale){

            for(i=0; i<n_note; i++){
                current_note = i ;
                note_start = true ;
                PT_YIELD_usec(note_duration) ;
            }
        }
        //
        // NEVER exit while
      } // END WHILE(1)
  PT_END(pt);
} // play thread

// ==================================================
// === ISR routine -- RUNNING on core 1
// ==================================================

void compute_sample(void)
{
    
    // ==== start a string with new data ===
    if(note_start) {
      // reset the start flag
      note_start = false ;
      // load the string
      for (int i=0; i<=string_length[current_note]; i++) {
         if (pluck_enable) string[i] =  init_string[current_pluck][i] ; 
         else string[i] = 0. ;
      } 
      // reset the pointers
      ptrin = 1;
      ptrout = 2;
      // pluck time marker
      sample_number = 0 ;
      // drive starts at zero
      current_drive_amp = 0 ;
      lowpass_out = 0 ;
      tuning_out = 0 ;
      bow_phase_accum = 0 ;
      //
    } // note start

    // ==== play the KS ===
    //else if(play_scale) {
      
      // bowing DDS, even if not used
      bow_phase_accum += bow_phase_inc[current_note] ;
      if (bow_phase_accum>(255)) bow_phase_accum = 0 ; ///

      // === low pass filter ===
      // the following is the classic KS lowpass
      //lowpass_out =  ((string[ptrin] + string[ptrout])>>1) ;
      //
      //this is a 1 pole IIR lowpass, which i prefer
      lowpass_out = (low_pass_coeff * (string[ptrin] - lowpass_out)) + lowpass_out ;
      // now add the lumped damping
      damping_out = (damping_coeff * lowpass_out) ;
      // and kill the energy at the end of the note
      //if(((50*sample_number) > (note_duration-20000) && pluck_enable==0)) damping_out = (final_damping * damping_out) ;

      // === tuning all-pass filter ===
      tuning_out=  (eta[current_note] * (damping_out - last_tune_out)) + last_tune_in ;
      // all-pass state vars
      last_tune_out = tuning_out;
      last_tune_in = damping_out ; //lowpass_out;

      // input calculations to generate the envelope of the bow drive
    if ((sample_number)<(int)(drive_rise + drive_fall)){
      // adjust scale so that drive  ~1
        // and sample number is periodic with the length of the sting
        string_input = ((current_drive_amp)/4000 * bow_table[bow_type][(int)(bow_phase_accum)] );

        current_drive_amp = ((sample_number) < (int)(drive_rise))? 
            current_drive_amp + drive_rise_inc : 
            current_drive_amp - drive_fall_inc ;
    }
    else {
        string_input = 0 ;
        current_drive_amp = 0 ;
    }

      // === string feedback ===
      // feedback the calculation into the shift register
      // depending on the damping, need to attenuate input
      string[ptrin] = tuning_out + (string_input/4) ; ///(ptrin==20)?(string_input) : 0 ;(sample_number)?
      // could add input here

      // === DAC sscaling and output ===
      int temp = (int)(string[ptrin]);
      DAC_data = (DAC_config_chan_A | (( temp + 1048) & 0xfff))  ;
              
     // === update and wrap pointers ===
      if (ptrin==string_length[current_note]) ptrin=1;
      else ptrin=ptrin+1;
     //
      if (ptrout==string_length[current_note]) ptrout=1; 
      else ptrout=ptrout+1;  

      // Write data to DAC
      // spi_write16_blocking(SPI_PORT, &DAC_data, 1) ;
      // NOTE === nonblocking SPI write ===tempo 4
      spi0_hw->dr = DAC_data ;
      
      // keep track of samples for input drive
      sample_number++;

    //} // current amp > 0    
} // end ISR  


// ========================================
// === 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_serial) ;
   //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) ;


  // set up the sring initial conditions arrays
  for (int i=0; i<max_string_size; i++){
    // random pluck
    init_string[0][i] = (float)((rand() % 2048) - 1024);  
    // slghgtly lowpassed random drive to soften pluck transient
    if(i>3){
      init_string[1][i] = (init_string[0][i-3] + init_string[0][i-2] + init_string[0][i-1] + init_string[0][i])/4; 
    }
   // Gaussian pluck
    init_string[2][i] = (800 * exp(-(float)(i - pluck_pos)*(i - pluck_pos)/(pluck_width * pluck_width)));  
    // sawtooth
    init_string[3][i] = (i<11)?(float)(i * 100) : (i<22)? (float)(1000 - i * 100) : 0 ; //int_to_s15x16(1000-(i-10)*2);
	}

  // bowing tables for DDS
  for (int i=0; i<256; i++){
    bow_table[0][i] = (1000 * sin(bow_freq*6.28*(float)i/256.)) ;
    //bow_table[1][i] = float_to_s15x16(1000 * sin(4.2*3.14149*(float)i/256.)) ;
   // bow_table[1][i] = (1000 * (i<20)? 1.0 : 0) ;
    bow_table[1][i] =  (float) ((rand() % 2048) - 1024) ;  
    //bow_table[3][i] = int_to_s15x16((rand() % 2048) - 1024) ;  
  }
     
  // start core 1 threads
  multicore_reset_core1();
  multicore_launch_core1(&core1_main);

  // === config threads ========================
  // for core 0
  //pt_add_thread(protothread_toggle25);
  //
  // === initalize the scheduler ===============
  pt_schedule_start ;
  // NEVER exits
  // ===========================================
} // end main