/*
  FFT, CW Decode for uBITX
  KD8CEC, Ian Lee

  Version : 0.8
  -----------------------------------------------------------------------
  License : See fftfunctions.cpp for FFT and CW Decode.
**********************************************************************/

#include <ADC.h>
#include <i2c_t3.h>   // using i2c_t3 library for multiple I2C busses
#include <EEPROM.h>
#include "TeensyDSP.h"

//const uint8_t responseHeader[11]={'p', 'm', '.', 's', 'p', '.', 't', 'x', 't', '=', '"'}; //for Spectrum from DSP
//const uint8_t responseFooter[4]={'"', 0xFF, 0xFF, 0xFF};
//const char hexCodes[16] = {'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'a', 'b', 'c', 'd', 'e', 'f', };

unsigned long SAMPLE_INTERVAL = 0;

int i2cCommand = 0;
//void calculateCoeff(uint8_t freqIndex);
uint8_t cwDecodeHz = 9;
int magnitudelimit_low = 30;

char forwardBuff[MAX_FORWARD_BUFF_LENGTH + 1];
static int nowBuffIndex = 0;
static char etxCount = 0;
static char nowSendingProtocol = 0;

uint8_t SMeterToUartSend       = 0; //0 : Send, 1: Idle
uint8_t SMeterToUartIdleCount  = 0;
#define SMeterToUartInterval  4

char DSPType = 1; //0 : Not Use, 1 : FFT, 2 : Morse Decoder, 3 : RTTY Decoder
char FFTToUartIdleCount = 0;
#define FFTToUartInterval 2

elapsedMillis sinceForward = 0;
uint8_t responseCommand = 0;  //
bool isTX = false;

/**********************************************************************/

void responseConfig()
{
  if (responseCommand == 2)
  {
    unsigned long returnValue = 0;
    if (DSPType == 0)
    {
      returnValue = 94; //None
    }
    else if (DSPType == 1)
    {
      returnValue = 95; //Spectrum (FFT) mode
    }
    else if (DSPType == 2)
    {
      returnValue = 100 + cwDecodeHz;
    }

    returnValue = returnValue << 8;
    returnValue = returnValue |  (SMeterToUartSend & 0xFF);
    returnValue = returnValue << 8;
    uint8_t tmpValue = 0;
    if (magnitudelimit_low > 255)
      tmpValue = 255;
    else if (magnitudelimit_low < 1)
      tmpValue = 0;
    else
      tmpValue = magnitudelimit_low;
    returnValue = returnValue |  (tmpValue & 0xFF);

    sendCommandUL('v', returnValue);     //Return data
    sendCommandUL('g', 0x6A);     //Return data
  }
  responseCommand = 0;
}

//Result : if found .val=, 1 else 0
/*!
    @brief Parse commands...
*/

char commandParser(int lastIndex)
{
  //Analysing Forward data
  //59 58 68 4A   1C 5F 6A E5     FF FF 73
  //Find Loopback protocol
  // 0  1  2  3  4  5  6  7  8  9 10 11 12 13 14 15 16 17 18 19    20 21 22
  //70 6D 2E 76 76 2E 76 61 6C 3D 33 38 34 38 39 35 33 36 32 38    FF FF FF
  //pm.vv.val=3848953628\xFF\xFF\xFF
  //1234567890XXX
  //
  int startIndex = 0;

  //Loop back command has 13 ~ 23
  if (lastIndex < 13)
  {
    return 0;
  }

  //Protocol MAX Length : 22
  if (lastIndex >= 22)
  {
    startIndex = lastIndex - 22;
  }
  else
  {
    startIndex = 0;
  }

  for (int i = lastIndex - 3; i >= startIndex + 7; i--)
  {
    //Find =
    if (forwardBuff[i - 3] == 'v' && forwardBuff[i - 2] == 'a' && forwardBuff[i - 1] == 'l' && forwardBuff[i] == '=') //0x3D
    {
      uint8_t command1 = forwardBuff[i - 6]; //v
      uint8_t command2 = forwardBuff[i - 5]; //v
      //     i-4    //.
      forwardBuff[lastIndex - 2] = 0;
      long commandVal = atol(&forwardBuff[i + 1]);
      uint8_t *readBuff = (uint8_t *)&commandVal;

      //Loop Back
      if (command1 == 'v' && command2 == 'v')
      {
        int calcChecksum = readBuff[0] + readBuff[1] + readBuff[2];
        calcChecksum = calcChecksum % 256;

        //Correct Checksum and Receiver is DSP Moudle protocol v1.0
        if (calcChecksum == readBuff[3] && readBuff[2] == 0x6A)
        {
          //Serial1.print("Correct Checksum Command : ");
          //Serial1.println(readBuff[1]);
          uint8_t cmd1 = readBuff[1];
          if (cmd1 == 94)
          {
            DSPType = 0;
            EEPROM.put(EEPROM_DSPTYPE, DSPType);
          }
          else if (cmd1 == 95)
          {
            //Serial1.println("Spectrum Mode");
            DSPType = 1;
            EEPROM.put(EEPROM_DSPTYPE, DSPType);
          }
          else if (cmd1 >= 100 && cmd1 <= 145)
          {
            cwDecodeHz = cmd1 - 100;
            //calculateCoeff(cwDecodeHz);
            DSPType = 2;
            EEPROM.put(EEPROM_DSPTYPE, DSPType);
            EEPROM.put(EEPROM_CW_FREQ, cwDecodeHz);
          }
          else if (cmd1 > 1 && cmd1 <= 5) //2~5 : Request Configuration
          {
            responseCommand = cmd1;
          }
          else if (cmd1 == 50 || cmd1 == 51) //Set Configuration
          {
            SMeterToUartSend = (cmd1 == 51);
            EEPROM.put(EEPROM_SMETER_UART, SMeterToUartSend);
          }
          else if (cmd1 >= 146 && cmd1 <= 156 )
          {
            //Save Mode
            magnitudelimit_low = (cmd1 - 146) * 10;
            EEPROM.put(EEPROM_CW_MAG_LOW, magnitudelimit_low);
          } //end of if
        } //end of check Checksum
      }   //end of check Protocol (vv)
      else if (command1 == 'c' && command2 == 't')  //TX, RX
      {
        if (commandVal == 0)  //RX
        {
          isTX = false;
          SMeterToUartIdleCount = 0;
        }
        else if (commandVal == 1) //TX
        {
          isTX = true;
          SMeterToUartIdleCount = 0;
        }
      }

      return 1;
    }     //end of check Protocol (.val)
  }       //end of for

  //Not found Protocol (.val=
  return 0;
}



//#define PROTOCOL_TIMEOUT = 100

/*!
    @brief Forwards serial data from the RX line to the TX line.
*/

void forwardData(void)
{
  char recvChar;

  if (Serial1.available() > 0)
  {
    Serial1.flush();

    // Check RX buffer for available data.
    while (Serial1.available() > 0)
    {
      recvChar = char(Serial1.read());

      forwardBuff[nowBuffIndex] = recvChar;

      if (recvChar == 0xFF)       // found ETX
      {
        etxCount++;               // Nextion protocol, ETX: 0xFF, 0xFF, 0xFF
        if (etxCount >= 3)
        {
          // Finished Protocol
          if (commandParser(nowBuffIndex) == 1)
          {
            nowSendingProtocol = 0; // finished 1 set command
            etxCount = 0;
            nowBuffIndex = 0;
          }
        }
      }
      else
      {
        nowSendingProtocol = 1; // sending data
        etxCount = 0;
      }

      Serial1.write(recvChar);
      sinceForward = 0;
      nowBuffIndex++;

      if (nowBuffIndex > MAX_FORWARD_BUFF_LENGTH - 2)
      {
        nowBuffIndex = 0;
      }
    }

    Serial1.flush();
  }
  else
  {
    // check timeout
  }
}

/**********************************************************************/

void sendMeterData(uint8_t isSend)
{
  scaledSMeter = Sensors.sMeterScaled();

  /*
    1 : with noise (not use 0 ~ S3)
    2 : -93 ~ -89
    3 : -88 ~ -81
    4 : -80 ~ -78
    5 : -77 ~ -72
    6 : -71 ~ -69
  */

  if (isSend == 1)
  {
    if (L_scaledSMeter != scaledSMeter)
    {
      L_scaledSMeter = scaledSMeter;
      sendCommand1Num(CMD_SMETER, L_scaledSMeter);
    }
  }
}

/**********************************************************************/

//void sendFFTData(void)
//{
//  int readValue = 0;
//    for (int i = 0; i < 11; i++)
//      Serial1.write(responseHeader[i]);
//
//    for(int i = 1; i < 64; i++)
//    {
//      readValue = (int)(FFTReal[i]);
//      if (readValue < 0)
//      {
//        readValue = 0;
//      }
//      else if (readValue>255)
//      {
//        readValue=255;
//      }
//      Serial1.write(hexCodes[readValue >> 4]);
//      Serial1.write(hexCodes[readValue & 0xf]);
//    }
//
//  for (int i = 0; i < 4; i++)
//    Serial1.write(responseFooter[i]);
//}

void setup()
{
  // load configuration
  EEPROM.get(EEPROM_DSPTYPE, DSPType);
  if (DSPType > 5)
  {
    DSPType = 1;
  }

  // signal meter
  EEPROM.get(EEPROM_SMETER_UART, SMeterToUartSend);
  if (SMeterToUartSend > 2)
  {
    SMeterToUartSend = 1;
  }

  // something with CW decoding...
  EEPROM.get(EEPROM_CW_FREQ, cwDecodeHz);
  if (cwDecodeHz > 40 || cwDecodeHz < 1)
  {
    cwDecodeHz = 9;
  }

  // EEPROM_CW_MAG_LOW
  EEPROM.get(EEPROM_CW_MAG_LOW, magnitudelimit_low);
  if (magnitudelimit_low > 1000 || magnitudelimit_low < 1)
  {
    magnitudelimit_low = 50;
  }

  // put your setup code here, to run once:

  // slave Wire1 configuration for communication with the Raduino
  Wire1.begin(I2CMETER_ADDR);
  Wire1.onReceive(i2cReceiveEvent);
  Wire1.onRequest(i2cRequestEvent);

  // Serial1 configuration for communication with Raduino (RX) and Nextion (TX)
  Serial1.begin(9600, SERIAL_8N1);
  Serial1.flush();

  SAMPLE_INTERVAL = round(1000000 * (1.0 / SAMPLE_FREQUENCY));
  //calculateCoeff(cwDecodeHz);  //Set 750Hz //9 * 50 + 300 = 750Hz
  //Serial1.println("Start...");

  DBGCMD( DSP.begin() );
  DBGCMD( TR.begin()  );
}

/*!
    @brief  Receive a command via I2C.  The most recent command will be received, which will
            indicate which data the DSP should be preparing to return.
    @param  numBytes
            Number of bytes received--not used in this procedure.
*/
void i2cReceiveEvent(size_t numBytes)
{
  int readCommand = 0;

  while (Wire1.available() > 0) // for Last command
  {
    readCommand = Wire1.read();
    // KC4UPR:  Note that this looks to be only reading the last command, i.e.
    // if multiple commands have been queued up, only the last will get executed.
  }

  if (0x50 <= readCommand && readCommand <= 0x59)
  {
    i2cCommand = readCommand;
  }
}

/*!
    @brief  Respond to a request from the I2C Master (Raduino).  Returns the appropriate data
            based on whatever command was previously issued.
*/
void i2cRequestEvent(void)
{
  //int maxValue = 0;
  //int minValue = 30000;
  //int readValue = 0;
  //unsigned long curr = 0;

  switch (i2cCommand) {
    case I2CMETER_CALCS:
      // Returns an already-calculated S-meter value.
      Wire1.write(scaledSMeter);
      break;

    case I2CMETER_UNCALCS:
      // Returns a raw signal strength value.
      Wire1.write(Sensors.sMeterUnscaled() >> 2); // divided by 4... do we want this?
      break;

    case I2CMETER_CALCP:
      // Returns a raw forward power value.
      Wire1.write(fwdPower);
      break;

    case I2CMETER_CALCR:
      // Returns a raw reverse power value.
      Wire1.write(revPower);
      break;

    default:
      break;
  }
}

//extern void Decode_Morse(float magnitude);
//extern double coeff;

#define LAST_TIME_INTERVAL 159

// for boot delay, a lot of data to transfer
// Delay 2.5 Sec
byte isBooted = 0;

//======================================================================
// ADC PROCESSES
//======================================================================

elapsedMillis sinceFrameMillis = 0;
elapsedMillis sinceADCMillis = 0;

#define FRAME_RATE        40
#define FRAME_INTERVAL_MS (1000/FRAME_RATE)

const int frameIntervalMillis = FRAME_INTERVAL_MS;

#define ADC_SAMPLE_RATE   120
#define ADC_INTERVAL_MS   (1000/ADC_SAMPLE_RATE)

const int adcIntervalMillis = ADC_INTERVAL_MS;

//======================================================================
// MAIN LOOP
//======================================================================

#ifdef DEBUG
int frameCounter = 0;
#endif

void loop()
{
  //char isProcess = 0; // 0: init, 1: complete ADC sampling, 2: complete FFT
  //isProcess = 0;

  forwardData();

  if (isBooted < 100)
  {
    //Delay 20msec
    for (int i = 0; i < 20; i++)
    {
      forwardData();
      delay(1);
    }
    isBooted++;
    return;
  }

  if (sinceFrameMillis > frameIntervalMillis) {
    // Do stuff that we do once per frame--I/O.
    // TODO: debug output (frame skipping / utilization).
    sinceFrameMillis = 0;

#ifdef DEBUG
    // For debugging, output some debug info every 1.0" (40 frames @ 40 Hz).
    frameCounter++;
    if (frameCounter % 40 == 0) {
      Serial.print("DBG: Frame: ");
      Serial.print(frameCounter);
      if (isTX) {
        Serial.print(", Loop State: TX");
      } else {
        Serial.print(", Loop State: RX");
      }
      if (TR.transmitting()) {
        Serial.println(", TR State: TX");
      } else {
        Serial.println(", TR State: RX");
      }
      Serial.print("DBG: S-Meter Raw: ");
      Serial.print(Sensors.sMeterUnscaled());
      Serial.print(", S-Meter Scaled: ");
      Serial.println(scaledSMeter);
      Serial.print("DBG: VSWR Calc: ");
      Serial.print(calcVSWR, 2);      
      Serial.print(", VSWR Scaled: ");
      Serial.print(scaledVSWR);
      Serial.print(", FWD PWR: ");
      Serial.print(fwdPower, 2);
      Serial.print(", REV PWR: ");
      Serial.println(revPower, 2);
    }
#endif

    TR.update();

    if (isTX) {
      calcVSWR = Sensors.VSWR();
      scaledVSWR = byte(Sensors.scaledVSWR());
      fwdPower = Sensors.Pfwd();
      revPower = Sensors.Prev();

      // Send SWR meter information.
      if (L_scaledVSWR != scaledVSWR) {
        L_scaledVSWR = scaledVSWR;
        sendCommand1Num(CMD_SMETER, scaledVSWR);
      }

      // Send forward power.
      if (L_fwdPower != fwdPower) {
        L_fwdPower = fwdPower;
        sendCommandL('m', fwdPower * 100);  // watts x 100?
        sendCommand1Num('m', 2);
      }

      // Send reverse power.
      //if (L_revPower != revPower) {
      //  L_revPower = revPower;
      //  sendCommandL('m', revPower * 100);  // watts x 100?
      //  sendCommand1Num('m', 2);
      //}

      // Does there need to be some kind of 250-500ms delay after this???
      // Delay 250msec ~ 500msec for Nextion LCD Processing (using m protocol)
      //for (int i = 0; i < 10; i++) {
      //  forwardData();
      //  if (!isTX) { //if TX -> RX break
      //    break;
      //  }
      //  delay(25);
      //} //end of delay time

      // Send SWR.
      if (L_calcVSWR != calcVSWR) {
        L_calcVSWR = calcVSWR;
        sendCommandL('m', int(calcVSWR * 100.0)); // SWR x 100?
        sendCommand1Num('m', 3);
      }

    } else {  // RX

      // Send Signal Meter to UART
      if (SMeterToUartSend == 1 && nowSendingProtocol == 0)        //SMeter To Uart Send
      {
        //nowSendingProtocol -> not finished data forward, (not found 0xff, 0xff, 0xff yet)
        sendMeterData(1);
      } else {
        sendMeterData(0); //only calculate Signal Level
      }

    }

    // Forward any data that came in while we were updating stuff.
    forwardData();
  }

  if (sinceADCMillis > adcIntervalMillis) {
    // Do stuff that we do once per ADC interval--ADC colllection.
    // TODO: debug output (frame skipping / utilization).
    sinceADCMillis = 0;

    if (isTX) {
      Sensors.updatePower();
    } else {  // RX
      Sensors.updateSMeter();
      Sensors.updateSupply();
    }

    // Forward any data that came in while we were reading sensors.
    //forwardData();
  }

  // Check Response Command
  if (responseCommand > 0 && sinceForward > LAST_TIME_INTERVAL)
  {
    responseConfig();
  }

  //  //===========================================
  //  //TRANSCEIVER STATUS : RX
  //  //===========================================
  //  //===================================================================================
  //  // DSP Routine
  //  //===================================================================================
  //  if (DSPType == 1 && sinceForward > LAST_TIME_INTERVAL) // spectrum: FFT => send To UART
  //  {
  //    FFTToUartIdleCount = 0;
  //
  //    if (isProcess == 1)
  //    {
  //      FFT(FFTReal, FFTImag, SAMPLESIZE, 7);
  //      isProcess = 2;
  //    }
  //
  //    forwardData();
  //
  //    if (isProcess == 2)
  //    {
  //      for (uint16_t k = 0; k < SAMPLESIZE; k++)
  //      {
  //        FFTReal[k] = sqrt(FFTReal[k] * FFTReal[k] + FFTImag[k] * FFTImag[k]);
  //      }
  //
  //      isProcess = 3;
  //    }
  //
  //    forwardData();
  //
  //    if (isProcess == 3)
  //    {
  //      if (nowSendingProtocol == 0)  //Idle Status
  //      {
  //        sendFFTData();
  //      }
  //    }
  //  }
  //  else if (DSPType == 2)          //Decode Morse
  //  {
  //    //Implement Goertzel_algorithm
  //    //https://en.wikipedia.org/wiki/Goertzel_algorithm
  //
  //    /*
  //    ω = 2 * π * Kterm / Nterms;
  //    cr = cos(ω);
  //    ci = sin(ω);
  //    coeff = 2 * cr;
  //
  //    sprev = 0;
  //    sprev2 = 0;
  //    for each index n in range 0 to Nterms-1
  //      s = x[n] + coeff * sprev - sprev2;
  //      sprev2 = sprev;
  //      sprev = s;
  //    end
  //
  //    power = sprev2 * sprev2 + sprev * sprev - coeff * sprev * sprev2;
  //     */
  //    double Q1 = 0;
  //    double Q2 = 0;
  //
  //    for (unsigned index = 0; index < DECODE_MORSE_SAMPLESIZE; index++)
  //    {
  //      float Q0;
  //      Q0 = coeff * Q1 - Q2 + FFTReal[index];
  //      Q2 = Q1;
  //      Q1 = Q0;
  //    }
  //    double magnitudeSquared = (Q1*Q1)+(Q2*Q2)-Q1*Q2*coeff;  // we do only need the real part //
  //    double magnitude = sqrt(magnitudeSquared);
  //
  //    Decode_Morse(magnitude);
  //  } //end of if

}

//======================================================================
// EOF
//======================================================================