#include <Servo.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <I2Cdev.h>
#include "MPU6050.h"
#define OUTPUT_READABLE_ACCELGYRO
#define LED_PIN 13
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro;
//MPU6050 accelgyro(0x69); // <-- use for AD0 high

int16_t ax, ay, az;
int16_t gx, gy, gz;
Servo Elevator;
Servo VStab;
Servo LWing;
Servo RWing;
LiquidCrystal_I2C lcd(0x27, 20, 4);
int pinsOut[] = {10, 9, 8, 7};
boolean dev_scr_on = false;
boolean main_menu_on = true;
boolean sel_menu = false;
boolean js_input = true;
bool blinkState = false;

void setup() {
  // put your setup code here, to run once:
  Elevator.attach(10);
  VStab.attach(9);
  LWing.attach(8);
  RWing.attach(7);
  pinMode(pinsOut, OUTPUT);
  pinMode(A0, INPUT);
  pinMode(A1, INPUT);
  pinMode(A2, INPUT);
  pinMode(A3, INPUT);
  pinMode(A4, INPUT);
  pinMode(A5, INPUT);
   if (!i2CAddrTest(0x27)) { 
    lcd = LiquidCrystal_I2C(0x3F, 16, 2); 
  } 
  lcd.init();                // initialize the lcd 
  lcd.backlight();           // Turn on backlight 
  lcd.print("Welp... I did it!");// Print a message to the LCD 
  // join I2C bus (I2Cdev library doesn't do this automatically)
    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
        Fastwire::setup(400, true);
    #endif

    // initialize serial communication
    // (38400 chosen because it works as well at 8MHz as it does at 16MHz, but
    // it's really up to you depending on your project)
    Serial.begin(38400);

    // initialize device
    Serial.println("Initializing I2C devices...");
    accelgyro.initialize();

    // verify connection
    Serial.println("Testing device connections...");
    Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");

    //  use the code below to change accel/gyro offset values
    /*
    Serial.println("Updating internal sensor offsets...");
    -76	-2359	1688	0	0	0
    Serial.print(accelgyro.getXAccelOffset()); Serial.print("\t"); // -76
    Serial.print(accelgyro.getYAccelOffset()); Serial.print("\t"); // -2359
    Serial.print(accelgyro.getZAccelOffset()); Serial.print("\t"); // 1688
    Serial.print(accelgyro.getXGyroOffset()); Serial.print("\t"); // 0
    Serial.print(accelgyro.getYGyroOffset()); Serial.print("\t"); // 0
    Serial.print(accelgyro.getZGyroOffset()); Serial.print("\t"); // 0
    Serial.print("\n");
    accelgyro.setXGyroOffset(220);
    accelgyro.setYGyroOffset(76);
    accelgyro.setZGyroOffset(-85);
    Serial.print(accelgyro.getXAccelOffset()); Serial.print("\t"); // -76
    Serial.print(accelgyro.getYAccelOffset()); Serial.print("\t"); // -2359
    Serial.print(accelgyro.getZAccelOffset()); Serial.print("\t"); // 1688
    Serial.print(accelgyro.getXGyroOffset()); Serial.print("\t"); // 0
    Serial.print(accelgyro.getYGyroOffset()); Serial.print("\t"); // 0
    Serial.print(accelgyro.getZGyroOffset()); Serial.print("\t"); // 0
    Serial.print("\n");
    */

    // configure Arduino LED pin for output
    pinMode(LED_PIN, OUTPUT);
}

void loop() {
  // put your main code here, to run repeatedly:
  int RHorz = analogRead(A0);
  int RVert = analogRead(A1);
  int LHorz = analogRead(A2);
  int LVert = analogRead(A3);
  int LCD_tct = analogRead(A5);
  int MapRH = map(RHorz, 0, 1023, 0, 180);
  int MapRV = map(RVert, 0, 1023, 0, 180);
  int MapLH = map(LHorz, 0, 1023, 0, 180);
  int MapLV = map(LVert, 0, 1023, 0, 180);
  int Rev = 180 - MapLH;
  
  if(LCD_tct == 0 && main_menu_on == true) {
  main_menu_on = false;
  dev_scr_on = true;
  lcd.clear();
  delay(30);
  } else if(LCD_tct == 0 && main_menu_on == false) {
  main_menu_on = true;
  dev_scr_on = false;
  lcd.clear();
  delay(30);
  }
  if(dev_scr_on == true) {
  dev_scr();
  delay(30);
  }
  if(main_menu_on == true) {
  main_menu();
  }
  
  if(js_input == true) {
  Elevator.write(MapLV);
  VStab.write(MapRH);
  LWing.write(MapLH);
  RWing.write(Rev);
  }

  // Serial.print(RHorz);
  // Serial.print(" ");
  // Serial.print(RVert);
  // Serial.print(" ");
  // Serial.print(LHorz);
  // Serial.print(" ");
  // Serial.print(LVert);
  // Serial.print(" ");
  // Serial.print(MapRH);
  // Serial.print(" ");
  // Serial.print(MapRV);
  // Serial.print(" ");
  // Serial.print(MapLH);
  // Serial.print(" ");
  // Serial.print(MapLV);
  // Serial.print(" ");
  // Serial.println(LCD_tct);
  // AllTasks();
  // read raw accel/gyro measurements from device
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

    // these methods (and a few others) are also available
    //accelgyro.getAcceleration(&ax, &ay, &az);
    //accelgyro.getRotation(&gx, &gy, &gz);

    #ifdef OUTPUT_READABLE_ACCELGYRO
        // display tab-separated accel/gyro x/y/z values
        Serial.print("a/g:\t");
        Serial.print(ax); Serial.print("\t");
        Serial.print(ay); Serial.print("\t");
        Serial.print(az); Serial.print("\t");
        Serial.print(gx); Serial.print("\t");
        Serial.print(gy); Serial.print("\t");
        Serial.println(gz);
    #endif

    #ifdef OUTPUT_BINARY_ACCELGYRO
        Serial.write((uint8_t)(ax >> 8)); Serial.write((uint8_t)(ax & 0xFF));
        Serial.write((uint8_t)(ay >> 8)); Serial.write((uint8_t)(ay & 0xFF));
        Serial.write((uint8_t)(az >> 8)); Serial.write((uint8_t)(az & 0xFF));
        Serial.write((uint8_t)(gx >> 8)); Serial.write((uint8_t)(gx & 0xFF));
        Serial.write((uint8_t)(gy >> 8)); Serial.write((uint8_t)(gy & 0xFF));
        Serial.write((uint8_t)(gz >> 8)); Serial.write((uint8_t)(gz & 0xFF));
    #endif

    // blink LED to indicate activity
    blinkState = !blinkState;
    digitalWrite(LED_PIN, blinkState);
 }
 void main_menu() {
  lcd.setCursor(0,0);
    lcd.print("Speed: unavail rn:(");
    lcd.setCursor(0,1);
    lcd.print("Alt: unavail rn:(");
    lcd.setCursor(0,2);
    lcd.print("TA: unavail rn:(");

 }


void dev_scr() {
  int RHorz = analogRead(A0);
  int RVert = analogRead(A1);
  int LHorz = analogRead(A2);
  int LVert = analogRead(A3);
  int MapRH = map(RHorz, 0, 1023, 0, 180);
  int MapRV = map(RVert, 0, 1023, 0, 180);
  int MapLH = map(LHorz, 0, 1023, 0, 180);
  int MapLV = map(LVert, 0, 1023, 0, 180);
  int Rev = 180 - MapLH;
  lcd.setCursor(0, 0);
  lcd.print("Dev Screen v1.0");
  lcd.setCursor(0, 2);
  lcd.print(RHorz);
  lcd.setCursor(5, 2);
  lcd.print(RVert);
  lcd.setCursor(10, 2);
  lcd.print(LHorz);
  lcd.setCursor(15, 2);
  lcd.print(LVert);
  lcd.setCursor(0, 3);
  lcd.print(MapRH);
  lcd.setCursor(5, 3);
  lcd.print(MapRV);
  lcd.setCursor(10, 3);
  lcd.print(MapLH);
  lcd.setCursor(15, 9);
  lcd.print(MapLV);
  lcd.setCursor(0, 0);
  // v This part just resets the screen
  lcd.setCursor(0, 2);
  lcd.print("    ");
  lcd.setCursor(5, 2);
  lcd.print("    ");
  lcd.setCursor(10, 2);
  lcd.print("    ");
  lcd.setCursor(15, 2);
  lcd.print("    ");
  lcd.setCursor(0, 3);
  lcd.print("    ");
  lcd.setCursor(5, 3);
  lcd.print("    ");
  lcd.setCursor(10, 3);
  lcd.print("    ");
  lcd.setCursor(15, 9);
  lcd.print("    ");
  // lcd.clear();
}

// void AllTasks() {
//   int LCD_tct = digitalRead(6);
//   if(LCD_tct == HIGH && main_menu == false) {
//   main_menu = true;
//   dev_scr_on = false;
//   delay(30);
//   } else if(LCD_tct == HIGH && main_menu == true) {
//   main_menu = false;
//   dev_scr_on = true;
//   delay(30);
//   }
//   while(main_menu == true) {
//     lcd.setCursor(0,0);
//     lcd.print("Speed: unavail rn:(");
//     lcd.setCursor(0,1);
//     lcd.print("Alt: unavail rn:(");
//     lcd.setCursor(0,2);
//     lcd.print("TA: unavail rn:(");
//   }
//   while(dev_scr_on == true) {
//     dev_scr();
//   }
  
//   delay(10);
// }
bool i2CAddrTest(uint8_t addr) { 
  Wire.begin(); 
  Wire.beginTransmission(addr); 
  if (Wire.endTransmission() == 0) { 
    return true; 
  } 
  return false; 
}