/****************************************************************
 * Example1_Basics.ino
 * ICM 20948 Arduino Library Demo
 * Use the default configuration to stream 9-axis IMU data
 * Owen Lyke @ SparkFun Electronics
 * Original Creation Date: April 17 2019
 *
 * Please see License.md for the license information.
 *
 * Distributed as-is; no warranty is given.
 ***************************************************************/
#include "ICM_20948.h" // Click here to get the library: http://librarymanager/All#SparkFun_ICM_20948_IMU
#include "BLECStringCharacteristic.h"
#include "EString.h"
#include "RobotCommand.h"
#include <ArduinoBLE.h>


//////////// BLE UUIDs ////////////
#define BLE_UUID_TEST_SERVICE "4b03ace2-e7d1-4f19-b326-5e1d6207f755"


#define BLE_UUID_RX_STRING "9750f60b-9c9c-4158-b620-02ec9521cd99"


#define BLE_UUID_TX_FLOAT "27616294-3063-4ecc-b60b-3470ddef2938"
#define BLE_UUID_TX_STRING "f235a225-6735-4d73-94cb-ee5dfce9ba83"

//////////// BLE UUIDs ////////////


//////////// Global Variables ////////////
BLEService testService(BLE_UUID_TEST_SERVICE);


BLECStringCharacteristic rx_characteristic_string(BLE_UUID_RX_STRING, BLEWrite, MAX_MSG_SIZE);


BLEFloatCharacteristic tx_characteristic_float(BLE_UUID_TX_FLOAT, BLERead | BLENotify);
BLECStringCharacteristic tx_characteristic_string(BLE_UUID_TX_STRING, BLERead | BLENotify, MAX_MSG_SIZE);


// RX
RobotCommand robot_cmd(":|");

// TX
EString tx_estring_value;
float tx_float_value = 0.0;
const int array_size = 500;
int time_data[array_size] = {0};
double temp_data[array_size] = {0};
bool RECORD_IMU = false;  // Flag to control recording

//#define USE_SPI       // Uncomment this to use SPI

#define SERIAL_PORT Serial

#define SPI_PORT SPI // Your desired SPI port.       Used only when "USE_SPI" is defined
#define CS_PIN 2     // Which pin you connect CS to. Used only when "USE_SPI" is defined

#define WIRE_PORT Wire // Your desired Wire port.      Used when "USE_SPI" is not defined
// The value of the last bit of the I2C address.
// On the SparkFun 9DoF IMU breakout the default is 1, and when the ADR jumper is closed the value becomes 0
#define AD0_VAL 1

#ifdef USE_SPI
ICM_20948_SPI myICM; // If using SPI create an ICM_20948_SPI object
#else
ICM_20948_I2C myICM; // Otherwise create an ICM_20948_I2C object
#endif

enum CommandTypes
{
    PING,
    SEND_TWO_INTS,
    SEND_THREE_FLOATS,
    ECHO,
    DANCE,
    SET_VEL,
    GET_TIME_MILLIS,
    TIME_DATA_LOOP,
    SEND_TIME_DATA,
    GET_TEMP_READINGS,
    START_IMU_DATA,
    STOP_IMU_DATA,

};


void
handle_command()
{  
    // Set the command string from the characteristic value
    robot_cmd.set_cmd_string(rx_characteristic_string.value(),
                             rx_characteristic_string.valueLength());


    bool success;
    int cmd_type = -1;


    // Get robot command type (an integer)
    /* NOTE: THIS SHOULD ALWAYS BE CALLED BEFORE get_next_value()
     * since it uses strtok internally (refer RobotCommand.h and
     * https://www.cplusplus.com/reference/cstring/strtok/)
     */
    success = robot_cmd.get_command_type(cmd_type);


    // Check if the last tokenization was successful and return if failed
    if (!success) {
        return;
    }


    // Handle the command type accordingly
    switch (cmd_type) {
        /*
         * Write "PONG" on the GATT characteristic BLE_UUID_TX_STRING
         */
        case PING:
            tx_estring_value.clear();
            tx_estring_value.append("PONG");
            tx_characteristic_string.writeValue(tx_estring_value.c_str());


            Serial.print("Sent back: ");
            Serial.println(tx_estring_value.c_str());


            break;
        /*
         * Extract two integers from the command string
         */
        case SEND_TWO_INTS:
            int int_a, int_b;


            // Extract the next value from the command string as an integer
            success = robot_cmd.get_next_value(int_a);
            if (!success)
                return;


            // Extract the next value from the command string as an integer
            success = robot_cmd.get_next_value(int_b);
            if (!success)
                return;


            Serial.print("Two Integers: ");
            Serial.print(int_a);
            Serial.print(", ");
            Serial.println(int_b);
           
            break;
        /*
         * Extract three floats from the command string
         */
        case SEND_THREE_FLOATS:
            float float_a, float_b, float_c;

            // Extract the next value from the command string as a float
            success = robot_cmd.get_next_value(float_a);
            if (!success)
                return;

            // Extract the next value from the command string as a float
            success = robot_cmd.get_next_value(float_b);
            if (!success)
                return;

            // Extract the next value from the command string as a float
            success = robot_cmd.get_next_value(float_c);
            if (!success)
                return;

            Serial.print("Three Floats: ");
            Serial.print(float_a);
            Serial.print(", ");
            Serial.print(float_b);
            Serial.print(", ");
            Serial.println(float_c);
            break;
        /*
         * Add a prefix and postfix to the string value extracted from the command string
         */
        case ECHO:


            char char_arr[MAX_MSG_SIZE];


            // Extract the next value from the command string as a character array
            success = robot_cmd.get_next_value(char_arr);
            if (!success)
                return;


            // Add prefix and postfix
            tx_estring_value.clear();
            tx_estring_value.append("Robot says ->  ");
            tx_estring_value.append(char_arr);
            tx_estring_value.append(" :)");


            // Send the modified string back
            tx_characteristic_string.writeValue(tx_estring_value.c_str());

            Serial.print("Echoed: ");
            Serial.println(tx_estring_value.c_str());
           
            break;

        /*
         * DANCE
         */
        case DANCE:
            Serial.println("Look Ma, I'm Dancin'!");


            break;
       

        /*
         * SET_VEL
         */
        case SET_VEL:


            break;


        case GET_TIME_MILLIS:

            tx_estring_value.clear();
            tx_estring_value.append("T: ");
            tx_estring_value.append((float) millis());
            tx_characteristic_string.writeValue(tx_estring_value.c_str());

            Serial.println("Sent time");

        break;


        case TIME_DATA_LOOP:
            {
            unsigned long startT = millis();

            for (int count = 0; millis() - startT < 500; ++count) {
                tx_estring_value.clear();
                tx_estring_value.append("Sample ");
                tx_estring_value.append(count);
                tx_estring_value.append(": ");
                tx_estring_value.append((float) millis());
                tx_characteristic_string.writeValue(tx_estring_value.c_str());
            }

            Serial.println("Sent multiple time samples");

            break;
          }


        case SEND_TIME_DATA:
        {
            unsigned long startT = millis();  
            int i = 0;

            for (; (millis() - startT < 5000); ++i) {
                time_data[i] = millis();

                if (i == array_size - 1)
                    Serial.println("Memory Full");
            }

            for (int j = 0; j < array_size; ++j) {
                tx_estring_value.clear();
                tx_estring_value.append("Sample ");
                tx_estring_value.append(j);
                tx_estring_value.append(": ");
                tx_estring_value.append(time_data[j]);
                tx_characteristic_string.writeValue(tx_estring_value.c_str());
            }

            Serial.println("Sent all stored time data.");

            break;
        }

        
        case GET_TEMP_READINGS:
        {
          unsigned long startT = millis();

          for (int i = 0; (millis() - startT < 5000) && (i < array_size); ++i) {
              time_data[i] = millis();  
              temp_data[i] = int((getTempDegF()));  

              delay(100);  

              if (i == array_size - 1)
                  Serial.println("Memory full");
          }

            for (int i = 0; i < array_size; i++) {

                if (time_data[i] == 0 || temp_data[i] == 0.0)
                break;

                tx_estring_value.clear();
                tx_estring_value.append(time_data[i]);
                tx_estring_value.append(" , ");
                tx_estring_value.append(temp_data[i]);
                tx_characteristic_string.writeValue(tx_estring_value.c_str());
                delay(50);  
            }


            Serial.println("Stored time and temperature data.");
            break;
        }
        case START_IMU_DATA:
        {
            Serial.println("Recording IMU data...");
            digitalWrite(LED_BUILTIN, HIGH);
            RECORD_IMU = true;
            break;
        }


        case STOP_IMU_DATA:
        {
            Serial.println("Stopped recording IMU data.");
            digitalWrite(LED_BUILTIN, LOW);
            RECORD_IMU = false;
            break;
        }


        /*
         * The default case may not capture all types of invalid commands.
         * It is safer to validate the command string on the central device (in python)
         * before writing to the characteristic.
         */
        default:
            Serial.print("Invalid Command Type: ");
            Serial.println(cmd_type);
            break;
    }
}


void setup()
{

  SERIAL_PORT.begin(115200);
  Serial.begin(115200);

  //BLE for communication 
  BLE.begin();


  // Set advertised local name and service
  BLE.setDeviceName("Artemis BLE");
  BLE.setLocalName("Artemis BLE");
  BLE.setAdvertisedService(testService);


  // Add BLE characteristics
  testService.addCharacteristic(tx_characteristic_float);
  testService.addCharacteristic(tx_characteristic_string);
  testService.addCharacteristic(rx_characteristic_string);


  // Add BLE service
  BLE.addService(testService);

  // Output MAC Address
  Serial.print("Advertising BLE with MAC: ");
  Serial.println(BLE.address());


  BLE.advertise();
  Serial.println("BLE Initialized and Advertising...");

  while (!SERIAL_PORT)
  {
  };
    pinMode(LED_BUILTIN, OUTPUT); // Set LED pin as output

    // Blink LED three times slowly
    for (int i = 0; i < 3; i++) {
        digitalWrite(LED_BUILTIN, HIGH);
        delay(500);
        digitalWrite(LED_BUILTIN, LOW);
        delay(500);
    }

#ifdef USE_SPI
  SPI_PORT.begin();
#else
  WIRE_PORT.begin();
  WIRE_PORT.setClock(400000);
#endif

  //myICM.enableDebugging(); // Uncomment this line to enable helpful debug messages on Serial

  bool initialized = false;
  while (!initialized)
  {

#ifdef USE_SPI
    myICM.begin(CS_PIN, SPI_PORT);
#else
    myICM.begin(WIRE_PORT, AD0_VAL);
#endif

    SERIAL_PORT.print(F("Initialization of the sensor returned: "));
    SERIAL_PORT.println(myICM.statusString());
    if (myICM.status != ICM_20948_Stat_Ok)
    {
      SERIAL_PORT.println("Trying again...");
      delay(500);
    }
    else
    {
      initialized = true;
    }
  }
}




// void sendxyz(float ax, float ay, float az) {
//     // Convert float to integer (scaled by 100)
//     int ax_int = ax * 100;
//     int ay_int = ay * 100;
//     int az_int = az * 100;

//     // Format manually using integer values
//     char imu_data[50];
//     sprintf(imu_data, "%d,%d,%d", ax_int, ay_int, az_int);

//     Serial.print("Sending over BLE: ");
//     Serial.println(imu_data);  // Debugging output

//     tx_characteristic_string.writeValue(imu_data);
// }

//const float alpha = 0.60;
float pitch = 0.0;
float roll = 0.0;

float pitch_offset = 0.9476842105263155;
float roll_offset  = 0.16389473684210526;

void calculatePitchRoll(float ax, float ay, float az) {

    pitch = atan2(ay, az) * 180.0 / M_PI + pitch_offset;
    roll  = atan2(ax, az) * 180.0 / M_PI + roll_offset;

    Serial.print("Pitch: ");
    Serial.print(pitch);
    Serial.print(" | Roll: ");
    Serial.println(roll);

    // Convert floats to strings manually
    // Convert float to integer (scaled by 100)
    int pitch_int = pitch * 100;
    int roll_int = roll * 100;

    // Format manually using integer values
    char imu_data[50];
    sprintf(imu_data, "%d,%d", pitch_int, roll_int);

    Serial.print("Sending over BLE: ");
    //Serial.println(imu_data);  // Debugging output

    tx_characteristic_string.writeValue(imu_data);
}

// Gyroscope angles array (store 100 samples, adjust if needed)
#define SAMPLES 500
float gyro_pitch_raw[SAMPLES] = {0};
float gyro_roll_raw[SAMPLES] = {0};
float gyro_yaw_raw[SAMPLES] = {0};

unsigned long lastT = 0;
int i = 0;  // Index for storing data

void compute_gyroscope() {
    float dt = (millis() - lastT) / 1000.;
    lastT = millis();

    gyro_pitch_raw[i] += myICM.gyrY() * dt;
    gyro_roll_raw[i]  += myICM.gyrX() * dt;
    gyro_yaw_raw[i]   += myICM.gyrZ() * dt;

    // **print raw values correctly for serial plotter**
    Serial.print(gyro_pitch_raw[i]);
    Serial.print(",");
    Serial.print(gyro_roll_raw[i]);
    Serial.print(",");
    Serial.println(gyro_yaw_raw[i]);  // println ends the line

    i = (i + 1) % SAMPLES;  // Wrap index
}


// IMU Data Storage
float accel_pitch = 0.0, accel_roll = 0.0;
float gyro_pitch = 0.0, gyro_roll = 0.0;
float comp_pitch[SAMPLES] = {0};
float comp_roll[SAMPLES] = {0};
unsigned long timestamps[SAMPLES] = {0};
int sampleIndex = 0;

// Complementary filter weight (adjust if needed)
const float alpha = 0.98;


void compute_complementary_filter() {
    float dt = (millis() - lastT) / 1000.0;
    lastT = millis();

    accel_pitch = atan2(myICM.accY(), myICM.accZ()) * 180.0 / M_PI;
    accel_roll  = atan2(myICM.accX(), myICM.accZ()) * 180.0 / M_PI;

    gyro_pitch += myICM.gyrY() * dt;
    gyro_roll  += myICM.gyrX() * dt;

    comp_pitch[sampleIndex] = alpha * (comp_pitch[sampleIndex] + myICM.gyrY() * dt) + (1 - alpha) * accel_pitch;
    comp_roll[sampleIndex]  = alpha * (comp_roll[sampleIndex] + myICM.gyrX() * dt) + (1 - alpha) * accel_roll;

    timestamps[sampleIndex] = millis();
    
    sampleIndex = (sampleIndex + 1) % SAMPLES;
}





void loop()
{
  // Listen for connections
  BLEDevice central = BLE.central();


  // If a central is connected to the peripheral
  if (central) {
      Serial.print("Connected to: ");
      Serial.println(central.address());


      // // While central is connected
      while (central.connected()) {

          if (RECORD_IMU && myICM.dataReady())
          {
            myICM.getAGMT();         // The values are only updated when you call 'getAGMT'
                                    //    printRawAGMT( myICM.agmt );     // Uncomment this to see the raw values, taken directly from the agmt structure
            //printScaledAGMT(&myICM); // This function takes into account the scale settings from when the measurement was made to calculate the values with units

            compute_complementary_filter();
          }
          else
          {
            SERIAL_PORT.println("Waiting for data");
            delay(500);
          }
      }
      Serial.println("Disconnected");
  }


}

// Below here are some helper functions to print the data nicely!

void printPaddedInt16b(int16_t val)
{
  if (val > 0)
  {
    SERIAL_PORT.print(" ");
    if (val < 10000)
    {
      SERIAL_PORT.print("0");
    }
    if (val < 1000)
    {
      SERIAL_PORT.print("0");
    }
    if (val < 100)
    {
      SERIAL_PORT.print("0");
    }
    if (val < 10)
    {
      SERIAL_PORT.print("0");
    }
  }
  else
  {
    SERIAL_PORT.print("-");
    if (abs(val) < 10000)
    {
      SERIAL_PORT.print("0");
    }
    if (abs(val) < 1000)
    {
      SERIAL_PORT.print("0");
    }
    if (abs(val) < 100)
    {
      SERIAL_PORT.print("0");
    }
    if (abs(val) < 10)
    {
      SERIAL_PORT.print("0");
    }
  }
  SERIAL_PORT.print(abs(val));
}

void printRawAGMT(ICM_20948_AGMT_t agmt)
{
  SERIAL_PORT.print("RAW. Acc [ ");
  printPaddedInt16b(agmt.acc.axes.x);
  SERIAL_PORT.print(", ");
  printPaddedInt16b(agmt.acc.axes.y);
  SERIAL_PORT.print(", ");
  printPaddedInt16b(agmt.acc.axes.z);
  SERIAL_PORT.print(" ], Gyr [ ");
  printPaddedInt16b(agmt.gyr.axes.x);
  SERIAL_PORT.print(", ");
  printPaddedInt16b(agmt.gyr.axes.y);
  SERIAL_PORT.print(", ");
  printPaddedInt16b(agmt.gyr.axes.z);
  SERIAL_PORT.print(" ], Mag [ ");
  printPaddedInt16b(agmt.mag.axes.x);
  SERIAL_PORT.print(", ");
  printPaddedInt16b(agmt.mag.axes.y);
  SERIAL_PORT.print(", ");
  printPaddedInt16b(agmt.mag.axes.z);
  SERIAL_PORT.print(" ], Tmp [ ");
  printPaddedInt16b(agmt.tmp.val);
  SERIAL_PORT.print(" ]");
  SERIAL_PORT.println();
}

void printFormattedFloat(float val, uint8_t leading, uint8_t decimals)
{
  float aval = abs(val);
  if (val < 0)
  {
    SERIAL_PORT.print("-");
  }
  else
  {
    SERIAL_PORT.print(" ");
  }
  for (uint8_t indi = 0; indi < leading; indi++)
  {
    uint32_t tenpow = 0;
    if (indi < (leading - 1))
    {
      tenpow = 1;
    }
    for (uint8_t c = 0; c < (leading - 1 - indi); c++)
    {
      tenpow *= 10;
    }
    if (aval < tenpow)
    {
      SERIAL_PORT.print("0");
    }
    else
    {
      break;
    }
  }
  if (val < 0)
  {
    SERIAL_PORT.print(-val, decimals);
  }
  else
  {
    SERIAL_PORT.print(val, decimals);
  }
}

#ifdef USE_SPI
void printScaledAGMT(ICM_20948_SPI *sensor)
{
#else
void printScaledAGMT(ICM_20948_I2C *sensor)
{
#endif
  SERIAL_PORT.print("Scaled. Acc (mg) [ ");
  printFormattedFloat(sensor->accX(), 5, 2);
  SERIAL_PORT.print(", ");
  printFormattedFloat(sensor->accY(), 5, 2);
  SERIAL_PORT.print(", ");
  printFormattedFloat(sensor->accZ(), 5, 2);
  SERIAL_PORT.print(" ], Gyr (DPS) [ ");
  printFormattedFloat(sensor->gyrX(), 5, 2);
  SERIAL_PORT.print(", ");
  printFormattedFloat(sensor->gyrY(), 5, 2);
  SERIAL_PORT.print(", ");
  printFormattedFloat(sensor->gyrZ(), 5, 2);
  SERIAL_PORT.print(" ], Mag (uT) [ ");
  printFormattedFloat(sensor->magX(), 5, 2);
  SERIAL_PORT.print(", ");
  printFormattedFloat(sensor->magY(), 5, 2);
  SERIAL_PORT.print(", ");
  printFormattedFloat(sensor->magZ(), 5, 2);
  SERIAL_PORT.print(" ], Tmp (C) [ ");
  printFormattedFloat(sensor->temp(), 5, 2);
  SERIAL_PORT.print(" ]");
  SERIAL_PORT.println();
}