/*
fht_codec.pde
guest openmusiclabs.com 9.5.12
example sketch for running an fht on data collected
with the codecshield.  this will send out 128 bins of
data over the serial port at 115.2kb.  there is a
pure data patch for visualizing the data.

note: be sure to download the latest AudioCodec library
if yours is older than 8.16.12.  there were modifications
made that allow for code outside of the interrupt.
*/

#define SAMPLE_RATE 44 // 44.1Khz
#define ADCS 0 // no ADCs are being used
//#define LOG_OUT 1 // use the log output function
#define FHT_N 256 // set to 256 point fht

// include necessary libraries
#include "FHT.h"
#include <Wire.h>
#include <SPI.h>
#include <AudioCodec.h>

// create data variables for audio transfer
int left_in = 0x0000;
int left_out = 0x0000;
int right_in = 0x0000;
int right_out = 0x0000;
unsigned int count = 0;
volatile byte flag = 1;
int fht_input2[FHT_N];
int fht_output[FHT_N];
char fht_phase[FHT_N/2];
char fht_phase2[FHT_N/2];

PROGMEM  prog_uint8_t arctan[]  = {
  #include <arctan.csv>
};

void setup() {
  Serial.begin(115200); // use serial port
  AudioCodec_init();
}

void loop() {
  while(1) { // reduces clock jitter
    while(flag); // wait for samples to be collected
    fht_window(); // window the data
    fht_reorder(); // reorder for fht input
    fht_run(); // process fht
//    fht_mag_log(); // take output of fht
//    Serial.print('S'); // send out a start byte
//    Serial.print('o');

//    int i = 0;
//    while (i < FHT_N) {
//      Serial.println(fht_input[i]); // send out data bytes
//      i++;
//    }
    for (int i = 1; i < FHT_N/2; i++) {
      int j = fht_input[i];
      int k = fht_input[FHT_N - i];
      int l = j - k; // imaginary
      k = j + k; // real
      if (abs(l) <= 0x3ff) l << 5; // scale values by 32
      else k >> 5;
      if (k == 0) { // check edge cases
        if (l == 0) fht_phase[i] = 0x80; // indeterminate marker
        else if (l > 0) fht_phase[i] = 63; // pi/2 scaled
        else fht_phase[i] = -63; // -pi/2 scaled
      }
      else if (k > 0) {
        if (l >= 0) {
          j = l/k;
          if (j > 1023) fht_phase[i] = 63; // pi/2 scaled
          else fht_phase[i] = pgm_read_byte_near(arctan + j);
        }
        else { // invert all values for negatives
          j = -l/k;
          if (j > 1023) fht_phase[i] = -63; // pi/2 scaled
          else fht_phase[i] = -pgm_read_byte_near(arctan + j);
        }
      }
      else { // k is negative
        if (l >= 0) {
          j = -l/k;
          if (j > 1023) fht_phase[i] = 63; // pi/2 scaled
          else fht_phase[i] = 126 - pgm_read_byte_near(arctan + j);
        }
        else { // invert all values for negatives
          j = l/k;
          if (j > 1023) fht_phase[i] = -63; // pi/2 scaled
          else fht_phase[i] = pgm_read_byte_near(arctan + j) - 126;
        }
      }
    }
    for (int i = 0; i < FHT_N; i++) {
      fht_input[i] = fht_input2[i];
    }
    fht_window(); // window the data
    fht_reorder(); // reorder for fht input
    fht_run(); // process fht
//    fht_mag_log(); // take output of fht
//    Serial.print('t'); // send second packet indicator
//    i = 0;
//    while (i < FHT_N) {
//      Serial.println(fht_input[i]); // send out data bytes
//      i++;
//    }
    for (int i = 1; i < FHT_N/2; i++) {
      int j = fht_input[i];
      int k = fht_input[FHT_N - i];
      int l = j - k; // imaginary
      k = j + k; // real
      if (abs(l) <= 0x3ff) l << 5; // scale values by 32
      else k >> 5;
      if (k == 0) { // check edge cases
        if (l == 0) fht_phase2[i] = 0x80; // indeterminate marker
        else if (l > 0) fht_phase2[i] = 63; // pi/2 scaled
        else fht_phase2[i] = -63; // -pi/2 scaled
      }
      else if (k > 0) {
        if (l >= 0) {
          j = l/k;
          if (j > 1023) fht_phase2[i] = 63; // pi/2 scaled
          else fht_phase2[i] = pgm_read_byte_near(arctan + j);
        }
        else { // invert all values for negatives
          j = -l/k;
          if (j > 1023) fht_phase2[i] = -63; // pi/2 scaled
          else fht_phase2[i] = -pgm_read_byte_near(arctan + j);
        }
      }
      else { // k is negative
        if (l >= 0) {
          j = -l/k;
          if (j > 1023) fht_phase2[i] = 63; // pi/2 scaled
          else fht_phase2[i] = 126 - pgm_read_byte_near(arctan + j);
        }
        else { // invert all values for negatives
          j = l/k;
          if (j > 1023) fht_phase2[i] = -63; // pi/2 scaled
          else fht_phase2[i] = pgm_read_byte_near(arctan + j) - 126;
        }
      }
    }
    for (int i = 1; i < FHT_N/2; i++) {
      char m = fht_phase2[i];
      char n = fht_phase[i];
      if (m == 0x80); // dont add null cases
      else if (n == 0x80) fht_phase2[i] = n;
      else {
        int p = m - n;
        if (p > 126) p -= 252; // deal with wrap around
        else if (p < -126) p += 252;
        fht_phase2[i] = p;
      }
    }
    // at this point fht_phase2 is full of the phase differences
    flag = 1; // tell the codec that processing is done
  }
}

// timer1 interrupt routine - data collected here
ISR(TIMER1_COMPA_vect) { // store registers (NAKED removed)

  // &'s are necessary on data_in variables
  AudioCodec_data(&left_in, &right_in, left_out, right_out);
  left_out = left_in; // pass audio through
  right_out = right_in;
  if (flag) { // check if the fht is ready for more data
    fht_input[count] = left_in; // put real data into bins
    fht_input2[count] = right_in;
    count++; // increment to next bin
    if (count >= FHT_N) { // check if all bins are full
      flag = 0; // tell the fht to start running
      count = 0; // reset the bin counter
    }
  }
}