/*/   Mobile Robotic Arm Teleoperation Controller   /*/
// Code by Michael Fernandez, Nov 2024

#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <ESP8266WiFi.h>
#include <espnow.h>

// REPLACE WITH RECEIVER MAC Address
uint8_t broadcastAddress[] = {0xCC, 0x50, 0xE3, 0xF4, 0x64, 0xC2};
// ESP NOW data transfer
typedef struct struct_message {
  uint16_t joyXdata;
  uint16_t joyYdata;
  bool gyroCtrlData;
  float gyroZdata;
  float gyroYdata;
  bool clawBtn1Data;
  bool clawBtn2Data;
} struct_message;
struct_message transferData;

//IMU sensor
Adafruit_MPU6050 mpu;
unsigned long previousTime = 0;
const unsigned int sensitivityFactor = 100;

// Servo
float accumulatedZAngle = 90;
float accumulatedYAngle = 90;
uint8_t clawServoAngle = 90;

// Claw button
const uint8_t clawBtn1 = 0; // D3
const uint8_t clawBtn2 = 2; // D4

// Joystick
const uint8_t joystickBtn = 14; // D5
const uint8_t joyYreadCtrl = 12; // D6
const uint8_t joyXreadCtrl = 13; // D7
bool gyroCtrl = false;

// Battery check
bool battCheck = false;
const uint8_t battCtrl = 16; // D0
const float voltageDividerFactor = 2.0;
const float maxADCValue = 1024.0;
const float referenceVoltage = 3.2;

const uint8_t analogInput = A0;
const uint8_t warnLed = 15; // D8


// For debugging only!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
void OnDataSent(uint8_t *mac_addr, uint8_t sendStatus) {
  Serial.print("Last Packet Send Status: ");
  if (sendStatus == 0){
    Serial.println("Delivery success");
  }
  else{
    Serial.println("Delivery fail");
  }
}

// Gyro & claw button logic
void sendGryoValue() {
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  // Calculate the time difference
  unsigned long currentTime = millis();
  float deltaTime = (currentTime - previousTime) / 1000.0;
  previousTime = currentTime;

  float accelXAngle = a.acceleration.x;

  float ZgyroRate = g.gyro.z * sensitivityFactor;
  float ZangleChange = ZgyroRate * deltaTime;
  
  float YgyroRate = g.gyro.y * sensitivityFactor;
  float YangleChange = YgyroRate * deltaTime;

  accumulatedYAngle += YangleChange;
  accumulatedYAngle = constrain(accumulatedYAngle, 0, 180);
  
  Serial.print("yaw: "); //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  Serial.println(accumulatedYAngle);

  transferData.gyroYdata = accumulatedYAngle; // used for yaw servo

  if (accelXAngle <= 7 && accelXAngle >= -4) {
    accumulatedZAngle += ZangleChange;
    accumulatedZAngle = constrain(accumulatedZAngle, 0, 180);

    Serial.print("pitch: ");  //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
    Serial.println(accumulatedZAngle);

    transferData.gyroZdata = accumulatedZAngle; // used for pitch servo
  }
  
    Serial.print("accel y: ");  //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
    Serial.println(accelXAngle);
    Serial.print("gyro y: ");  //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
    Serial.println(YgyroRate);
    Serial.print("gyro z: ");  //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
    Serial.println(ZgyroRate);
}

// Battery read logic
void checkBattery() {
  digitalWrite(battCtrl, HIGH);
  int battRawValue = analogRead(analogInput);
  digitalWrite(battCtrl, LOW);

  float Vout = (battRawValue / maxADCValue) * referenceVoltage;  // Calculate Vout
  float battVin = Vout * voltageDividerFactor;  // Calculate Vin (battery voltage)

  Serial.print("raw batt value= "); //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  Serial.println(battRawValue);
  Serial.print("battery volt= ");
  Serial.println(battVin);

  if (battVin <= 3.3) {
    static uint8_t ledLoopCount = 0;
    static bool warnLedState = false;
    Serial.println("LOW battery"); //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

    if (ledLoopCount == 50) {
      warnLedState? digitalWrite(warnLed, LOW) : digitalWrite(warnLed, HIGH);
      warnLedState = !warnLedState;
      ledLoopCount = 0;
    }else{
      ledLoopCount++;
    }
  }else if (battVin > 3.4) {
    digitalWrite(warnLed, LOW);
    Serial.println("NORM battery"); //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!  
  }
}

// Joystick read logic
void checkJoystick() {
  digitalWrite(joyXreadCtrl, HIGH);
  unsigned int joyXvalue = analogRead(analogInput);
  digitalWrite(joyXreadCtrl, LOW);
  transferData.joyXdata = joyXvalue;

  Serial.print("joystick x = "); //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  Serial.println(joyXvalue);

  digitalWrite(joyYreadCtrl, HIGH);
  unsigned int joyYvalue = analogRead(analogInput);
  digitalWrite(joyYreadCtrl, LOW);
  transferData.joyYdata = joyYvalue;

  Serial.print("joystick y = "); //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  Serial.println(joyYvalue);
}


void setup(void) {
  Serial.begin(115200);
  Wire.begin();

  // Pin setup
  pinMode(warnLed, OUTPUT);
  pinMode(joystickBtn, INPUT_PULLUP);
  pinMode(joyXreadCtrl, OUTPUT);
  pinMode(joyYreadCtrl, OUTPUT);
  pinMode(clawBtn1, INPUT_PULLUP);
  pinMode(clawBtn2, INPUT_PULLUP);
  pinMode(battCtrl, OUTPUT);

  // ESP NOW setup
  WiFi.mode(WIFI_STA);
  if (esp_now_init() != 0) {
    Serial.println("Error initializing ESP-NOW");
    return;
  }
  esp_now_set_self_role(ESP_NOW_ROLE_CONTROLLER);
  esp_now_register_send_cb(OnDataSent);
  esp_now_add_peer(broadcastAddress, ESP_NOW_ROLE_SLAVE, 1, NULL, 0);
  

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

  // Configure the accelerometer and gyroscope
  mpu.setAccelerometerRange(MPU6050_RANGE_16_G); // You can change to 2G, 4G, or 16G
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);      // Set to 500 degrees/sec range
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

  // Record the initial time
  previousTime = millis();
}

void loop() {
  bool clawBtn1State = digitalRead(clawBtn1) == LOW;
  bool clawBtn2State = digitalRead(clawBtn2) == LOW;
  transferData.clawBtn1Data = clawBtn1State;
  transferData.clawBtn2Data = clawBtn2State;

  bool currentJoyState = digitalRead(joystickBtn) == LOW;

  Serial.println(""); //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  Serial.print("joystick button= ");
  Serial.println(currentJoyState);
  Serial.print("claw button 1= ");
  Serial.println(clawBtn1State); 
  Serial.print("claw button 2= ");
  Serial.println(clawBtn2State);

  checkJoystick();

  static bool lastJoyBtnState = false;
  if (currentJoyState && !lastJoyBtnState){
    gyroCtrl? gyroCtrl = false : gyroCtrl = true;
    gyroCtrl? digitalWrite(warnLed, HIGH) : digitalWrite(warnLed, LOW);
    transferData.gyroCtrlData = gyroCtrl;
  }
  lastJoyBtnState = currentJoyState;

  Serial.print("gyro control = "); //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  Serial.println(gyroCtrl);
  
  if (gyroCtrl) {
    sendGryoValue();
  }else {
    checkBattery();

    static uint8_t resetLoopCount = 0;
    switch (resetLoopCount) {
      case 0:
        if (clawBtn1State == HIGH && clawBtn2State == HIGH) {
          resetLoopCount++;
        }
        break;
      default:
        if (clawBtn1State == HIGH && clawBtn2State == HIGH && resetLoopCount < 100) {
          resetLoopCount++;
        }else if (clawBtn1State == LOW && clawBtn2State == LOW) {
          resetLoopCount = 0;
        }else {
          accumulatedZAngle = 90;
          accumulatedYAngle = 90;
        }
        break;
    }
  }

  esp_now_send(broadcastAddress, (uint8_t *) &transferData, sizeof(transferData));
  delay(10);
}