/**
 * @file Theremin_vE.ino
 * @author Kyle Horn, Quentin Bolsee, Elsa Forberger
 * @brief Documented Version Theremin Software 
 * @version 3.1
 * @date 2024-12-13
 * 
 */


/*
    Includes
*/
#include "Adafruit_FreeTouch.h"
#include <Ewma.h>
#include <Wire.h>
#include "audio_sample.h"

#include <Adafruit_NeoPixel.h>
#define PIN A4 
#define NUMPIXELS 4 
Adafruit_NeoPixel pixels(NUMPIXELS, PIN, NEO_GRB + NEO_KHZ800);

//#include <Adafruit_NeoPixel.h>

// #define PIN_NEO_PIXEL 2  // The SAMD21 pin connected to NeoPixel
// #define NUM_PIXELS 4      // The number of LEDs (pixels) on NeoPixel

/*
    Definitions
*/
#define PIN_BUZZER    2
#define FACTOR_MAX    20000UL
#define CLK_HZ        48000000UL 
#define LOW_TONE      ((float)65)
#define HIGH_TONE     ((float)1024)


/*
    Globals
*/
Ewma adcFilter(0.05);   // Less smoothing - faster to detect changes, but more prone to noise
Adafruit_FreeTouch t6 = Adafruit_FreeTouch(0,OVERSAMPLE_64,RESISTOR_100K,FREQ_MODE_NONE); // pin 6 is actually 0
Adafruit_FreeTouch t7 = Adafruit_FreeTouch(1,OVERSAMPLE_64,RESISTOR_100K,FREQ_MODE_NONE); // pin 7 is actually 1
int t6min,t7min;
int32_t baseline6,baseline7;
uint32_t factor = 0;
int i_sample = 0;
uint32_t cycles_for_testing = 0;

//Adafruit_NeoPixel NeoPixel(NUM_PIXELS, PIN_NEO_PIXEL, NEO_GRB + NEO_KHZ800);

// int L = LOW_TONE;
// int H = HIGH_TONE;

// int Freq_Range = H-L;
// int32_t RS = Freq_Range / 6;

// int32_t ROY[2] = {L, L + RS};
// int32_t YG[2] = {L+ RS, L + 2*RS};
// int32_t GB[2] = {L+ 2*RS, L + 3*RS};
// int32_t BB[2] = {L+ 3*RS, L + 4*RS};
// int32_t BV[2] = {L+ 4*RS, L + 5*RS};
// int32_t VR[2] = {L+ 5*RS, H};

/**
 * @brief Converts a tone in Hz to a sample rate based on the length of the sin wave 
 * 
 * @param tone float in Hz
 * @return uint16_t sample rate in Hz
 */
uint16_t convertToneToSampleRate(float tone) {
  return (uint16_t) tone * N_SAMPLES; // lacks overflow protection
}

/**
 * @brief Converts a sample rate in Hz to a tick count. Uses the macro CLK_HZ.
 * 
 * @param sr sample rate
 * @return uint16_t tick count
 */
uint16_t convertSampleRateToTickCount(uint16_t sr) {
  return (uint16_t) (CLK_HZ / (uint32_t (sr)));
}


/**
 * @brief configures TC5
 * 
 * @param startingTickCount a tick count obtained with convertSampleRateToTickCount
 */
void tcConfigure(uint16_t startingTickCount) {
  // enable the general clock (CLKEN), use general clock 0 (GEN_GCLK0), and use Timer/Counter 5 as the source
  GCLK->CLKCTRL.reg = (uint16_t) (GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK0 | GCLK_CLKCTRL_ID(GCM_TC4_TC5)) ;
  while (GCLK->STATUS.bit.SYNCBUSY);
  tcReset();
  TC5->COUNT16.CTRLA.reg |= TC_CTRLA_MODE_COUNT16;
  TC5->COUNT16.CTRLA.reg |= TC_CTRLA_WAVEGEN_MFRQ;
  tcChangeCount(startingTickCount);
  while (tcIsSyncing());

  NVIC_DisableIRQ(TC5_IRQn);
  NVIC_ClearPendingIRQ(TC5_IRQn);
  NVIC_SetPriority(TC5_IRQn, 0);
  NVIC_EnableIRQ(TC5_IRQn);

  // enable interrupt
  TC5->COUNT16.INTENSET.bit.MC0 = 1;
}

/**
 * @brief changes the tick count of TC5, which changes the frequency at which the sin wave plays.
 * 
 * @param newTickCount exactly what it says on the tin
 */
void tcChangeCount(uint16_t newTickCount) {
    TC5->COUNT16.CC[0].reg = newTickCount;
}

bool tcIsSyncing() {
  return TC5->COUNT16.STATUS.reg & TC_STATUS_SYNCBUSY;
}

void tcStartCounter() {
  TC5->COUNT16.CTRLA.reg |= TC_CTRLA_ENABLE;
  while (tcIsSyncing()); //wait until snyc'd
}

void tcReset() {
  TC5->COUNT16.CTRLA.reg = TC_CTRLA_SWRST;
  while (tcIsSyncing());
  while (TC5->COUNT16.CTRLA.bit.SWRST);
}

void tcDisable() {
  TC5->COUNT16.CTRLA.reg &= ~TC_CTRLA_ENABLE;
  while (tcIsSyncing());
}

void setup() {
  Serial.begin(9600);
 // while (!Serial);

  uint16_t low_sr = convertToneToSampleRate(LOW_TONE);
  uint16_t high_sr = convertToneToSampleRate(HIGH_TONE);

  uint16_t low_tr = convertSampleRateToTickCount(low_sr);
  uint16_t high_tr = convertSampleRateToTickCount(high_sr);

  uint16_t tick_count_range =  low_tr - high_tr;
  float tone_resolution = (HIGH_TONE - LOW_TONE) / tick_count_range;

  char strBuf[200];
  sprintf(strBuf, "Given low tone frequency %d, sampling rate is %d and tick count is %d\n", (int)LOW_TONE, low_sr, low_tr);
  Serial.print(strBuf);
  sprintf(strBuf, "Given high tone frequency %d, sampling rate is %d and tick count is %d\n", (int)HIGH_TONE, high_sr, high_tr);
  Serial.print(strBuf);
  sprintf(strBuf, "The range of tick counts between the given tones is %d long, giving a tone resolution of %d*0.01 Hz\n", tick_count_range, (int)(100*tone_resolution));
  Serial.print(strBuf);

  t6.begin();
  t7.begin();
  t6min = t7min = 1e6;
  baseline6 = t6.measure();
  baseline7 = t7.measure();

  // BUZZER
  pinMode(PIN_BUZZER, OUTPUT);
  analogWriteResolution(255);
  analogWrite(PIN_BUZZER, 0);

  // SYNTHESIZER
  tcConfigure(low_tr);
  tcStartCounter();

  // TEST MATH  
  //testTone(5.0);
  //testTone(7.0);
  //testTone(1000.0);
  //testTone(high_tr);
  //testTone(low_tr);

 // NeoPixel.begin();

  pixels.begin(); 
  pixels.clear();
  pixels.setPixelColor(0, pixels.Color(150, 0, 0));
  pixels.setPixelColor(1, pixels.Color(150, 150, 0));
  pixels.setPixelColor(2, pixels.Color(0, 150, 0));
  pixels.setPixelColor(3, pixels.Color(0, 0, 150));
  pixels.show();

}



void TC5_Handler (void) {
  // busy DAC, abort this sample (womp womp)
  if (DAC->STATUS.bit.SYNCBUSY == 1) {
    Serial.print("Busy DAC...\n");
    TC5->COUNT16.INTFLAG.bit.MC0 = 1; // re-enable interrupt
    return;
  }

  uint32_t samp = 0;

  // sin wave
  samp = samples[i_sample] * factor / FACTOR_MAX;
  i_sample++;
  if (i_sample >= N_SAMPLES) {
    i_sample = 0;
  }

  // increment test counter
  cycles_for_testing++;

  // send sample to DAC
  DAC->DATA.reg = samp & 0x3FF; // 10 bit DAC

  // re-enable interrupt
  TC5->COUNT16.INTFLAG.bit.MC0 = 1;
}

// void testTone(float toneFreq) {
//   char strBuf[200];
//   uint16_t sr = convertToneToSampleRate(toneFreq);
//   uint16_t tc = convertSampleRateToTickCount(sr);
//   tcChangeCount(tc);
//   cycles_for_testing = 0;
//   delay(1000);
//   sprintf(strBuf, "TEST: Tone %d Hz (rounded) gives frequency %d Hz and a tick count of %d. The actual ISR frequency was %d Hz.\n", (int)toneFreq, sr, tc, cycles_for_testing);
//   Serial.print(strBuf);
//   cycles_for_testing = 0;
// }


void loop() {

  uint16_t low_sr = convertToneToSampleRate(LOW_TONE);
  uint16_t high_sr = convertToneToSampleRate(HIGH_TONE);

  uint16_t low_tr = convertSampleRateToTickCount(low_sr);
  uint16_t high_tr = convertSampleRateToTickCount(high_sr);

  uint16_t tick_count_range =  low_tr - high_tr;
  float tone_resolution = (HIGH_TONE - LOW_TONE) / tick_count_range;

  int32_t result6,result7;
  
  // plotting scale limits
  
  // Serial.print(0);
  // Serial.print(",");
  // Serial.print(300);
  // Serial.print(",");
  
  // read values
  result6 = t6.measure()-baseline6;
  if (result6 < t6min) t6min = result6;

  result7 = t7.measure()-baseline7;
  if (result7 < t7min) t7min = result7;

  // filter, constrain, and translate results
  // if (result7 <= 50){
  //   factor = FACTOR_MAX - (max(0,map(result7, 0, 50, 0, FACTOR_MAX)));
  // } else{ 
  //   factor = FACTOR_MAX;
  // }



  if (result7 <= 50){
    factor = map(result7, 0, 50, FACTOR_MAX, 0);
  }
  else{ 
    factor = 0;
  }

  float filtered1 = adcFilter.filter(result6);

  // Linearization of filtered data
  // See MultiMap Code

  

  uint16_t target_freq = (uint16_t) min(max(map((int) filtered1, 0, 60, low_sr, high_sr),low_sr),high_sr);
  uint16_t tick_count = convertSampleRateToTickCount(target_freq);
  tcChangeCount(tick_count); 

  // uint32_t vol = factor/FACTOR_MAX;
  // uint32_t red;
  // uint32_t green;
  // uint32_t blue;


  // if (target_freq > ROY[1] && target_freq < ROY[2]) {
  //   red = 1*vol;
  //   green = map(target_freq, ROY[1], ROY[2], 0, 1)*vol;
  //   blue = 0*vol;
  //   NeoPixel.setPixelColor(0, NeoPixel.Color(red, green, blue));
  //   NeoPixel.show();
  // }

  // if (target_freq > YG[1] && target_freq < YG[2]) {
  //   red = map(target_freq, YG[1], YG[2], 1, 0)*vol;
  //   green = 1*vol;
  //   blue = 0*vol;
  //   NeoPixel.setPixelColor(0, NeoPixel.Color(red, green, blue));
  //   NeoPixel.show();
  // }

  // if (target_freq > GB[1] && target_freq < GB[2]) {
  //   red = 0*vol;
  //   green = 1*vol;
  //   blue = map(target_freq, GB[1], GB[2], 0, 1)*vol;
  //   NeoPixel.setPixelColor(0, NeoPixel.Color(red, green, blue));
  //   NeoPixel.show();
  // }

  // if (target_freq > BB[1] && target_freq < BB[2]) {
  //   red = 0*vol;
  //   green = map(target_freq, BB[1], BB[2], 1, 0)*vol;
  //   blue = 1*vol;
  //   NeoPixel.setPixelColor(0, NeoPixel.Color(red, green, blue));
  //   NeoPixel.show();
  // }

  // if (target_freq > BV[1] && target_freq < BV[2]) {
  //   red = map(target_freq, BV[1], BV[2], 0, 1)*vol;
  //   green = 0*vol;
  //   blue = 1*vol;
  //   NeoPixel.setPixelColor(0, NeoPixel.Color(red, green, blue));
  //   NeoPixel.show();
  // }

  // if (target_freq > VR[1] && target_freq < VR[2]) {
  //   red = 1*vol;
  //   green = 0*vol;
  //   blue = map(target_freq, VR[1], VR[2], 1, 0)*vol;
  //   NeoPixel.setPixelColor(0, NeoPixel.Color(red, green, blue));
  //   NeoPixel.show();
  // }

  // print results to serial
  
  // char strBuf[200];
  // sprintf(strBuf, "r6: %d -> %d (rounded) -> %d Hz -> %d ticks, r7: %d -> %d\n", result6, (int)filtered1,  target_freq, tick_count, result7, factor);
  // Serial.print(strBuf);

  char strBuf[200];
  sprintf(strBuf, "%d, %d, %d\n", target_freq, tick_count, factor);
  Serial.print(strBuf);
      
}