#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(1000);
      
  //~~~~~~~~~~~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->freq1, myPedal->freq2, myPedal->pan1, myPedal->pan2);

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

      //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
      //~~~~~~~~~Modulation Processing~~~~~~~~~~~~~
      //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
        lfo1.update();
        lfo2.update();

        //txr = 0x7fff * myPedal->pan2 * lfo2.getOutput(); // lfo only
        //txl = 0x7fff * myPedal->pan1 * lfo1.getOutput();
      
        txl = ((1 - myPedal->pan1) * rxl) + (myPedal->pan1 * (rxl * lfo1.getOutput()));
        txr = ((1 - myPedal->pan2) * rxr) + (myPedal->pan2 * (rxr * lfo2.getOutput()));
      //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

        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