//---------------------------------------------------------------
// sketch.ino file
//---------------------------------------------------------------

#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <WiFi.h>
#include <ArduinoJson.h>
#include <PubSubClient.h>
#include <WiFiClientSecure.h>
#include <HardwareSerial.h>
#include <TinyGPSPlus.h>
#include <string.h>
#include "config_task3.h"
#include <stdint.h>

#define AWS_IOT_PUB_TOPIC "esp32/pub"
#define AWS_IOT_SUB_TOPIC "esp32/sub"

#define TX_gps 10    // GPS - uart1
#define RX_gps 9    
#define TX_temp 17   // Temp - uart2
#define RX_temp 16

// Timer variables
unsigned long MPUDelay = 1000;  //milliseconds (200 readings in one second)
unsigned long gpsDelay = 1000;  // One reading per second
unsigned long tempDelay = 5000; // One reading in 5 seconds

unsigned long lastTimeMPU = 0;  
unsigned long lastTimegps = 0;  
unsigned long lastTimeTemp = 0; 

//Variables to store sensor data
//float temperature = 0.0;
String mpu6050_data = "";

// MPU sensor object
Adafruit_MPU6050 mpu;

// Sensor events
sensors_event_t a, g, t;

float gyro_x, gyro_y, gyro_z;
float acc_x, acc_y, acc_z;
double mpu_vals_array;

String gps_input;
int temp;
double lat_val, long_val, alt_val;
double gps_vals_array[5][3] = {0.0};

TinyGPSPlus gps;

WiFiClientSecure net = WiFiClientSecure();
PubSubClient client(net);

HardwareSerial gps_serial(1);
HardwareSerial temp_serial(2);

// Json
StaticJsonDocument<1200> doc;
JsonObject root = doc.to<JsonObject>();
JsonArray mpu_data_collection = root["mpu_data_collection"].to<JsonArray>();

// JsonArray mpu_data_collection = doc.createNestedArray("mpu_data_collection");
// JsonArray mpu_data = mpu_data_collection.createNestedArray();
// JsonArray mpu_data_collection = doc.to<JsonArray>();


void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);

  gps_serial.begin(115200, SERIAL_8N1, RX_gps, TX_gps);
  temp_serial.begin(115200, SERIAL_8N1, RX_temp, TX_temp);

  connectToWifi();
  connectAWS();
  init_mpu();
}

int cnt = 1;

void loop() {
  // put your main code here, to run repeatedly:

  if ((millis() - lastTimeMPU) >= MPUDelay)
  {
    get_mpu_val();
    lastTimeMPU = millis();

    // JsonArray mpu_data_inner = mpu_data.createNestedArray();
    JsonArray mpu_data_inner = mpu_data_collection.createNestedArray();
    mpu_data_inner.add(cnt++);
    mpu_data_inner.add(gyro_x);
    mpu_data_inner.add(gyro_y);
    mpu_data_inner.add(gyro_z);
    mpu_data_inner.add(acc_x);
    mpu_data_inner.add(acc_y);
    mpu_data_inner.add(acc_z);

    // mpu_data_collection.add(mpu_data);
    // mpu_data.clear();

    // root["temp"] = cnt++;

    // char json_buffer[1000];
    // serializeJson(doc, json_buffer);
    // serializeJson(doc,Serial);
    // Serial.println("");


  }

  if ((millis() - lastTimegps) >= gpsDelay)
  {
    get_gps_val();
    lastTimegps = millis();
  }

    if ((millis() - lastTimeTemp ) >= tempDelay)
  {
    get_temp_val_frm_arduino();
    lastTimeTemp = millis();
    publishMessage();
  }

  client.loop();
}

//---------------------------------------------------------------------------
//---------------------------------------------------------------------------
//---------------------------------------------------------------------------

void connectToWifi()
{
//  WiFi.mode(WIFI_STA);
  WiFi.begin("Wokwi-GUEST", "", 6);
  Serial.println("");
  Serial.print("Connecting to WiFi...");

  while (WiFi.status() != WL_CONNECTED)
  {
    Serial.print(".");
    delay(1000);
  }

  Serial.println("Connected");
  Serial.println(WiFi.localIP());
}

void connectAWS()
{
  // Configure WiFiClientSecure to use the AWS IoT device credentials
  net.setCACert(AWS_CERT_CA);
  net.setCertificate(AWS_CERT_CRT);
  net.setPrivateKey(AWS_CERT_PRIVATE);

  // int MQTT_PORT = 8883;    // not connecting if i use this

  // Connect to MQTT broker on AWS endpt
  client.setServer(AWS_IOT_ENDPOINT, 8883);

  // Create a message handler
  client.setCallback(messageHandler_callback);

  Serial.println("Connecting to AWS IOT");

  while (!client.connect(THINGNAME))
  {
    Serial.print(".");
    // delay(100);
  }

  if (!client.connected())
  {
    Serial.println("AWS IOT Timeout");
    return;
  }

  // Subscribe to a topic
  client.subscribe(AWS_IOT_SUB_TOPIC);

  Serial.println("AWS IOT Connected!");
}
 
// Init MPU6050
void init_mpu()
{
  if (!mpu.begin())
  {
    Serial.println("Failed to find MPU6050 chip");
    while (1)
    {
      delay(10);
    }
  }
  Serial.println("MPU6050 Found!");
}

void get_mpu_val()
{
  mpu.getEvent(&a, &g, &t);

  gyro_x = g.gyro.x;
  gyro_y = g.gyro.y;
  gyro_z = g.gyro.z; 

  acc_x = a.acceleration.x;
  acc_y = a.acceleration.y;
  acc_z = a.acceleration.z;
}

void get_gps_val()
{
  // gps_input = Serial1.readString();

  while (gps_serial.available() > 0)
  {
    gps.encode(gps_serial.read());

    if (gps.location.isUpdated())
    {
      lat_val = gps.location.lat();
      long_val = gps.location.lng();
    }

    if (gps.altitude.isUpdated())
    {
      alt_val = gps.altitude.meters();
    }
  }
  Serial.print("Lat val: ");
  Serial.println(lat_val);
}

void get_temp_val_frm_arduino()
{
  temp = temp_serial.read();
  // temp = atoi(temp);
}

void messageHandler_callback(char* topic, byte* payload, unsigned int length)
{
  Serial.print("incoming: ");
  Serial.println(topic);

  StaticJsonDocument<200> doc;
  deserializeJson(doc,payload);
  const char* message = doc["message"];
  Serial.println(message);
}

void publishMessage()
{
  

  // MPU data


  // JsonArray mpu_data = doc.createNestedArray("mpu_data");
  // mpu_data.add(gyro_x);
  // mpu_data.add(gyro_y);
  // mpu_data.add(gyro_z);
  // mpu_data.add(acc_x);
  // mpu_data.add(acc_y);
  // mpu_data.add(acc_z);

  // doc["MPU"] = {lat_val, long_val, alt_val};
  // doc["GPS"] = {gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z};
    root["Temp"] = 30;


    // const uint8_t* json_buffer;

    char json_buffer[1000];
    serializeJson(doc, json_buffer);
    // serializeJson(doc,Serial);
    // Serial.println("");

  client.publish(AWS_IOT_PUB_TOPIC, json_buffer);
  // client.publish(AWS_IOT_PUB_TOPIC, json_buffer, (sizeof(json_buffer)/sizeof(uint8_t)), false);

  // Clearing json arrays
  doc.clear();
  mpu_data_collection.clear();
  mpu_data_collection = root["mpu_data_collection"].to<JsonArray>();
  // cnt = 1;
}