#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
//#include <MPU6050.h>
#include <MPU6050_light.h>

#define ACTION_BUTTON 13
#define MENU_BUTTON 27
#define CAL_BUTTON 14
//#define BLUETOOTH_BUTTON 14
#define AD0_IN 19
#define MPU_INT 18

#define VEL_PIN 12
#define VERT_PIN 35
#define HORZ_PIN 34
#define SEL_PIN 33

#define LED_RED 17
#define LED_GREEN 16  
#define LED_BLUE 4

const int mpuAddr = 0x68; // I2C address
const int mpuAddr_not_used = 0x69; // not even used in the code.

//Adafruit_MPU6050 mpu;
MPU6050 mpu = MPU6050(Wire);

/*MPU6050 mpu[NUM_MPUS] = 
{
  MPU6050(Wire),
  MPU6050(Wire),
  MPU6050(Wire),
  MPU6050(Wire),
  MPU6050(Wire),
};/**/

bool action, start, bluetooth_on;

unsigned long previousMillis;
const unsigned long interval = 1000;

unsigned long button_time;
unsigned long last_button_time;
uint16_t debounce_delay = 500;

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

  Wire.begin();

  pinMode(ACTION_BUTTON, INPUT_PULLUP);
  //pinMode(BLUETOOTH_BUTTON, INPUT_PULLUP);

  pinMode(AD0_IN, OUTPUT);
  pinMode(MPU_INT, INPUT);
  
  pinMode(VERT_PIN, INPUT);
  pinMode(HORZ_PIN, INPUT);
  pinMode(SEL_PIN, INPUT_PULLUP);

  pinMode(LED_RED, OUTPUT);
  pinMode(LED_GREEN, OUTPUT);
  pinMode(LED_BLUE, OUTPUT);

  digitalWrite(AD0_IN, HIGH);

  digitalWrite(LED_RED, LOW);
  digitalWrite(LED_GREEN, LOW);
  digitalWrite(LED_BLUE, LOW);

  attachInterrupt(ACTION_BUTTON, debounce, FALLING);
  //attachInterrupt(BLUETOOTH_BUTTON, bluetooth_enable, FALLING);

  action = false;
  start = false;
  bluetooth_on = false;

  // Initialize all the MPU-6050 sensors.
  // Required for irl application, NOT required in simulation.
  Serial.println(F("Initializing the sensors and calculating offsets. Do not move MPU-6050 sensors."));
  Serial.println(F("Attention: Calculating the offset is good in real life, but it is better to skip this function in Wokwi."));
  delay(500);

  digitalWrite(AD0_IN, LOW); 

  byte status = mpu.begin();

  Serial.print(F("Initializing MPU-6050 "));
  Serial.print(F(", error = "));
  Serial.print(status);

  if(status == 0) Serial.print(F(" (no error)"));
  Serial.println();

  mpu.calcOffsets(true,true); // gyro and accelometer

  digitalWrite(AD0_IN, HIGH);
  Serial.println("Initialization done");
  /**/

  // A message to click on a sensor to change
  // the settings when running in Wokwi.
  Serial.println("Click on a MPU-6050 module and change the acceleration and gyro.");

  // The setup() took so long,
  // set previousMillis to a fresh value.
  previousMillis = millis();

  delay(100);
}

void loop() {
  // put your main code here, to run repeatedly:
  //delay(10); // this speeds up the simulation

  int vel = map(analogRead(VEL_PIN), 0, 4095, 0, 90);
  int vert = map(analogRead(VERT_PIN), 0, 4095, -100, 100);
  int horz = map(analogRead(HORZ_PIN), 0, 4095, -100, 100);
  bool selPressed = digitalRead(SEL_PIN) == LOW;

  Serial.print("v: ");
  Serial.print(vel);
  Serial.print(", x: ");
  Serial.print(horz);
  Serial.print(", y: ");
  Serial.print(vert);
  Serial.print(", SEL: ");
  Serial.println(selPressed + "");

  //previousMillis = currentMillis;
  if (start) {

    digitalWrite(AD0_IN, LOW);
    mpu.update();

    Serial.print("MPU: (");
      Serial.print(F("T = "));
      Serial.print(mpu.getTemp(),0);
      Serial.println(")");
      Serial.print(F("ACC: "));
      Serial.print(mpu.getAccX());
      Serial.print(", ");
      Serial.print(mpu.getAccY());
      Serial.print(", ");
      Serial.println(mpu.getAccZ());

      Serial.print(F("GYRO: "));
      Serial.print(mpu.getGyroX(),0);
      Serial.print(", ");
      Serial.print(mpu.getGyroY(),0);
      Serial.print(", ");
      Serial.println(mpu.getGyroZ(),0);

      Serial.print(F("ACC ANGLE: "));
      Serial.print(mpu.getAccAngleX(),0);
      Serial.print(", ");
      Serial.print(mpu.getAccAngleY(),0);
      Serial.println(", ");
      //Serial.println(mpu.getAccAngleZ(),0);
      
      Serial.print(F("ANGLE: "));
      Serial.print(mpu.getAngleX(),0);
      Serial.print(", ");
      Serial.print(mpu.getAngleY(),0);
      Serial.print(", ");
      Serial.print(mpu.getAngleZ(),0);
      Serial.println();

      Serial.print("INT: ");
      Serial.println(analogRead(MPU_INT));

      Serial.println(F("======================================================================================\n"));
      
      delay(interval);

      digitalWrite(AD0_IN, HIGH);
  }
  else {
    delay(1000);
  }

  /*if (start) {
    Serial.println("started");
    unsigned long currentMillis = millis();

    digitalWrite(AD0_IN, HIGH);
    mpu.update();

    if (currentMillis - previousMillis >= interval) {
      Serial.println("read");
      /*previousMillis = currentMillis;

      Serial.print("MPU: (");
      Serial.print(F("T = "));
      Serial.print(mpu.getTemp(),0);
      Serial.println(")");
      Serial.print(F("ACC: "));
      Serial.print(mpu.getAccX());
      Serial.print(", ");
      Serial.print(mpu.getAccY());
      Serial.print(", ");
      Serial.println(mpu.getAccZ());

      Serial.print(F("GYRO: "));
      Serial.print(mpu.getGyroX(),0);
      Serial.print(", ");
      Serial.print(mpu.getGyroY(),0);
      Serial.print(", ");
      Serial.println(mpu.getGyroZ(),0);

      Serial.print(F("ACC ANGLE: "));
      Serial.print(mpu.getAccAngleX(),0);
      Serial.print(", ");
      Serial.print(mpu.getAccAngleY(),0);
      Serial.println(", ");
      //Serial.println(mpu.getAccAngleZ(),0);
      
      Serial.print(F("ANGLE: "));
      Serial.print(mpu.getAngleX(),0);
      Serial.print(", ");
      Serial.print(mpu.getAngleY(),0);
      Serial.print(", ");
      Serial.print(mpu.getAngleZ(),0);
      Serial.println();

      Serial.println(F("======================================================================================\n"));
      /*
    }
  }
  else {

  }/**/
}

void debounce() {
  button_time = millis();
  if (button_time - last_button_time >= debounce_delay) {
    //action = true;
    start = !start;
    if (start) {
      Serial.println("start");
      digitalWrite(LED_GREEN, HIGH);
    }
    else {
      Serial.println("stop");
      digitalWrite(LED_GREEN, LOW);
    }
    last_button_time = button_time;
  }/**/

  //Serial.println("Hello, ESP32!");
}

/*void bluetooth_enable() {
  button_time = millis();
  if (button_time - last_button_time >= debounce_delay) {
    //action = true;
    start = !start;
    if (start) Serial.println("start");
    else Serial.println("stop");
    last_button_time = button_time;
  }

  //Serial.println("Hello, ESP32!");
}/**/

void detect_defect() {

}

void print_output() {
      Serial.print("MPU: (");
      Serial.print(F("T = "));
      Serial.print(mpu.getTemp(),0);
      Serial.println(")");
      Serial.print(F("ACC: "));
      Serial.print(mpu.getAccX());
      Serial.print(", ");
      Serial.print(mpu.getAccY());
      Serial.print(", ");
      Serial.println(mpu.getAccZ());

      Serial.print(F("GYRO: "));
      Serial.print(mpu.getGyroX(),0);
      Serial.print(", ");
      Serial.print(mpu.getGyroY(),0);
      Serial.print(", ");
      Serial.println(mpu.getGyroZ(),0);

      Serial.print(F("ACC ANGLE: "));
      Serial.print(mpu.getAccAngleX(),0);
      Serial.print(", ");
      Serial.print(mpu.getAccAngleY(),0);
      Serial.println(", ");
      //Serial.println(mpu.getAccAngleZ(),0);
      
      Serial.print(F("ANGLE: "));
      Serial.print(mpu.getAngleX(),0);
      Serial.print(", ");
      Serial.print(mpu.getAngleY(),0);
      Serial.print(", ");
      Serial.print(mpu.getAngleZ(),0);
      Serial.println();

      Serial.print("INT: ");
      Serial.println(analogRead(MPU_INT));

      Serial.println(F("======================================================================================\n"));

}