#include "WiFi.h"
#include "ESPAsyncWebServer.h"
#include "SPIFFS.h"
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <PubSubClient.h>
#include "mymodel.h"
#include <math.h>


#define N_SAMPLES 15
#define N_AXES 3
#define ACCEL_THRESHOLD 10.0
#define INTERVAL 30



Adafruit_MPU6050 mpu;

float features[N_SAMPLES * N_AXES];
int flag = 0;


Eloquent::ML::Port::RandomForest model; //init model

const char* ssid = "S23";
const char* password = "04051995";

const char* mqtt_server = "192.168.1.202 ";
int mqtt_port = 1883;

const uint8_t R_FORW = 27;
const uint8_t R_BACK = 26;
const uint8_t L_FORW = 32;
const uint8_t L_BACK = 13;


int r_enc = 19;
int l_enc = 18;
int r_interruptCounter = 0;
int l_interruptCounter = 0;


WiFiClient espClient;
PubSubClient client(espClient);
long lastMsg = 0;

// Create AsyncWebServer object on port 80
AsyncWebServer server(80);


void setup() {

  Serial.begin(115200);


  // Connect to WiFi
  WiFi.begin(ssid, password);
  Serial.print("Connecting.");
  while (WiFi.status() != WL_CONNECTED) {
    Serial.print(".");
    delay(500);
  }
  Serial.print("\nWiFi connected - IP address: ");
  Serial.println(WiFi.localIP());
  delay(500);

  client.setServer(mqtt_server, 1883);

  // Initialize SPIFFS
  if (!SPIFFS.begin(true)) {
    Serial.println("An Error has occurred while mounting SPIFFS");
    return;
  }

  pinMode(L_FORW, OUTPUT);
  pinMode(L_BACK, OUTPUT);
  pinMode(R_FORW, OUTPUT);
  pinMode(R_BACK, OUTPUT);
  pinMode(r_enc, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(r_enc), r_handleInterrupt, RISING);
  pinMode(l_enc, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(l_enc), l_handleInterrupt, RISING);

  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }
  Serial.println("MPU6050 found!");

  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_5_HZ);

  Serial.println("");
  delay(100);

  // Route for root / web page
  server.on("/", HTTP_GET, [](AsyncWebServerRequest * request) {
    request->send(SPIFFS, "/as.html");
  });

  // Route to load style.css file
  server.on("/styles.css", HTTP_GET, [](AsyncWebServerRequest * request) {
    request->send(SPIFFS, "/styles.css", "text/css");
  });

  // Route to set CAR to FORWARD
  server.on("/forward", HTTP_GET, [](AsyncWebServerRequest * request) {
    digitalWrite(L_FORW, HIGH);
    digitalWrite(L_BACK, LOW);
    digitalWrite(R_FORW, HIGH);
    digitalWrite(R_BACK, LOW);
    request->send(SPIFFS, "/as.html");
  });

  // Route to set CAR to BACKWARD
  server.on("/backward", HTTP_GET, [](AsyncWebServerRequest * request) {
    digitalWrite(L_FORW, LOW);
    digitalWrite(L_BACK, HIGH);
    digitalWrite(R_FORW, LOW);
    digitalWrite(R_BACK, HIGH);
    request->send(SPIFFS, "/as.html");
  });

  // Route to set CAR to LEFT
  server.on("/left", HTTP_GET, [](AsyncWebServerRequest * request) {
    digitalWrite(L_FORW, LOW);
    digitalWrite(L_BACK, LOW);
    digitalWrite(R_FORW, HIGH);
    digitalWrite(R_BACK, LOW);
    request->send(SPIFFS, "/as.html");
  });

  // Route to set CAR to RIGHT
  server.on("/right", HTTP_GET, [](AsyncWebServerRequest * request) {
    digitalWrite(L_FORW, HIGH);
    digitalWrite(L_BACK, LOW);
    digitalWrite(R_FORW, LOW);
    digitalWrite(R_BACK, LOW);
    request->send(SPIFFS, "/as.html");
  });

  // Route to set CAR to STOP
  server.on("/stop", HTTP_GET, [](AsyncWebServerRequest * request) {
    digitalWrite(L_FORW, LOW);
    digitalWrite(L_BACK, LOW);
    digitalWrite(R_FORW, LOW);
    digitalWrite(R_BACK, LOW);
    request->send(SPIFFS, "/as.html");
  });

  // Start server
  server.begin();

}


void reconnect() {
  // Loop until we're reconnected
  while (!client.connected()) {
    Serial.print("Attempting MQTT connection...");
    // Attempt to connect
    if (client.connect("ESP32 Client")) {
      Serial.println("connected");
    } else {
      Serial.print("failed, rc=");
      Serial.print(client.state());
      Serial.println(" try again in 5 seconds");
      // Wait 5 seconds before retrying
      delay(5000);
    }
  }
}

// function to read data from imu
void mpu_readings(float *ax, float *ay, float *az) {
  float _ax, _ay, _az;
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);
  *ax = a.acceleration.x;
  *ay = a.acceleration.y;
  *az = a.acceleration.z;

}


void loop() {

  if (!client.connected()) {
    reconnect();
  }
  client.loop();

  float ax, ay, az;

// IMU readings which we did the training of the ml model
  float front[N_SAMPLES * N_AXES] = { 0.72, -0.56, 9.16, 1.40, -0.27, 9.06, 1.46, -0.44, 9.02, 1.23, -0.49, 8.99, 0.89, -0.06, 9.14, 0.47, 0.00, 9.00, 0.29, 0.03, 9.13, -0.25, 1.01, 8.95, -0.17, 0.26, 8.99, 0.24, -0.06, 8.97, 0.11, -0.23, 9.01, -0.05, -0.15, 9.18, -0.24, -0.55, 9.03, -0.91, -0.32, 8.97, -2.21, -0.28, 9.02};
  float back[N_SAMPLES * N_AXES] = {1.39, 0.35, 8.92, -2.12, 0.98, 9.05, -2.06, 0.94, 8.75, -1.60, 0.46, 9.02, -1.34, -0.08, 8.97, -0.81, 0.15, 8.86, -0.79, -0.01, 9.02, -0.62, -0.05, 8.91, -0.36, 0.16, 9.00, -0.45, -0.01, 8.96, -0.34, 0.20, 8.84, -0.54, -0.46, 9.05, -0.29, -0.06, 8.97, 0.23, 0.14, 9.11, 1.53, -0.24, 9.18};
  float left[N_SAMPLES * N_AXES] = {0.20, -0.82, 9.07, 0.80, -0.34, 9.05, 0.68, -1.01, 9.10, 0.37, -1.26, 9.18, 0.19, -1.49, 8.94, 0.35, -1.53, 9.03, 0.86, -1.23, 9.68, 0.30, 0.46, 9.01, 0.27, 0.78, 9.05, 0.46, 0.91, 9.18, 0.57, 0.06, 9.33, 0.57, -0.04, 9.02, 0.30, -0.30, 8.94, 0.40, -0.19, 8.72, 0.45, 0.05, 9.18};
  float right[N_SAMPLES * N_AXES] = {0.26, 0.83, 9.07, 0.61, 1.01, 9.14, 0.29, 0.31, 9.03, 0.15, 0.14, 9.05, -0.01, 1.41, 9.01, -0.04, 0.90, 9.07, 0.02, 0.57, 9.05, 0.11, 0.50, 8.92, 0.21, 0.46, 9.01, 0.26, 0.58, 9.01, 0.50, 0.62, 9.17, 0.50, 0.53, 8.99, 0.55, 0.56, 9.05, 0.62, 0.46, 8.99, 0.59, 0.10, 9.06};

  if (flag == 0) {
    Serial.println("Ready, waiting for move");
  }
  mpu_readings(&ax, &ay, &az);

   if (millis() > lastMsg + 250) {
    lastMsg = millis();

   client.publish("esp32/Speed", String(speedometer()).c_str());
  }

  if (!motionDetected(ax, ay, az)) {
    flag = 1;
    delay(10);
    return;
  }
  flag = 0;

  recordMPU();
  //printFeatures();


  float front_similarity = cosineSimilarity(front, features,N_SAMPLES * N_AXES);
  float back_similarity = cosineSimilarity(back, features,N_SAMPLES * N_AXES);
  float left_similarity = cosineSimilarity(left, features,N_SAMPLES * N_AXES);
  float right_similarity = cosineSimilarity(right, features,N_SAMPLES * N_AXES);

 if ((front_similarity > 0.8) || (back_similarity > 0.8) || (left_similarity > 0.8) || (right_similarity > 0.8)) {

    Serial.print("Front similarity: ");
    Serial.println(front_similarity);
    Serial.print("Back similarity: ");
    Serial.println(back_similarity);
    Serial.print("Left similarity: ");
    Serial.println(left_similarity);
    Serial.print("Right similarity: ");
    Serial.println(right_similarity);

    classify();
  }
  else {
    Serial.println("The readings doesn't match with the current classes!");
    Serial.print("Front similarity: ");
    Serial.println(front_similarity);
    Serial.print("Back similarity: ");
    Serial.println(back_similarity);
    Serial.print("Left similarity: ");
    Serial.println(left_similarity);
    Serial.print("Right similarity: ");
    Serial.println(right_similarity);
 


  Serial.print("Left Pulse: ");
  Serial.println(l_interruptCounter);
  Serial.print("Right Pulse: ");
  Serial.println(r_interruptCounter);
  
//MQTT publish
  if (millis() > lastMsg + 250) {
    lastMsg = millis();

    char payload[100];
    snprintf(payload, sizeof(payload), "{\"XAcceleration\": %.2f, \"YAcceleration\": %.2f, \"ZAcceleration\": %.2f}", ax, ay, az);

    client.publish("esp32/Acceleration", payload);
    client.publish("esp32/Speed", String(speedometer()).c_str());
  }
  delay(2000);

}

//Optical encoder pulse counter
void r_handleInterrupt() {
  r_interruptCounter++;
}

void l_handleInterrupt() {
  l_interruptCounter++;
}


bool motionDetected(float ax, float ay, float az) {
  return (abs(ax) + abs(ay) + abs(az)) > ACCEL_THRESHOLD;
}



// Feature extraction
void recordMPU() {
  float ax, ay, az;

  for (int i = 0; i < N_SAMPLES; i++) {
    mpu_readings(&ax, &ay, &az);


    features[i * N_AXES + 0] = ax;
    features[i * N_AXES + 1] = ay;
    features[i * N_AXES + 2] = az;

    delay(INTERVAL);
  }
}

/**
   Dump the feature vector to Serial monitor
*/
void printFeatures() {
  const uint16_t numFeatures = sizeof(features) / sizeof(float);

  for (int i = 0; i < numFeatures; i++) {
    Serial.print(features[i]);
    Serial.print(i == numFeatures - 1 ? '\n' : ',');
  }
}

// Classification
void classify() {
  Serial.print("Predicted Movement: ");
  Serial.println(model.predictLabel(features));
}

// Cosine Similarity
float cosineSimilarity(float vectA[], float vectB[], int vectSize) {
  float dotProduct = 0;
  float magnitudeA = 0;
  float magnitudeB = 0;

  for (int i = 0; i < vectSize; i++) {
    dotProduct += vectA[i] * vectB[i];
    magnitudeA += pow(vectA[i],2);
    magnitudeB += pow(vectA[i],2);
  }
  magnitudeA = sqrt(magnitudeA);
  magnitudeB = sqrt(magnitudeB);

  if (magnitudeA == 0 || magnitudeB == 0) {
    return 0;
  }

  return dotProduct / (magnitudeA * magnitudeB);
}

// Calculates the speed of vehicle. 
float speedometer(){
  unsigned long previousMillis = 0;
  const long interval = 1000;
  float distancePerPulse;
  const float wheelRadius = 0.063;
  float wheelCircumference = 2 * 3.1416 * wheelRadius;
  float pulsesPerRevolution = 20;
  float metersPerSecond1 = 0;
  float metersPerSecond2 = 0;
  float averageMetersPerSecond = 0;

  unsigned long currentMillis = millis();

  if(currentMillis - previousMillis >= interval){
    previousMillis = currentMillis;

    distancePerPulse = wheelCircumference / pulsesPerRevolution;
    float distanceTraveled1 = r_interruptCounter * distancePerPulse;
    float distanceTraveled2 = l_interruptCounter * distancePerPulse;

    metersPerSecond1 = distanceTraveled1 / (interval / 1000);
    metersPerSecond2 = distanceTraveled2 / (interval / 1000);

    
    if(r_interruptCounter > 0 || l_interruptCounter > 0)
    averageMetersPerSecond = (metersPerSecond1 + metersPerSecond2) / 2;
    else
    averageMetersPerSecond = 0;

    r_interruptCounter =0;
    l_interruptCounter =0;
    }
    return averageMetersPerSecond;
  }
