
 /*
 * This program implements the 
 * Sliding Windowed Infinite Fourier Transform (SWIFT)
 * and the slightly lower noise version, alpha-SWIFT
 * 
 * Logan L. Grado, Matthew D. Johnson, and Theoden I. Netoff
 *  Digital Object Identifier 10.1109/MSP.2017.2718039
 *  https://ieeexplore.ieee.org/document/8026592
 * 
 * A bank of digital, complex, resonaters each with the appropriate window
 * copmputes a complete fourier transform at each input sample time.
 * Sample rate in 10 KHz.

 * HARDWARE CONNECTIONS
 *  - GPIO 16 ---> VGA Hsync
 *  - GPIO 17 ---> VGA Vsync
 *  - GPIO 18 ---> 470 ohm resistor ---> VGA Green lo-bit |__ both wired to VGA Green 
 *  - GPIO 19 ---> 330 ohm resistor ---> VGA Green hi_bit |   
 *  - GPIO 20 ---> 330 ohm resistor ---> VGA Blue
 *  - GPIO 21 ---> 330 ohm resistor ---> VGA Red
 *  - RP2040 GND ---> VGA GND
 * 
 *  - GPIO 28 ADC input 
 * in this example, input cam from a laptop through a high-pass biased to Vdd/2
 * 
 * -- SPI for DAC
 * #define PIN_CS   5
 * define PIN_SCK  6
 * define PIN_MOSI 7
 * define SPI_PORT spi0
 *
 * RESOURCES USED
 *  - PIO state machines 0, 1, and 2 on PIO instance 0
 *  - DMA channels 0, 1, 2, and 3
 *  - 153.6 kBytes of RAM (for pixel color data)
 *
 */

// ==========================================
// === VGA graphics library
// ==========================================
#include "vga16_graphics_v3.h"
#include <stdio.h>
#include <stdlib.h>
#include <math.h>

// pico specific
#include "pico/stdlib.h"
#include "hardware/pio.h"
#include "hardware/dma.h"
#include "hardware/adc.h"
#include "hardware/spi.h"

//
//#include "hardware/vreg.h"
//#include "hardware/clocks.h"
//
// ==========================================
// === protothreads globals
// ==========================================
#include "hardware/sync.h"
#include "hardware/timer.h"
#include "pico/multicore.h"
#include "string.h"
// protothreads header
#include "pt_cornell_rp2040_v1_4.h"

// unprotected global to halt display
static int run_stop = 1 ;
// restart flag
int restart_graphics = false ;


// ==========================================
// === 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 TIMER0_IRQ_1
// ISR interval will be 100 uSec
// 10 KHz
volatile int alarm_period = 100 ;
float Fs = 10000 ;
//
// the actual ISR
void compute_sample(void);
//
static void alarm_irq(void) {
    // mark ISR entry
    gpio_put(15,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 actual computation
    compute_sample();

    // mark ISR exit
    gpio_put(15,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;   
}


// ===========================================
// dSP definitions 
// ===========================================
// The Sliding Windowed Infinite Fourier Transform -- with alpha-SWIFT
// Logan L. Grado, Matthew D. Johnson, and Theoden I. Netoff
// Digital Object Identifier 10.1109/MSP.2017.2718039
//file:///home/bruce/Downloads/10078055.pdf

// === SWIFT setup
// can be any number, limited by the speed of the cpu
#define N_FREQ  200   

// A list of the frequencys to transform
// need not be uniform
float F[N_FREQ] ;

// the complex spectral estimate
// frequencies are normailsed and run 0 to pi
// at 10khz sample rate that is 0 to 5kz
float Xr[N_FREQ], Xi[N_FREQ] ;
float freq_spacing = 20.0f ; //20.0f

// additive components of the alpha-SWIFT
float slow_Xr[N_FREQ], slow_Xi[N_FREQ] ;
float fast_Xr[N_FREQ], fast_Xi[N_FREQ] ;

// and the mag of X
float Fmag[N_FREQ], lp_Fmag[N_FREQ];
// and the gain for the plot
float  plot_gain = 0.005f ;

// the per-sample complex phase shift at each frequecy
// omega = 2*pi*F/Fs so OHM = cos(omega) + j*sin(omega)
float slow_OHMr[N_FREQ], slow_OHMi[N_FREQ] ;
float fast_OHMr[N_FREQ], fast_OHMi[N_FREQ] ;

// the decay rate derived from number of samples 
// value will be exp(-1/(number of samples))
float dk_rate[N_FREQ] ;
float fast_dk = 10 ;
float fast_dk_rate ;
//. false==fixed; true==relative
int dk_mode = true  ; 
float dk_relative = 10.0f, dk_fixed = 100.0f ;

//  dds to calibrate independent of input source
unsigned int accum1, accum2 ;
unsigned int inc1, inc2 ;
float dds_freq1 = 1000.0f, dds_freq2 =2000.0f ;
float sine_table[256] ;
float input_sample ;

// at each sample time:
// and for each different frequency omega (or F)
// Xn(omega) = exp(-1/(number of samples)) * exp(j*omega) * X(n-1)(omega) + new sample



//====================================
// === magnitude approx good to about +/-2%
// see https://en.wikipedia.org/wiki/Alpha_max_plus_beta_min_algorithm
#define min(a,b) (((a)<(b))?(a):(b))
#define max(a,b) (((a)>(b))?(a):(b))
//====================================
float magnitude(float real, float img){
  float mmax, mmin ;
  float c1 = 0.89820f ;
  float c2 = 0.48597f ;
  
  mmin = min(fabsf(real), fabsf(img)) ; 
  mmax = max(fabsf(real), fabsf(img)) ;
  // 
  return max(mmax, ((mmax*c1) + (mmin*c2) )); 
}


// ==================================================
// === graphics demo -- RUNNING on core 0
// ==================================================
static PT_THREAD (protothread_graphics(struct pt *pt)) {
    PT_BEGIN(pt);

    // Draw some filled rectangles
    fillRect(20, 1, 400, 50, LIGHT_BLUE); // blue box
    drawTextVGA437(100, 20, "FFT spectrogram and SWIFT", WHITE, LIGHT_BLUE) ;

    fillRect(435, 1, 200, 50, YELLOW); // green box
    // Write some text
     // Draw some filled rectangles
    fillRect(0, 0, 640, 480, BLACK); // erase
    fillRect(64, 0, 176, 50, BLUE); // blue box
    fillRect(250, 0, 176, 50, YELLOW); // red box
    fillRect(435, 0, 176, 50, GREEN); // green box

    // Write some text
    drawTextTiny8(65, 0, "Raspberry Pi Pico 2", WHITE, BLUE) ;
    drawTextGLCD(65, 10, "SWIFT spectrum", WHITE, BLUE) ;
    drawTextGLCD(65, 20, "Bruce Land brl4@cornell.edu", WHITE, BLUE) ;
    drawTextGLCD(65, 30, "Hunter Adams vha3@cornell.edu", WHITE, BLUE) ;
    //
    drawTextGLCD(438, 10, "Protothreads rp2040/2350 1.4", BLACK, GREEN) ;
   // drawTextGLCD(438, 20, "Two CPU; clock 300 Mhz", BLACK, GREEN) ;
    drawTextGLCD(438, 30, "vga16_v3 driver", BLACK, GREEN) ;
    drawTextGLCD(438, 40, "Double Buffer off", BLACK, GREEN) ;
    //
    drawTextVGA437(260, 5, "SWIFT Transform", BLACK, YELLOW) ;
    //
   
    setCursor(80, 122) ;
    setTextSize(1) ;
    setTextColor2(BLACK, YELLOW);
    //writeString(" Spectrum 0-6.4 KHz ") ;
    setCursor(350, 122) ;
    //writeString(" Log Spectrum 0-6.4 KHz ") ;

    copy_buffer_to_other() ;

    // === congure the ADC =========
    // ===  and start a buffer fill
    //ADC_setup() ;

    // set up F, decay and phase shift arrays
    
    for(int i=0; i<N_FREQ; i++){
      F[i] = freq_spacing * (float)i ;

      //decay rate will be exp(-1/(number of samples))
      //dk_rate[i] = exp(-1.0f/dk);
      // dk_rate sets the bandwidth of a bin bigger is narrrower bandwidth
      // different uses willl require setting this
      // for speech 3.0*Fs/F seems good
      if (dk_mode==false)
        {dk_rate[i] = exp(-1.0f/(dk_relative*Fs/F[i])); }
      else
        {dk_rate[i] = exp(-1.0f/(dk_fixed));}
      //

      fast_dk_rate = exp(-1.0f/fast_dk);
      //

      // omega = 2*pi*F/Fs so OHM = cos(omega) + j*sin(omega)
      float omega = 6.28318f * F[i]/Fs ;
      slow_OHMr[i] = dk_rate[i] * cos(omega) ;
      slow_OHMi[i] = dk_rate[i] * sin(omega); 
      fast_OHMr[i] = fast_dk_rate * cos(omega) ;
      fast_OHMi[i] = fast_dk_rate * sin(omega);      
    }

    // dds
    inc1 = dds_freq1 * pow(2,32)/ Fs ;
    inc2 = dds_freq2 * pow(2,32)/ Fs ;

    while(true) {
        
        PT_YIELD_UNTIL(pt, draw_start_signal()); 
        // restaert logic 
      if (restart_graphics){
            // reset the flag
            restart_graphics = false ;       
            // restarts the current thread at PT_BEGIN(pt);
            // as if it has not executed before
            PT_RESTART(pt) ;
        }
        // cleart spectyrum
        clearRegion(60, 310, BLACK) ;

        // clear spectrum
        // comput mag
        // plot spectrum
        // === plot single SWIFT spectrum for testing
        int max_bin = 0 ;
        float max_value = 0 ;
       for(int i=2; i<N_FREQ; i++){
         // the scale factors will change depending on recording conditions
         //Fmag[i] = magnitude(Xr[i], Xi[i]) * plot_gain ;
         Fmag[i] = lp_Fmag[i] * plot_gain ;
         fillRect(i*2+40, -(short)Fmag[i]+300, 2, 2, GREEN) ;
        
         // find max bin
         if(Fmag[i] > max_value){
            max_value = Fmag[i] ;
            max_bin = i ;
         }

      }
      char str[80] ;
      sprintf(str, "max bin freq = %5.1f   ", F[max_bin]);
      drawTextTiny8(100, 60, str, WHITE, BLACK);
      //
        
   }
   PT_END(pt);
} // graphics thread


// ==================================================
// === user's serial input thread on core 1
// ==================================================
// serial_read an serial_write do not block any thread
// except this one
static PT_THREAD (protothread_serial(struct pt *pt))
{
    PT_BEGIN(pt);
      
      static char *cmd, *arg1, *arg2, *arg3, *arg4;

      while(1) {
        // prompt the user for input
        sprintf(pt_serial_out_buffer, "cmd> ") ;
        // spawn a thread to do the non-blocking serial write
        // to print the pt_serial_out_buffer string
        // this output routine YIELDs between each character printed.
        serial_write ; 
        // wait for user
        serial_read ;
        // tokenize the input string
        //  --- NOTE that strtok is not re-entrant!
        //  ---   Use the reentrant, thread-safe, version if you need to use it
        //   ---  in moe than one thread!
        cmd = strtok(pt_serial_in_buffer, "  ");
        arg1 = strtok(NULL, "  ");
        arg2 = strtok(NULL, "  ");
        arg3 = strtok(NULL, "  ");
        arg4 = strtok(NULL, "  ");

        //wait
        sleep_ms(10);

        // restart graphics thread
        if(strcmp(cmd,"restart")==0){
          restart_graphics = true ;  
        }
        //
        // plot gain
        else if(strcmp(cmd,"gain")==0){
          sscanf(arg1,"%f", &plot_gain);  
        }
        //
        // dds freq
       else if(strcmp(cmd,"dds")==0){
          sscanf(arg1,"%f", &dds_freq1); 
          sscanf(arg2,"%f", &dds_freq2);  
          restart_graphics = true ;  
        }
        //
        // dk r/f ratio/value r==relative f==fixed
        else if(strcmp(cmd,"dk")==0){
          sscanf(arg1,"%c", &dk_mode); 
          if (dk_mode == 'r') {
            dk_mode = false ;
            sscanf(arg2,"%f", &dk_relative); 
          }
          else {
            dk_mode = true ;
            sscanf(arg2,"%f", &dk_fixed); 
          }
          restart_graphics = true ;  
        }
        
        //
         else{
          printf("Huh?\n\r") ;
        }
        //



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

// ==================================================
// === dsp ISR -- 
// ==================================================
// 
void compute_sample(void){
    // DDS
    // make the sine wave to send to filter
    accum1 += inc1 ;
    // update main waveform
    sine_table[accum1>>24] ;
    input_sample = sine_table[accum1>>24] ; ;

    accum2 += inc2 ;
    //add 
    sine_table[accum2>>24] ;
    input_sample = 0.5f * (input_sample + sine_table[accum2>>24])  ;


    // don't output very low frequencies
    // bin  3 is 3x20Hz
    for(int i=3; i<N_FREQ; i++){   //N_FREQ
      float temp ;
      // the slow arm of the swift
      temp = (slow_Xr[i] * slow_OHMr[i] - slow_Xi[i] * slow_OHMi[i]) + input_sample ; //-
      slow_Xi[i] = (slow_Xr[i] * slow_OHMi[i] + slow_Xi[i] * slow_OHMr[i]) ;//-
      slow_Xr[i] = temp ;
      //
      // the fast arm of the swift
      temp = (fast_Xr[i] * fast_OHMr[i] - fast_Xi[i] * fast_OHMi[i]) + input_sample ; //-
      fast_Xi[i] = (fast_Xr[i] * fast_OHMi[i] + fast_Xi[i] * fast_OHMr[i]) ;//-
      fast_Xr[i] = temp ;
      //
      // combine the two to make X
      Xr[i] = slow_Xr[i] - fast_Xr[i] ;
      Xi[i] = slow_Xi[i] - fast_Xi[i] ;
      //
      lp_Fmag[i] = 0.99f * lp_Fmag[i] + 0.01f * magnitude(Xr[i], Xi[i]);
    }
    //   
    
}

// ========================================
// === core 1 main -- started in main below
// ========================================
void core1_main(){ 
  //
  //  === add threads  ====================
  // for core 1
  pt_add_thread(protothread_serial) ;
  pt_add_thread(protothread_graphics);
  //
  // === initalize the scheduler ==========
  pt_schedule_start ;
  // NEVER exits
  // ======================================
}

// ========================================
// === core 0 main
// ========================================
int main(){
  // set the clock
  //set_sys_clock_khz(250000, true); // 171us
  // start the serial i/o
  stdio_init_all() ;
  // announce the threader version on system reset
  printf("\n\rProtothreads RP2040/2350 v1.4 two-core\n\r");

  // Initialize the VGA screen
  initVGA() ;

  //
  // fire off interrupt
  alarm_in_us(alarm_period);

  gpio_init(15) ;	
     gpio_set_dir(15, GPIO_OUT) ;

  // start core 1 threads
  multicore_reset_core1();
  multicore_launch_core1(&core1_main);

  // dds table for testing
  for (int i=0; i<256; i++) {
    // sine table is in natural +1/-1 range
    sine_table[i] = 1000.0f*(sin(2*3.14159*(float)i/256)) ;
  }

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