// Facing Problems
// Too much data is flashing too fast not able to see anything
// 1. Can now make it like this video and code: 
// https://www.youtube.com/watch?v=MeWgnt0YLj8&ab_channel=Robojax
// http://robojax.com/learn/arduino/?vid=robojax_analog_push_buttons1
// 2. https://www.youtube.com/watch?v=QvQAl-zbiCI&ab_channel=BPLearning


//#include <MPU6050_tockn.h>

// Display Management Switch (DM)
#define DM 1 // the number of the pushbutton pin
int DMstate = 0; // variable for reading the pushbutton status
int DMcount =0;
int DMprestate =0;

// Trim-Up Button (TUB)
#define TUB 2
int TUBstate = 0;
int TUBcount = 0;
int TUBprestate = 0;

// Trim-Down Button (TDB)
#define TDB 3
int TDBstate = 0;
int TDBcount = 0;
int TDBprestate = 0;
float Trim = 0;

// Weapon Management Switch (W)
#define Wbutton 4
int Wstate = 1;

// Priority Switch (P)
#define Pbutton 5
int Pstate = 1;

/*
// Gyro (G)
MPU6050 mpu6050(Wire);
long timer = 0;

// Force (F)
int fsrPin = 0; // the FSR and 10K pulldown are connected to a0
int fsrReading; // the analog reading from the FSR resistor divider
int fsrVoltage; // the analog reading converted to voltage
unsigned long fsrResistance; // The voltage converted to resistance, can be very big so make "long"
unsigned long fsrConductance; 
long fsrForce; // Finally, the resistance converted to force
*/

void setup() 
{
  Serial.begin(9600);
  
  // Display Management Switch (DM)
  pinMode(DM, INPUT_PULLUP);  
  Serial.println("BlueKei Solutions"); 

  // Trim Up Button
  pinMode(TUB, INPUT_PULLUP);
  
  //Trim Down Button
  pinMode(TDB, INPUT_PULLUP);

  // Weapon Management Switch (W)
  pinMode(Wbutton, INPUT_PULLUP);

  // Priority Switch (P)
  pinMode(Pbutton, INPUT_PULLUP);

  // Gyro (G)
  //Wire.begin();
  //mpu6050.begin();
  //mpu6050.calcGyroOffsets(true);
}


void loop() 
{
// Display Management Switch (DM)
  DMstate = digitalRead(DM);
  delay(60);
  if (DMstate == HIGH && DMprestate == 0) 
  {
    DMcount++;
    DMprestate = 1;
  }
  else if(DMstate == LOW) 
  {
    DMprestate = 0;
  }
  switch(DMcount)
  {
    case 1:
    Serial.println("Display Mode: Primary Flight Display");
    break;
    case 2:
    Serial.println("Display Mode: Navigation Mode");
    break;
    case 3:
    Serial.println("Display Mode: Combat Mode");
    break;
 }
  if(DMcount > 3){DMcount = 1;}

// Trim Up Down
  TUBstate = digitalRead(TUB);
  delay(60);

  if (TUBstate == HIGH & TUBprestate == LOW)
  { 
    TUBcount++;  
    if (Trim < 5.1){
    Trim = Trim + 0.25;}
    TUBprestate = HIGH;
  }
  else if(TUBstate == LOW) 
  {
    TUBprestate = LOW;
  }

// Trim Down Button 
  TDBstate = digitalRead(TDB);
  delay(60);
  
  if (TDBstate == HIGH & TDBprestate == 0)
  {
    TDBcount++;
    if(Trim > -5){
    Trim = Trim - 0.25;}
    TDBprestate = 1;
  }
  else if(TDBstate == LOW)
  {
    TDBprestate = 0;
  }
  Serial.println(Trim);
  
// Weapons Management Switch
  Wstate = digitalRead(Wbutton);
  if(Wstate == HIGH)
  {
    Serial.println("Weapon Status: Ready to Fire");
    
  } 
  else if(Wstate == LOW)
  {
    Serial.println("Weapon Status: Shots Fired");
  }

// Priority Swtich
Pstate = digitalRead(Pbutton);
  if (Pstate == HIGH)
  {
    Serial.println("NOT IN PRIORITY"); // text to display
  } 
  else if(Pstate == LOW)
  {
    Serial.println("IN PRIORITY");
  }

/*
// Gyro Module
mpu6050.update();
  if(millis() - timer > 1000)
  {
    Serial.println("=======================================================");
    Serial.print("gyroX : ");Serial.print(mpu6050.getGyroX());
    Serial.print("\tgyroY : ");Serial.print(mpu6050.getGyroY());
    Serial.print("\tgyroZ : ");Serial.println(mpu6050.getGyroZ());
    timer = millis();
  }

// Force
fsrReading = analogRead(fsrPin);
  // analog voltage reading ranges from about 0 to 1023 which maps to 0V to 5V (= 5000mV)
  fsrVoltage = map(fsrReading, 0, 1023, 0, 5000);
  Serial.print("Voltage reading in mV = ");
  Serial.println(fsrVoltage);  

  if (fsrVoltage == 0) 
  {
    Serial.println("No pressure");
  } 
  else 
  {
    // The voltage = Vcc * R / (R + FSR) where R = 10K and Vcc = 5V
    // so FSR = ((Vcc - V) * R) / V        yay math!
    fsrResistance = 5000 - fsrVoltage;     // fsrVoltage is in millivolts so 5V = 5000mV
    fsrResistance *= 10000;                // 10K resistor
    fsrResistance /= fsrVoltage;
    Serial.print("FSR resistance in ohms = ");
    Serial.println(fsrResistance);
 
    fsrConductance = 1000000;           // we measure in micromhos so 
    fsrConductance /= fsrResistance;
    Serial.print("Conductance in microMhos: ");
    Serial.println(fsrConductance);
 
    // Use the two FSR guide graphs to approximate the force
    if (fsrConductance <= 1000) {
      fsrForce = fsrConductance / 80;
      Serial.print("Force in Newtons: ");
      Serial.println(fsrForce);      
    } else {
      fsrForce = fsrConductance - 1000;
      fsrForce /= 30;
      Serial.print("Force in Newtons: ");
      Serial.println(fsrForce);            
    }
  }
  Serial.println("--------------------");
  delay(1000);
  */
}