#include <Servo.h>
#include <Wire.h>

// Initializing servo objects
Servo servo1;
Servo servo2;

// Declaring joystick anlogue inputs
const int joyX=0;
const int joyY=1;
// Servo PWM pin inputs
const int servoOnePin=3;
const int servoTwoPin=5;
// Button status checker input
const int switchPin=2;
// Constants for the accelerometer
const int MPU=0x68;
int16_t AcX,AcY,AcZ,xOff,yOff,zOff;
int32_t squaredSum;
float mA;
float pitch;
// Variables used to check the status given by the button
int nButtonVal;
int oButtonVal=1;
// Boolean that tells 
bool useAccelerometer=false;
int servoVal,tempP,tempY;

// Accelerometer calibration function
void accCali(){  
  // Beginning transmission and writing to wire
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);  
  Wire.endTransmission(false);
  Wire.requestFrom(MPU,12,true);
  // Getting initial accelerometer values to set the offsets
  xOff=Wire.read()<<8|Wire.read();    
  yOff=Wire.read()<<8|Wire.read();  
  zOff=Wire.read()<<8|Wire.read();
  zOff-=16384;
}

// Setting up servos, accelerometer, pin, and serial output
void setup() 
{
  Wire.begin();
  // Using the Wire class to initialize accelerometer data collection
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);  
  Wire.write(0);    
  Wire.endTransmission(true);
  Serial.begin(9600);
  // Accelerometer Calibration
  accCali();
  // Servo setup
  servo1.attach(servoOnePin);
  servo2.attach(servoTwoPin);
  // Pin setup
  pinMode(switchPin,INPUT);
}
  
void loop()
{
  // Reading instantaneous button input
  nButtonVal=digitalRead(switchPin);
  // If statement checks if button was clicked
  if(oButtonVal==0 && nButtonVal==1){
    // If button was clicked and the accelerometer is not being used, switch to using the accelerometer
    if(!useAccelerometer){
      useAccelerometer=true;
    }
    // Opposite of previous comment
    else{
      useAccelerometer=false;
    }
  }
  // Code for when the accelerometer is not being used (joystick control)
  if(!useAccelerometer){
    // This code reads the joystick values (x and y) and then maps it to an angle which is then written to the corresponding servo
    // Reading
    servoVal=analogRead(joyX);
    // Printing raw data
    Serial.print(servoVal);
    Serial.print(" ");
    // Mapping
    servoVal=map(servoVal, 0, 1023, 0, 180);
    tempY=servoVal-90;
    // Writing value to servo
    servo1.write(servoVal);
    // Printing angle servo is told to move
    Serial.print(servoVal);
    Serial.print(" ");
    // Reading
    servoVal=analogRead(joyY);
    // Printing raw data
    Serial.print(servoVal);
    Serial.print(" ");
    // Mapping
    servoVal=map(servoVal, 0, 1023, 0, 180);
    tempP=servoVal-90;
    // Printing angle servo is told to move
    servo2.write(servoVal);
    Serial.print(servoVal);
    Serial.print(" ");
    Serial.print(tempY);
    Serial.print(" ");
    Serial.println(tempP);
  }
  else{
    // This code finds the pitch from the accelerometer and corrects the elevator while allowing the captain to remain in control of the yaw
    // Initializing wires to collect accelerometer data
    Wire.beginTransmission(MPU);
    Wire.write(0x3B);  
    Wire.endTransmission(false);
    Wire.requestFrom(MPU,12,true);
    // Getting accelerometer data
    AcX=Wire.read()<<8|Wire.read();    
    AcY=Wire.read()<<8|Wire.read();  
    AcZ=Wire.read()<<8|Wire.read(); 
    // Acounting for offset 
    AcX-=xOff;
    AcY-=yOff;
    AcZ-=zOff;
    // Perform squaring using int32_t to avoid overflow
    squaredSum=static_cast<int32_t>(AcX)*AcX + static_cast<int32_t>(AcY) * AcY + static_cast<int32_t>(AcZ) * AcZ;
    // Convert the result to float and take the square root
    mA=sqrt(static_cast<float>(squaredSum));
    pitch=asin(static_cast<float>(AcY)/mA)*(180.0/PI);
    pitch=90+pitch;
    // Printing data
    servo2.write(pitch);
    Serial.print(AcX);
    Serial.print(" ");
    Serial.print(AcY);
    Serial.print(" ");
    Serial.print(AcZ);
    Serial.print(" ");
    Serial.print(pitch-90);
    Serial.print(" ");
    Serial.print(pitch);
    Serial.print(" ");
    // Reading Yaw value
    servoVal=analogRead(joyX);
    // Printing raw data
    Serial.print(servoVal);
    Serial.print(" ");
    // Mapping
    servoVal=map(servoVal, 0, 1023, 0, 180);
    tempY=servoVal-90;
    // Writing angle to servo
    servo1.write(servoVal);
    // Displaying angle to servo
    Serial.print(servoVal);
    Serial.print(" ");
    Serial.println(tempY);
    servo2.write(pitch);
  }
  // Resetting button value
  oButtonVal=nButtonVal;
  delay(10);  
}