#include <Arduino.h>
#include "set_settings.h"
#include "set_module.h"
#include "set_codec.h"
#include "task.h"
#include <SD.h>
#include "sd_play.h"

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

SDplay mySDplay; //create an instance of SDplay class 

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~~SETUP~~~~~~~~~~~~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

void setup() 
{
  Serial.begin(115200);
  while(!Serial);
  delay(3000);
      
  //~~~~~~~~~~~codec is initialized  See Codec.cpp~~~~~~~~~~~~~~~~~
  //~~~~i2c is initialized within codec.init() with initI2C()~~~~~~

        Wire.begin();
        Serial.println("Initialize Codec Codec ");
        codec.begin();
        codec_sets();
        Serial.println("Codec Init success!!");

  //~~~~~~I2S  See set_settings.cpp for I2S ~~~~~~~~~~~~~~~
  
        I2S_init();

         mySDplay.SDCardInit();

        mySDplay.OpenWaveFile();
  
  //~~~~~~~~~~~~~~Monitor (can be commented out)~~~~~~~~~~
       
        Serial.println("I2S/SD setup complete");
        runSystemMonitor();  //for testing only

} //Setup End


//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~~MAIN LOOP~~~~~~~~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

void loop() 
{
    size_t readsize = 0;
    byte txbuf[SAMPLES_BUFFER_SIZE]; //128 L+R signed 16 bit samples 
    float rxl, rxr, txl, txr;  //left and right single samples, processed as floats

    myPedal->init();
    taskSetup();

  while(1)
  {   //signal processing loop

    setDebugVars(myPedal->depth, myPedal->freq, myPedal->pan, myPedal->beatFrequency);

    //gather some input samples into Samples buffer from the SD wavfile,
      mySDplay.ReadFile(mySDplay.Samples);  
  
    for (int i=0; i<(SAMPLES_BUFFER_SIZE); i+=4)  //process samples one at a time from Samples[]  
    { 
       rxl = (float)((int16_t)(mySDplay.Samples[i+1] << 8) | mySDplay.Samples[i]);   // Left sample float
       rxr = (float)((int16_t)(mySDplay.Samples[i+3] << 8) | mySDplay.Samples[i+2]); // Right sample float

      //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
      //~~~~~~~~~stereoChorus Processing~~~~~~~~~~~~~
      //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
      delay1.write(rxl);
      delay2.write(rxr); //write anyway, no matter it's stereo or mono input
      
      lfo1.update();
      lfo2.update();

      float dt1 = (1 + lfo1.getOutput())* myPedal->depth;
      float dt2 = (1 + lfo2.getOutput())* myPedal->depth;  
      
        txl = ((1 - myPedal->pan) * rxl) + (myPedal->pan * delay1.read(dt1));
        txr = ((1 - myPedal->pan) * rxr) + (myPedal->pan * delay2.read(dt2));
      //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

        txbuf[i]   = ((int16_t) txl) & 0xff ;  // Left sample loaded as two bytes
        txbuf[i+1] = ((int16_t) txl) >> 8; 
        txbuf[i+2] = ((int16_t) txr) & 0xff ;  // Right sample loaded as two bytes
        txbuf[i+3] = ((int16_t) txr) >> 8;

    } // End of for loop

     // play processed transmit buffer by loading txbuf into DMA memory
    mySDplay.FillI2SBuffer(txbuf); 

  } // End of while(1) loop
}   // End of Main Loop