#include <SPI.h>
#include "ADS1X15.h"
#include <TinyGPS++.h>
#include <SoftwareSerial.h>
#include <Arduino_FreeRTOS.h>


// Variables for storing GPS and sensor data
float lat, lng, speed, voltage, current;

// ADS1115 object for analog-to-digital conversion
ADS1115 ADS(0x48);

// SoftwareSerial object for GPS communication
SoftwareSerial ss(4, 3);

// TinyGPSPlus object for parsing GPS data
TinyGPSPlus gps;

// Task function prototypes
void Read_GPS( void *pvParameters );
void Read_Voltage_Current( void *pvParameters );
void Send_Data( void *pvParameters );
void Blink_LED( void *pvParameters );


void setup() {
  Serial.begin(115200);
  Serial.print("Starting...");
  ss.begin(9600);
  Wire.begin();
  ADS.begin();
  SPI.begin();
  SPI.setClockDivider(SPI_CLOCK_DIV2);

  // Create tasks for different functionalities
  xTaskCreate(Read_GPS, "Read_GPS", 128, NULL, 1, NULL);
  xTaskCreate(Read_Voltage_Current, "Read_Voltage_Current", 128, NULL, 1, NULL);
  xTaskCreate(Send_Data, "Send_Data", 256, NULL, 1, NULL);
  //xTaskCreate(Blink_LED, "Blink_LED", 64, NULL, 1, NULL);
}

void loop() {
  // The loop is empty as tasks handle the main functionality
}

// Task to blink LEDs
void Blink_LED(void *pvParameters) {
  (void) pvParameters;
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(7, OUTPUT);
  pinMode(8, OUTPUT);
  for (;;) {

    // Blink LEDs in a pattern
    digitalWrite(5, LOW);
    digitalWrite(6, LOW);
    digitalWrite(7, LOW);
    digitalWrite(8, LOW);
    vTaskDelay(5000 / portTICK_PERIOD_MS);
    digitalWrite(5, HIGH);
    digitalWrite(6, HIGH);
    digitalWrite(7, HIGH);
    digitalWrite(8, HIGH);
    vTaskDelay(100 / portTICK_PERIOD_MS);
  }
}

// Task to read voltage and current data
void Read_Voltage_Current(void *pvParameters) {
  (void) pvParameters;
  for (;;) {
    // Read sensor data
    int16_t V_val = ADS.readADC(0);
    int16_t C_val = ADS.readADC(1);
    float f = ADS.toVoltage(2);
    voltage = V_val * f;
    current = C_val * f;
    Serial.println(voltage, 6);
    vTaskDelay(1000 / portTICK_PERIOD_MS);
  }
}

// Task to read GPS data
void Read_GPS(void *pvParameters) {
  (void) pvParameters;
  while (1) {
    // Read GPS data
    

    // while (ss.available() > 0) {
    //   gps.encode(ss.read());
    //   if (gps.location.isUpdated()) {
    //     lat = gps.location.lat();
    //     lng = gps.location.lng();
    //     Serial.println(lat, 6);
    //   }
    // }
  }
}

// Task to send data
void Send_Data(void *pvParameters) {
  (void) pvParameters;
  while (1) {
    // Send data
    float data[5] = { lat, lng, speed, voltage, current };
    digitalWrite(SS, LOW);
    for (int i = 0; i < 5; i++) {
      byte *dataByte = (byte *)&data[i];
      for (int j = 0; j < sizeof(float); j++) {
        SPI.transfer(dataByte[j]);
      }
    }
    for (int i = 0; i < 5; i++) {
      Serial.println(data[i]);
    }
    digitalWrite(SS, HIGH);
    vTaskDelay(5 / portTICK_PERIOD_MS);
  }
}