/**
 * Hunter Adams (vha3@cornell.edu)
 * 
 * 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
 *
 * 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)
 *
 * Protothreads v1.3
 * Threads:
 * core 0:
 * Graphics demo
 * blink LED25 
 * core 1:
 * Toggle gpio 4 
 * Serial i/o 
 * 
 * 50% overlap pseudocode
 * https://dsp.stackexchange.com/questions/48768/real-time-overlapping-buffer-for-fft
  repeat forever
    b = readSamples(1024)
    c = concat(a, b)
    fft(c)
    a = b
end repeat

Actual time to do 512  FFT+log+mag+plot is 2.8 mSec.
Frame time for 50% overlap is 20 mSec at 12.8 kHz sample rate.

So 512 FFT with 50% overlap would run real-time at 40 kHz!

// === compute MEL spectrum
        // (FFT bin 40 is 1000 Hz)
        //        stride in FFT  num bins
        //        3 below 1000   ~13 
        //        4 1000-1500     5
        //        5 1500-2000     4
        //        6 2000-2500     3   
        //        7 2500-3000     3
        //        8 3000-3500     2
        //        9 3500-4000     2
        // totoal 32 bins
 */
// ==========================================
// === VGA graphics library
// ==========================================
#include "vga16_graphics.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"
//
// ==========================================
// === 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"

// data semaphore to control fft on core 1
struct pt_sem run_fft_s, finish_fft_s, data_ready_s ;

// protection for printing
//spin_lock_t * lock_stdout ;

// unprotected global to halt display
static int run_stop = 1 ;

/* ==========================================
// Some globals for storing timer information
// ==========================================
volatile unsigned int time_accum = 0;
unsigned int time_accum_old = 0 ;
char timetext[40];

// Timer interrupt
bool repeating_timer_callback(struct repeating_timer *t) {

    time_accum += 1 ;
    return 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_IRQ_1
// ISR interval will be 78 uSec
// 12.8 KHz
volatile int alarm_period = 78 ;
float Fs = 12800 ;
//
// 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 ;
    //
    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;   
}


// ===========================================
// ADC setup 
// ===========================================
// setup ADC
#define adc_array_length 512
#define adc_half_length 256
// raw from ADC
short adc_data[adc_array_length] ;
int adc_data_count = adc_half_length ;
// formatted for scope trace
short display_data[adc_array_length] ;
short fft_display_data[adc_array_length] ;
short log_display_data[adc_array_length] ;
// copy of  data
short analysis_data[adc_array_length] ;

void ADC_setup(void){
  adc_init();
  adc_gpio_init(28);
  // p26 is ADC input 0 - gpio 28 is 2
  adc_select_input(2);
  //  48 MHz clock / 12.8 KHz sample rate
  //adc_set_clkdiv (3750);
  // 10 KHz  32 mSec window
  //adc_set_clkdiv (4800);
  // free run
  adc_run(1);
  // result is in adc_hw->result
}

// ==========================================
// === fixed point s1x14
// ==========================================
// s1.14 format
// == resolution 2^-14 = 6.1035e-5
// == dynamic range is +1.9999/-2.0
typedef signed short s1x14 ;
#define muls1x14(a,b) ((s1x14)((((int)(a))*((int)(b)))>>14)) 
#define float_to_s1x14(a) ((s1x14)((a)*16384.0)) // 2^14
#define s1x14_to_float(a) ((float)(a)/16384.0)
#define abss1x14(a) abs(a) 
#define divs1x14(a,b) ((s1x14)((((signed int)(a)<<14)/(b)))) 
// shift 12 bits into 13 bits so full scale ADC is about 0.25
#define adc_to_s1x14(a) ((s1x14)((a)<<1))


// ==========================================
// === fixed point s15x16
// ==========================================
// s15x16 fixed point macros ==
// == resolution 2^-16 = 1.5e-5
// == dynamic range is 32767/-32768
/*
typedef signed int s15x16;
#define muls15x16(a,b) ((s15x16)(((( signed long long )(a))*(( signed long long )(b)))>>16)) //multiply two fixed 16:16
#define float_to_s15x16(a) ((s15x16)((a)*65536.0)) // 2^16
#define s15x16_to_float(a) ((float)(a)/65536.0)
#define s15x16_to_int(a)    ((int)((a)>>16))
#define int_to_s15x16(a)    ((s15x16)((a)<<16))
#define divs15x16(a,b) ((s15x16)((((signed long long)(a)<<16)/(b)))) 
#define abss15x16(a) abs(a)
// the weird shift is to move the sign bits correctly
#define s1x14_to_s15x16(a) ((s15x16)(a)<<2) ;
*/

// ===========================================
// dSP definitions 
// ===========================================
//

// === FFT setup
#define N_WAVE          adc_array_length    /* size of FFT 512 */
#define LOG2_N_WAVE     9     /* log2(N_WAVE) 0 */

s1x14 Sinewave[N_WAVE]; // a table of sines for the FFT
s1x14 window[N_WAVE]; // a table of window values for the FFT
s1x14 fr[N_WAVE], fi[N_WAVE]; // input data

// === filter setup
// fixed, signed curent sample
s1x14 adc_sample ;

struct filter{
    // basics
    float F0, Q ;
    // computational parameters
    s1x14 b0, a1, a2 ;
    s1x14 x_1, x_2 ; 
    s1x14 y_1, y_2 ;
    s1x14 x_1a, x_2a ; 
    s1x14 y_1a, y_2a ;
    // lowpass shift f_out = y +()
    int alpha ;
    // lowpassed abs value
    s1x14 f_out ;
    // last lp, abs value
    s1x14 f_out_1;
};

#define n_filter 32
struct filter f[n_filter] ;

// ==================================
// === Init FFT arrays
//====================================
void FFTinit(void){
// one cycle sine table
  //  required for FFT
  for (int ii = 0; ii < N_WAVE; ii++) {
    // one cycle per window for FFT -- scall amp for number of bits
    Sinewave[ii] = float_to_s1x14(0.5 * sin(6.283 * ((float) ii) / N_WAVE));
    // Raised cos window
    window[ii] = float_to_s1x14(1.0 - cos(6.283 * ((float) ii) / (N_WAVE - 1)));
  }
}

// ==================================
// === Init filter array
//====================================
float F0[n_filter] = { 100, 175, 250, 300, 350, 400, 450, 500, 550, 600, 650, 700, 750, 800, 850, 900, 1000, // 17
                       1100, 1200, 1300, 1400, 1500, 1700 ,1850, 2000, // 8
                       2200, 2500, 2700, 3000, 3200, 3350, 3500 }; //7
void FilterInit(void){
    float pi = 3.14159 ;
    float w0, alpha;
    float Q = 10 ; //20
    for(int i=0; i<n_filter; i++){
        f[i].F0 = F0[i];
        //f[i].F0 = (float)(i+2) * 20;
        w0 = 2*pi*F0[i]/Fs ;
        if(i<3) Q =10 ;
        else Q = 10 ;
        alpha = sin(w0)/(2*Q) ;
        f[i].b0 = float_to_s1x14( (alpha)/(1 + alpha));
        f[i].a1 = float_to_s1x14( (-2*cos(w0))/(1 + alpha));
        f[i].a2 = float_to_s1x14( (1 - alpha)/(1 + alpha));
        if(i>2) f[i].alpha = 6 ; // 5 mSec
        else f[i].alpha = 7 ;// 10 mSec
        f[i].Q = Q ;
    }

}

// ==================================
// === FFT
//====================================
void FFTfix(s1x14 fr[], s1x14 fi[], int m){
//Adapted from code by:
//Tom Roberts 11/8/89 and Malcolm Slaney 12/15/94 malcolm@interval.com
//fr[n],fi[n] are real,imaginary arrays, INPUT AND RESULT.
//size of data = 2**m
// This routine does foward transform only

  int mr,nn,i,j,L,k,istep, n;
  s1x14 qr,qi,tr,ti,wr,wi;

  mr = 0;
  n = 1<<m;   //number of points
  nn = n - 1;

  /* decimation in time - re-order data */
  for(m=1; m<=nn; ++m){
      L = n;
      do L >>= 1; while(mr+L > nn);
      mr = (mr & (L-1)) + L;
      if(mr <= m) continue;
      tr = fr[m];
      fr[m] = fr[mr];
      fr[mr] = tr;
      ti = fi[m];   //for real inputs, don't need this
      fi[m] = fi[mr]; //for real inputs, don't need this
      fi[mr] = ti; //for real inputs, don't need this
  }

  L = 1;
  k = LOG2_N_WAVE-1;
  while(L < n) {
      istep = L << 1;
      for(m=0; m<L; ++m){
          j = m << k;
          wr =  Sinewave[j+N_WAVE/4];
          wi = -Sinewave[j];
          //wr >>= 1; //do need if scale table
          //wi >>= 1;

          for(i=m; i<n; i+=istep){
              j = i + L;
              tr = muls1x14(wr, fr[j]) - muls1x14(wi, fi[j]);
              ti = muls1x14(wr, fi[j]) + muls1x14(wi, fr[j]);
              qr = fr[i] >> 1;
              qi = fi[i] >> 1;
              fr[j] = qr - tr;
              fi[j] = qi - ti;
              fr[i] = qr + tr;
              fi[i] = qi + ti;
          }
      }
      --k;
      L = istep;
  }
}

//====================================
// === 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))
//====================================
void magnitude(s1x14 fr[], s1x14 fi[], int length){
  s1x14 mmax, mmin ;
  s1x14 c1 = float_to_s1x14(0.89820) ;
  s1x14 c2 = float_to_s1x14(0.48597) ;
  for (int ii = 0; ii < length; ii++) {
        mmin = min(abs(fr[ii]), abs(fi[ii])); //>>9
        mmax = max(abs(fr[ii]), abs(fi[ii]));
        // reuse fr to hold magnitude
        fr[ii] = max(mmax, (muls1x14(mmax,c1) + muls1x14(mmin,c2) )); 
        fi[ii] = 0;
      }
}
//====================================
// log2 approx
// see:
    // Generation of Products and Quotients Using Approximate Binary Logarithms 
    // for Digital Filtering Applications, 
    // IEEE Transactions on Computers 1970 vol.19 Issue No.02
//====================================
/*
void log2_approx0(s1x14 fr[], int length){
  s15x16 log_input, log_output ;
  // reduced range variable for interpolation
  s15x16 x;
  // low cutoff
  s15x16 low_cutoff = float_to_s15x16(0.00006) ;
  s15x16 c1 = float_to_s15x16(0.984) ;
  s15x16 c2 = float_to_s15x16(0.065) ;


  for (int ii = 0; ii < length; ii++) {
    log_input = s1x14_to_s15x16(fr[ii]) ;    
    //
    // check for too small or negative
    // and return smallest log2
    if(log_input <= low_cutoff){
      fr[ii] = -15 ;
      continue ;
    }
    // if the input is less than 2 the scale up by
    // 2^14 so always working on an integer
    // so we can get logs down to input of 0.00003 or so approx -14.85
    int frac_factor = 0 ;
    if (log_input < int_to_s15x16(2) ){
      // max size of shift to not overflow
      frac_factor = 14 ;
      log_input <<= frac_factor ;
    }

    // temp for finding msb
    s15x16 sx ;  
    sx = log_input ;

    // find the most-significant bit
    // equivalent to finding the characteristic of the log
    s15x16 y=1; // value of MSB
    s15x16 ly=0; // position of MSB
    while(sx>int_to_s15x16(2)) {
        y=y<<1 ; ly=ly+int_to_s15x16(1) ; sx=sx>>1;
    }
    // bound the bottom and detect negative input values
    // Two-segment approx is good to better than  0.02 log unit
    // equiv to finding the mantissa of the log, then adding the charastic
    // see:
    // Generation of Products and Quotients Using Approximate Binary Logarithms 
    // for Digital Filtering Applications, 
    // IEEE Transactions on Computers 1970 vol.19 Issue No.02
    // normalize the bits after dleting MSB
    x = (log_input-y)>>(int)ly ;
    // piecewise linear curve fit
    //if(x<0.5) log_output = (ly + x*1.163 + 0.0213) - frac_factor ;
    //else      log_output = (ly + x*0.828 + 0.1815) - frac_factor ;
    // one segment approx goodd to about 0.07 log unit
    log_output = (ly + x*c1 + c2) - frac_factor ;
    // and store it
    fr[ii] = log_output>>16 ;
  // 
  }
}
*/

#define log_min 0x00  
// reuse fr to hold log magnitude
// shifting finds most significant bit
// then make approxlog  = ly + (fr-y)./(y) + 0.043;
// BUT for an 8-bit approx (4 bit ly and 4-bit fraction)
// ly 1<=ly<=14
// omit the 0.043 because it is too small for 4-bit fraction
void log2_approx(s1x14 fr[], int length) {
  //
  int sx, y, ly, temp ;
  for (int i = 0; i < length; i++) {
    // interpret bits as integer
    sx = fr[i];
    y=1; ly=0;
    while(sx>1) {
        y=y*2; ly=ly+1; sx=sx>>1;
    }
    // shift ly into upper 4-bits as integer part of log
    // take bits below y and shift into lower 4-bits
    // !!NOTE that fr is no longer in s1x14 format!!
    fr[i] = ((ly)<<4) + ((fr[i]-y)>>(ly-4) ) ;
    // bound the noise at low amp
    if(fr[i]<log_min) fr[i] = log_min;
  }
}

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

    // DMA channel -- needed to check for DMA done
    //static int ADC_data_chan ;

    // the protothreads interval timer
    PT_INTERVAL_INIT() ;

    // Draw some filled rectangles
    fillRect(20, 1, 400, 50, LIGHT_BLUE); // blue box
    setCursor(100, 20) ;
    setTextColor(WHITE) ;
    writeStringBig("FFT spectrogram and MEL filterbank") ;

    fillRect(435, 1, 200, 50, YELLOW); // green box
    // Write some text
    setTextColor(BLACK) ;
    short text_x = 445 ;
    setCursor(text_x-1, 2) ;
    setTextSize(1) ;
    writeStringBold("Raspberry Pi Pico: ece 4760") ;
    setCursor(text_x, 12) ;
    writeString("Bruce Land: BRL4@cornell.edu") ;
    setCursor(text_x, 22) ;
    writeString("Hunter Adams:VHA3@cornell.edu") ;
    setCursor(text_x, 32) ;
    writeString("Protothreads rp2040 v1.3") ;
    setCursor(text_x, 42) ;
    writeString("16 color VGA") ;

    setCursor(80, 122) ;
    setTextSize(1) ;
    setTextColor2(BLACK, YELLOW);
    //writeString(" Spectrum 0-6.4 KHz ") ;
    setCursor(350, 122) ;
    //writeString(" Log Spectrum 0-6.4 KHz ") ;

    drawHLine(0,479,10, WHITE);
    setCursor(1, 469) ;
    setTextSize(1) ;
    setTextColor2(WHITE, BLACK);
    writeString("0");
    //
    drawHLine(0,479-40,10, WHITE);
    setCursor(1, 469-40) ;
    writeString("1k");
    //
    drawHLine(0,479-80,10, WHITE);
    setCursor(1, 469-80) ;
    writeString("2k");
    //
    drawHLine(0,479-120,10, WHITE);
    setCursor(1, 469-120) ;
    writeString("3k");
    //
    drawHLine(0,479-160,10, WHITE);
    setCursor(1, 469-160) ;
    writeString("4k");

    drawHLine(0,479-175,640, GREEN);
    setCursor(200, 469-160) ;
    setTextColor2(BLACK, YELLOW);
    writeString("FFT Spectogram -- n_bands = 160");
    setCursor(200, 469-180) ;
    setTextColor2(WHITE, LIGHT_BLUE);
    writeString("Filter Bank Spectrogram -- n_bands = 32");


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

    //for(int i=0; i<adc_array_length-1; i++){
        // init  initial dispplay points
    //    display_data[i] = 120 ;
    //}

    while(true) {
        
        // A brief nap
        PT_YIELD_usec(1000000) ;
   }
   PT_END(pt);
} // graphics thread

// ==================================================
// === toggle25 thread on core 0
// ==================================================
// the on-board LED blinks
static PT_THREAD (protothread_toggle25(struct pt *pt))
{
    PT_BEGIN(pt);
    static bool LED_state = false ;
    
     // set up LED p25 to blink
     gpio_init(25) ;	
     gpio_set_dir(25, GPIO_OUT) ;
     gpio_put(25, true);
     // data structure for interval timer
     PT_INTERVAL_INIT() ;

      while(1) {
        // yield time 0.1 second
        //PT_YIELD_usec(100000) ;
        PT_YIELD_INTERVAL(500000) ;

        // toggle the LED on PICO
        LED_state = LED_state? false : true ;
        gpio_put(25, LED_state);
        //
        // NEVER exit while
      } // END WHILE(1)
  PT_END(pt);
} // blink thread


// ==================================================
// === fft thread -- RUNNING on core 1
// ==================================================
//  
static PT_THREAD (protothread_fft(struct pt *pt))
{
    PT_BEGIN(pt);
     static short time_column = 12 ;
     static short fr_disp ;
     static short color_map[7] ={0, 1, 2, 3, 11, 15, 15};
     static uint64_t start_time ;
    
     PT_INTERVAL_INIT() ;

     FFTinit();
     FilterInit();

      while(1) {
        //
        // yield until ADC buffer full
        PT_YIELD_UNTIL(pt, run_stop);
        PT_SEM_SAFE_WAIT(pt, &data_ready_s) ;

        // get ADC buffer (restart is done in iSR)
        memcpy(analysis_data, adc_data, adc_array_length*2) ;
        // move second half to first half to cause 50% overlap
        memcpy(&adc_data[0], &adc_data[adc_half_length], adc_half_length*2) ;

        start_time = PT_GET_TIME_usec() ;
        // compute FFT on ADC buffer and draw one column
        // covert ADC to fixed point
        // with pre-emphisis out(t) = in(t) - .94*in(t-1)
        fr[0] =  adc_to_s1x14(analysis_data[0]>>2);
        for (int i = 1; i < N_WAVE; i++) {
          fr[i] =  adc_to_s1x14((analysis_data[i]-analysis_data[i-1]+(analysis_data[i-1]>>4))<<3) ;  //adc_to_s1x14
        }
        
        // === do the windowing
        for (int i = 0; i < N_WAVE; i++) {
          fr[i] = muls1x14(fr[i], window[i]); 
          fi[i] = 0;
        }

        // === do the FFT
        FFTfix(fr, fi, LOG2_N_WAVE);
        //
        // === compute power spectrum
        magnitude(fr, fi, N_WAVE);

        // === plot single FFT spectrum for testing
       // for(int i=2; i<adc_array_length/2; i++){
          // erase a point
        //  drawPixel(i+40, fft_display_data[i], BLACK) ;
         // fft_display_data[i] = -(short)(fr[i]>>5) + 120 ;
        //  drawPixel(i+40, fft_display_data[i], GREEN) ;
        //}

        // === plot log-spectrum for testing
        // !!After log, the returned fr is no longer in s1x14 format!!
        // do the APPROXIMATE log in u4x4 format!
        log2_approx(fr, adc_array_length/2) ;
        // plot 
        
        // === plot fft spectrogram
        // Spectrogram -- draw and move right
        // wrap the screen
        // at right edge of screen, reset to left edge
        time_column++ ;
        if (time_column == 636) time_column = 12;
        //drawHLine(0, 80,5,WHITE) ;
        for(int i=4; i<165; i++){
          // bound to 0 to 15
          fr[i] = max(fr[i], 64) - 64;
          fr_disp = color_map[min(7, max((fr[i]>>4)-1, 0))] ; //fr[i]>>3)-3
          drawPixel(time_column, 480-i, fr_disp ) ;
          //drawVLine(time_column, 480-2*i, 2, fr_disp) ;
        } 

       // s1x14 c[n_filter] ;
       // for(int i=0; i<n_filter; i++){
        //   c[i] = f[i].f_out  ;
        //}
        //log2_approx( c, n_filter) ;

        for(int i=0; i<n_filter; i++){
           // c[i] = max(c[i], 64) ; //- 64;
            int ci = f[i].f_out ;
            // you may need to tune the color thresholds
            drawPixel(time_column, 260-i*3,   (ci>5)?GREEN: (ci>2)?MED_GREEN: (ci>1)?DARK_GREEN: BLACK);//color_map[(int)max(0,min(c[i]>>4,7))]) ; //c>2?WHITE:DARK_GREEN) ; 
            drawPixel(time_column, 260-i*3-1, (ci>5)?GREEN: (ci>2)?MED_GREEN: (ci>1)?DARK_GREEN: BLACK);

            // drawPixel(time_column, 260-i*3, (ci>1)?GREEN:  BLACK);
            //drawPixel(time_column, 260-i*3-1, (ci>1)?GREEN:  BLACK);

            //drawVLine(i*4+30, 70, 100, BLACK) ;
           // drawVLine(i*4+30, 70, (int) ci, WHITE) ;
            //printf("%d\n\r", f[i].f_out) ;
        }
        //printf("%lld\n\r", PT_GET_TIME_usec()-start_time) ;
        // 100, 175, 250, 300
        //float ratio = s1x14_to_float(f[1].f_out) / max(.0001,s1x14_to_float(f[0].f_out));
        //printf("%f \n\r", ratio);
        // NEVER exit while
      } // END WHILE(1)
  PT_END(pt);
} // blink 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);
      
      //
      //for (int i = 0; i < N_WAVE; i++) {
       //   printf("%f \n\r", s1x14_to_float(window[i])) ; //m        
      //}

      while(1) {
        // print prompt
        sprintf(pt_serial_out_buffer, "input 0=stop 1=run: ");
        // spawn a thread to do the non-blocking write
        serial_write ;

        // spawn a thread to do the non-blocking serial read
         serial_read ;
        // convert input string to number
        sscanf(pt_serial_in_buffer,"%d ", &run_stop) ;


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

// ==================================================
// === dsp ISR -- 
// ==================================================
// 
void compute_sample(void){
    // read and subtrace zero point
    adc_data[adc_data_count] = adc_hw->result - 2048;
    // convert to fixed
    adc_sample = adc_to_s1x14(adc_data[adc_data_count]-adc_data[adc_data_count-1]+(adc_data[adc_data_count-1]>>3)) ;
    //adc_sample = adc_to_s1x14(adc_data[adc_data_count]) ;
    //adc_sample <<= 2 ;
    //
    for(int i=0; i<n_filter; i++){
        // filter i state 1
        s1x14 yy = muls1x14((adc_sample-f[i].x_2), f[i].b0);
        f[i].x_2 = f[i].x_1;
        f[i].x_1 = adc_sample;
        yy = yy - muls1x14(f[i].y_2,f[i].a2); 
        f[i].y_2 = f[i].y_1;
        yy = yy - muls1x14(f[i].y_1,f[i].a1); 
        f[i].y_1 = yy;
        // filter i stage 2
        /*
        s1x14 zz = muls1x14((yy-f[i].x_2a), f[i].b0);
        f[i].x_2a = f[i].x_1a;
        f[i].x_1a = yy;
        zz = zz - muls1x14(f[i].y_2a, f[i].a2); 
        f[i].y_2a = f[i].y_1a;
        zz = zz - muls1x14(f[i].y_1a, f[i].a1); 
        f[i].y_1a = zz;
        */
        // abs lowpass out = (yy-last_out)>>alpha + last_out
        f[i].f_out = ((abss1x14(yy) - f[i].f_out_1) >> f[i].alpha) + f[i].f_out_1 ;
        f[i].f_out_1 = f[i].f_out ;
    }
    // check buffer
    if (adc_data_count == adc_array_length-1){
        adc_data_count = adc_half_length ;
        if (run_stop) PT_SEM_SAFE_SIGNAL(pt, &data_ready_s) ;
    }
    else adc_data_count++;

}

// ========================================
// === core 1 main -- started in main below
// ========================================
void core1_main(){ 
  //
  //  === add threads  ====================
  // for core 1
  pt_add_thread(protothread_fft) ;
  pt_add_thread(protothread_serial) ;
  //
  // === 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 v1.1.1 two-core\n\r");

  // Initialize the VGA screen
  initVGA() ;

  // init the global fft ready signals
  PT_SEM_SAFE_INIT(&run_fft_s, 1) ;
  PT_SEM_SAFE_INIT(&finish_fft_s, 1) ;
  PT_SEM_SAFE_INIT(&data_ready_s, 0) ;
  
  // anybody can print first
  //PT_LOCK_INIT(lock_stdout, 31, 0) ;
     
  //
  // 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);

  // === 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