#include <Wire.h>

/*======================*/
const int numOfChannels = 5;
/*======================*/
float RollPID_Gain[] = {0, 0, 0}, PitchPID_Gain[] = {0, 0, 0}, YawPID_Gain[] = {0, 0, 0};
/*======================*/

#define GYRO_SCALE_RANGE
#define ACCEL_SCALE_RANGE

bool state1, state2, state3, state4;
int16_t gyro[3], accel[3], temperature;
int reciverChannels[numOfChannels], ESCout[4], PotVal;
float reciverChanData[5], gyroAngle[3], AccelAngle[2], RPY_Angle[3], RPY_Setpoint[3], RPY_Error[3], RPY_PID[3];
float RollPID[3], PitchPID[3], YawPID[3];
float gyroXYZ_Error[3], accelXYZ_Error[3];
unsigned long time1, currentTime, previousTime, elapsedTime;

/*Add Gyro and Accel Variables*/

void setup() {
  Serial.begin(115200);
  Serial.println("Hello, ESP32!");
  REG_SET_BIT(GPIO_ENABLE_REG, BIT2); REG_SET_BIT(GPIO_ENABLE_REG, BIT4);
  REG_SET_BIT(GPIO_ENABLE_REG, BIT5); REG_SET_BIT(GPIO_ENABLE_REG, BIT18);
}

void loop() {
  PotVal = map(analogRead(33), 0, 4095, 1000, 1800);
  RPY_PID[0]= map(analogRead(35), 0, 4095, -100, 100);
  RPY_PID[1]= map(analogRead(32), 0, 4095, -100, 100);
  RPY_PID[2]= map(analogRead(26), 0, 4095, -100, 100);

  REG_WRITE(GPIO_OUT_W1TS_REG, BIT2); REG_WRITE(GPIO_OUT_W1TS_REG, BIT4);
  REG_WRITE(GPIO_OUT_W1TS_REG, BIT5); REG_WRITE(GPIO_OUT_W1TS_REG, BIT18);
  currentTime = micros();

  //Serial.print(PotVal+" ");
  //Serial.print(joyYVal+" ");
  //Serial.println(joyXVal);

  ESCout[0] = PotVal - RPY_PID[0] - RPY_PID[1] - RPY_PID[2]; //Front Left
  ESCout[1] = PotVal + RPY_PID[0] - RPY_PID[1] + RPY_PID[2]; //Front Right
  ESCout[2] = PotVal + RPY_PID[0] + RPY_PID[1] - RPY_PID[2]; //Rear Right
  ESCout[3] = PotVal - RPY_PID[0] + RPY_PID[1] + RPY_PID[2]; //Rear Left
  //Serial.println(ESCout[0]);
  while((micros()-currentTime) < 1000){
    state1 = true; state2 = true; state3 = true; state4 = true;
    while(state1 || state2 || state3 || state4){
      time1 = micros();
      if((time1 - currentTime) >= ESCout[0] && state1){
        REG_WRITE(GPIO_OUT_W1TC_REG, BIT2);
        state1 = false;
      }
      if((time1 - currentTime) >= ESCout[1] && state2){
        REG_WRITE(GPIO_OUT_W1TC_REG, BIT4);
        state2 = false;
      }
      if((time1 - currentTime) >= ESCout[2] && state3){
        REG_WRITE(GPIO_OUT_W1TC_REG, BIT5);
        state3 = false;
      }
      if((time1 - currentTime) >= ESCout[3] && state4){
        REG_WRITE(GPIO_OUT_W1TC_REG, BIT18);
        state4 = false;
      }
    }
  }
  delay(10);
}

void setGyroAccelRegisters(){
 //Start the MPU-6050
  Wire.beginTransmission(0x68);                                        //Start communicating with the MPU-6050
  Wire.write(0x6B);                                                    //Send the requested starting register
  Wire.write(0x00);                                                    //Set the requested starting register
  Wire.endTransmission();                                              //End the transmission
  //Configure the accelerometer (+/-8g) divide by 4096
  Wire.beginTransmission(0x68);                                        //Start communicating with the MPU-6050
  Wire.write(0x1C);                                                    //Send the requested starting register
  Wire.write(0x10);                                                    //Set the requested starting register
  Wire.endTransmission();                                              //End the transmission
  //Configure the gyro (500dps full scale) divide by 65.5
  Wire.beginTransmission(0x68);                                        //Start communicating with the MPU-6050
  Wire.write(0x1B);                                                    //Send the requested starting register
  Wire.write(0x08);                                                    //Set the requested starting register
  Wire.endTransmission();
}

void readGyro(){
  Wire.beginTransmission(0x68);
  Wire.write(0x43);
  Wire.endTransmission();
  Wire.requestFrom(0x68,6);
  gyro[0] = Wire.read()<<8|Wire.read();
  gyro[1] = Wire.read()<<8|Wire.read();
  gyro[2] = Wire.read()<<8|Wire.read();

  gyro[0] -= gyroXYZ_Error[0];
  gyro[1] -= gyroXYZ_Error[1];
  gyro[2] -= gyroXYZ_Error[2];
}

void readAccel(){
  Wire.beginTransmission(0x68);
  Wire.write(0x3B);
  Wire.endTransmission();
  Wire.requestFrom(0x68,6);
  accel[0] = Wire.read()<<8|Wire.read();
  accel[1] = Wire.read()<<8|Wire.read();
  accel[2] = Wire.read()<<8|Wire.read();

  accel[0] -= accelXYZ_Error[0];
  accel[1] -= accelXYZ_Error[1];
  accel[2] -= accelXYZ_Error[2];
}

void gyroCalibration(){

}

/*void accelCalibration(){

}*/

void readReciver(){

}