#include <ESP32Servo.h>
#include "BluetoothSerial.h"
#include <Wire.h>
#include <Adafruit_INA219.h>
S
BluetoothSerial ESP_BT;
Adafruit_INA219 ina219;

int incoming;
bool BT_Connect = false;
bool id = false;

// horizontal servo
Servo horizontal;
int servoh = 90;
  
int servohLimitHigh = 180;
int servohLimitLow = 0;

Servo vertical;
int servov = 90;

int servovLimitHigh = 145;
int servovLimitLow = 0;


// LDR pin connections
int ldrTR = 35; // LDR top right (green)
int ldrTL = 4; // LDR top left (white)
int ldrBR = 12; // LDR bottom right (Black)
int ldrBL = 34; // LDR bottom left (Red)

int Vbatt;

unsigned long prevMillis = 0;
const long interval = 1000;

void setup() {
  Serial.begin(19200);
  while (!Serial) {
      delay(1);
  }
  if (! ina219.begin()) {
    Serial.println("Failed to find INA219 chip");
    while (1) { delay(10); }
  }
  ESP_BT.begin("Hardware");
  ESP_BT.register_callback (Bt_Status);

  // servo connections
  horizontal.attach(14);
  vertical.attach(15);
  
  // move servos
  horizontal.write(90);
  vertical.write(45);
  delay(3000);
  Serial.println("Hardware ready");
}

void Bt_Status (esp_spp_cb_event_t event, esp_spp_cb_param_t *param) {
  if (event == ESP_SPP_SRV_OPEN_EVT) {
    BT_Connect = true;
    Serial.println ("Client Connected");
  }else if (event == ESP_SPP_CLOSE_EVT ) {
    BT_Connect = false;
    Serial.println ("Client Disconnected");
  }
}

void loop() {
  unsigned long currentMillis = millis();
  if (currentMillis - prevMillis >= interval){
    prevMillis = currentMillis;
    if (BT_Connect == false || id == false){
      if(ESP_BT.available()){
        incoming = ESP_BT.read();
        if(incoming == 77){
          Serial.println("Recived identifier");
          delay(3000);
          Serial.println("Sending response");
          ESP_BT.write(77);
          id = true;
        }
      }    
    }
    if (BT_Connect == false){
      id = false;
    }  
    if (BT_Connect == true && id == true){
      float current_mA = 0;
      float loadvoltage = 0;
      float shuntvoltage = 0;
      float busvoltage = 0;
      float capacity = 0;
      float VbattVal = 0;

      shuntvoltage = ina219.getShuntVoltage_mV();
      current_mA = ina219.getCurrent_mA();
      busvoltage = ina219.getBusVoltage_V();
      loadvoltage = busvoltage + (shuntvoltage / 1000);
      VbattVal = analogRead(Vbatt);
      capacity = (((VbattVal-1.946)*100)/1.323);
      if (capacity<0){
        capacity = 0;
      }
      if (capacity>100){
        capacity = 100;
      }
      capacity = round(capacity);   

      /*Serial.print("Load Voltage:  "); Serial.print(loadvoltage); Serial.println(" V");
      Serial.print("Current:       "); Serial.print(current_mA); Serial.println(" mA");
      Serial.print("Capacity:      "); Serial.print(capacity); Serial.println(" %");*/
      ESP_BT.print(loadvoltage);
      ESP_BT.print(";");
      ESP_BT.print(current_mA);
      ESP_BT.print(";");
      ESP_BT.print(capacity);
      ESP_BT.println(";");
    }
  }

  int tr = analogRead(ldrTR); // top right
  int tl = analogRead(ldrTL); // top left
  int br = analogRead(ldrBR); // bottom right
  int bl = analogRead(ldrBL); // bottom left

  int dtime = 100; // change for debugging only
  int tol = 30;

  int avt = (tl + tr) / 2; // average value top
  int avd = (bl + br) / 2; // average value bottom
  int avl = (tl + bl) / 2; // average value left
  int avr = (tr + br) / 2; // average value right

  int dvert = avt - avd;  // check the difference of up and down
  int dhoriz = avl - avr; // check the difference of left and right


  // send data to the serial monitor if desired
  Serial.print(tl);
  Serial.print(" ");
  Serial.print(tr);
  Serial.print(" ");
  Serial.print(bl);
  Serial.print(" ");
  Serial.print(br);
  Serial.print("  ");
  Serial.print(avt);
  Serial.print(" ");
  Serial.print(avd);
  Serial.print(" ");
  Serial.print(avl);
  Serial.print(" ");
  Serial.print(avr);
  Serial.print("  ");
  Serial.print(dtime);
  Serial.print("   ");
  Serial.print(tol);
  Serial.print("  ");
  Serial.print(servov);
  Serial.print("   ");
  Serial.print(servoh);
  Serial.print(" ");
  Serial.print(dhoriz);
  Serial.print(" ");
  Serial.println(dvert);
  

  // check if the difference is in the tolerance else change vertical angle
  if (-1 * tol > dvert || dvert > tol) {
    if (avt > avd) {
      servov = ++servov;
      if (servov > servovLimitHigh) {
        servov = servovLimitHigh;
      }
    }
    else if (avt < avd) {
      servov = --servov;
      if (servov < servovLimitLow) {
        servov = servovLimitLow;
      }
    }
    vertical.write(servov);
  }

  // check if the difference is in the tolerance else change horizontal angle
  if (-1 * tol > dhoriz || dhoriz > tol) {
    if (avl > avr) {
      servoh = --servoh;
      if (servoh < servohLimitLow) {
        servoh = servohLimitLow;
      }
    }
    else if (avl < avr) {
      servoh = ++servoh;
      if (servoh > servohLimitHigh) {
        servoh = servohLimitHigh;
      }
    }
    else if (avl = avr) {
      // nothing
    }
    horizontal.write(servoh);
  }
  
  delay(dtime);
  
}