// Controlling multiple servo motors usiing arrays
#include <IRremote.h>
#include <Servo.h>

int IRPIN = 2;       // Ir receive

int relay1Pin = 9;   // Trottle of aircraft (engin on off)
int relay2Pin = 10;  // Position lights (red and green leds) on/off
int relay3Pin = 11;  // Landing lights (white leds) on/off
int relay4Pin = 12;  // Strobe light (red led)  pulsing

bool strobeActive = false;  // have we pressed the button to start the Strobe 
bool throttleActive = false;  // have we pressed the button to start/stop the Engine
bool positionLightsActive = false; // have we pressed the button to turn on/off the Position lights
bool landingLightsActive = false; // have we pressed the button to turn on/off the Landing lights

unsigned long previousMillis;
const unsigned long interval = 10;   // 10 or 20 milliseconds are common values

Servo servo[5]; //array of servos
const byte servoPins[] = {8,7,6,5,4}; // array of the servo pins
const int servoStartDegrees[] = {2,2,2,2,2};
int servoDegrees[] = {2,2,2,2,2}; // array to control the number of degrees each servo moves at a time
int servoStartPositions[] = { 90, 90, 90, 90, 90 };            // Array to hold the positions of the servos
int servoPositions[] = {90,90,90,90,90}; // Array to hold the positions of the servos during movement, starting from 0
const int servoMinDegrees[] = {0,0,0,0,0};  // the limits to servo movement
const int servoMaxDegrees[] = {180,180,180,180,180};
unsigned long previousServoMillis[] = {0,0,0,0,0}; // the time when the servo was last moved
int servoIntervals[] = {80,80,80,80,80}; // millisecs between servo moves
unsigned long currentMillis = 0;  // stores the value of millis() in each iteration of loop()

bool servo1Active = false;
bool servo2Active = false;
bool servo3Active = false;
bool servo4Active = false;
bool servo5Active = false;


void setup() 
{
  Serial.begin(115200);
  
  Serial.println("Enabling IRin");
  IrReceiver.begin(IRPIN, ENABLE_LED_FEEDBACK);
  Serial.println("Enabled IRin");

  for(int n = 0; n < 5; n++)
  {
    servo[n].attach(servoPins[n]);
  }

  pinMode(relay1Pin, OUTPUT);  //Trottle of aircraft (engin on off)
  pinMode(relay2Pin, OUTPUT);  // Position lights (red and green leds)
  pinMode(relay3Pin, OUTPUT);  // Landing lights (white leds)
  pinMode(relay4Pin, OUTPUT);  // Strobe light (red led)

  Serial.print("throttleActive = ");
  Serial.print("false Relay pin is ");
  Serial.println(digitalRead(relay1Pin));

}

void loop() 
{
  currentMillis = millis();
  
  // Serial.print("Relay pin is ");
  // Serial.println(digitalRead(relay1Pin));

  if (IrReceiver.decode()) {

    Serial.println("--- Value received ---");
    Serial.println(IrReceiver.decodedIRData.decodedRawData, HEX);
    Serial.println(IrReceiver.decodedIRData.decodedRawData);
    Serial.println("----------------------");

    switch (IrReceiver.decodedIRData.decodedRawData) {
      case 0x9768FF00:  //Keypad button "0" Throttle
        Serial.println("----- Start -------");
        Serial.println("Keypad button 0 pressed");
        Serial.print("Relay pin is ");
        Serial.println(digitalRead(relay1Pin));
        if (digitalRead(relay1Pin) == HIGH) {
          digitalWrite(relay1Pin, LOW);
        } else {
          digitalWrite(relay1Pin, HIGH);
        };

        /*
        if (throttleActive) { // Engine is switched ON so we will turn it OFF
          Serial.println("STOP motor");
          Serial.print("throttleActive = true Relay pin is ");
          Serial.println(digitalRead(relay1Pin));
          Serial.println("Setting throttleActive = false and changing Relay pin HIGH");
          throttleActive = false;
          digitalWrite(relay1Pin, HIGH);  //Trottle (Engin off)
          Serial.print("throttleActive = false Relay pin is ");
          Serial.println(digitalRead(relay1Pin));
          Serial.println("----- Finish -------");
        } else {
          Serial.println("START motor");
          Serial.print("throttleActive = false Relay pin is ");
          Serial.println(digitalRead(relay1Pin));
          Serial.println("Setting throttleActive = true and changing Relay pin LOW");
          digitalWrite(relay1Pin, LOW);  // Trottle (Engin on)
          throttleActive = true;
          Serial.print("throttleActive = true Relay pin is ");
          Serial.println(digitalRead(relay1Pin));
          Serial.println("----- Finish -------");
        };
        */

        break;

      case 0xCF30FF00:  //Keypad button "1" Position Lights
        Serial.println("Keypad button 1 pressed");
        if (digitalRead(relay2Pin) == HIGH) { // Position Lights switched ON so we will turn it OFF
          digitalWrite(relay2Pin, LOW);  //Position lights (red and green leds off)
        } else {
          digitalWrite(relay2Pin, HIGH);  //Position lights (red and green leds on)
        };

        // digitalWrite(relay2Pin, HIGH);
        // if (positionLightsActive) { // Position Lights switched ON so we will turn it OFF
        //   positionLightsActive = false;
        //   digitalWrite(relay2Pin, HIGH);  //Position lights (red and green leds off)
        // } else {
        //   digitalWrite(relay2Pin, LOW);  //Position lights (red and green leds on)
        //   positionLightsActive = true;
        // };
        Serial.print("Relay pin is ");
        Serial.println(digitalRead(relay2Pin));
        
        break;

      case 0xE718FF00:  //Keypad button "2" Landing Lights
        Serial.println("Keypad button 2 pressed");
        if (digitalRead(relay3Pin) == HIGH) { // Landing Lights switched ON so we will turn it OFF
          digitalWrite(relay3Pin, LOW);  //Landing lights (white leds off)
        } else {
          digitalWrite(relay3Pin, HIGH);  //Landing lights (white leds on)
        };
        
        Serial.print("Relay pin is ");
        Serial.println(digitalRead(relay3Pin));

        break;

      case 0x857AFF00:  //Keypad button "3" Strobe Lights
        Serial.println("Keypad button 3 pressed");
        // Here we only control wheter it should flash or not, the actual flashing is controlled by calling updateStrobe_State() at the end of the loop after the different case button checks
        if (digitalRead(relay4Pin) == HIGH) { // Strobe LED is flashing so we will turn it OFF
          digitalWrite(relay4Pin, LOW);  //Strobe light (red led)off
          // previousStrobe_Millis = 0;  // reset the previousStrobe_Millis so it will start from 0 again next time we activate it
        } else {
          digitalWrite(relay4Pin, HIGH);  //Strobe light (red led)on
        };
        
        Serial.print("Relay pin is ");
        Serial.println(digitalRead(relay4Pin));

        break;

      case 0xEF10FF00:  //Keypad button "4" Allerons Left
        Serial.println("Keypad button 4 pressed");
        if (servo1Active) {
          resetServo(0);
          servo1Active = false;
        } else {
          servo1Active = true;
        };
        
        break;

      case 0xC738FF00:  //Keypad button "5" Allerons Right
        Serial.println("Keypad button 5 pressed");
        if (servo2Active) {
          resetServo(1);
          servo2Active = false;
        } else {
          servo2Active = true;
        };

        // servo1.write(0), delay(1000), servo1.write(180), delay(1000);  // Allerons right go up and down
        break;

      case 0xA55AFF00:  //Keypad button "6" Ellevators
        Serial.println("Keypad button 6 pressed");
        if (servo3Active) {
          resetServo(2);
          servo3Active = false;
        } else {
          servo3Active = true;
        };
        // servo2.write(0), delay(1000), servo2.write(180), delay(1000);  // Ellevators go up and down
        break;

      case 0xBD42FF00:  //Keypad button "7" Rudder
        Serial.println("Keypad button 7 pressed");
        if (servo4Active) {
          resetServo(3);
          servo4Active = false;
        } else {
          servo4Active = true;
        };
        // servo3.write(0), delay(1000), servo3.write(180), delay(1000);  // Rudder Servo pin(0 to 180 degrees) left/right
        break;

      case 0xB54AFF00:  //Keypad button "8" ?? Rudder
        Serial.println("Keypad button 8 pressed");
        // servo4.write(0), delay(1000), servo4.write(180), delay(1000);  // Rudder Servo pin(0 to 180 degrees) left/right
        break;

      case 0xAD52FF00:  //Keypad button "9" Landing Gear
        Serial.println("Keypad button 9 pressed");
        if (servo5Active) {
          resetServo(4);
          servo5Active = false;
        } else {
          servo5Active = true;
        };
        // servo5.write(0), delay(1000), servo5.write(180), delay(1000);  // Landing Servo pin(0 to 180 degrees)gear in out
        break;
    }

    IrReceiver.resume();
  }

  checkMoveServo();

  // for(int n = 0; n < 5; n++)
  // {
  //   // Serial.print(F("Moving servo "));
  //   // Serial.println(n);
  //   // Serial.print(F("From "));
  //   // Serial.println(servoPositions[n]);
  //   moveServo(n);

  //   //delay(2000);
    
  // }
}

void checkMoveServo() {

  if (servo1Active) {
    moveServo(0);
  }

  if (servo2Active) {
    moveServo(1);
  }

  if (servo3Active) {
    moveServo(2);
  }

  if (servo4Active) {
    moveServo(3);
  }

  if (servo5Active) {
    moveServo(4);
  }


}

void moveServo(int servoNo) {
  // Serial.println(F("moveServo Called "));
  if (currentMillis - previousServoMillis[servoNo] >= servoIntervals[servoNo]) {
    // its time for another move
    previousServoMillis[servoNo] += servoIntervals[servoNo];

    servoPositions[servoNo] = servoPositions[servoNo] + servoDegrees[servoNo];  // servoDegrees might be negative

    /* Removing interval speedup/ slow down for now
    if (servoPositions[servoNo] <= servoMinDegrees[servoNo]) {
      // when the servo gets to its minimum position change the interval to change the speed
      if (servo1Interval == servoSlowInterval) {
        servo1Interval = servoFastInterval;
      } else {
        servo1Interval = servoSlowInterval;
      }
    }
    */

    if ((servoPositions[servoNo] >= servoMaxDegrees[servoNo]) || (servoPositions[servoNo] <= servoMinDegrees[servoNo])) {
      // if the servo is at either extreme change the sign of the degrees to make it move the other way
      servoDegrees[servoNo] = -servoDegrees[servoNo];  // reverse direction
                                     // and update the position to ensure it is within range
      servoPositions[servoNo] = servoPositions[servoNo] + servoDegrees[servoNo];
    }

    // make the servo move to the next position
    servo[servoNo].write(servoPositions[servoNo]);
    Serial.println("----------------");
    Serial.print("servo");
    Serial.print(servoNo + 1);
    Serial.print("Position = ");
    Serial.println(servoPositions[servoNo]);
    Serial.println("----------------");
    // and record the time when the move happened
  }

}

void resetServo(int servoNo) {

    Serial.print("Stopping Servo");
    Serial.println(servoNo + 1);
    Serial.print("Position = ");
    Serial.println(servoPositions[servoNo]);

    servo[servoNo].write(servoStartPositions[servoNo]);
    previousServoMillis[servoNo] = 0;
    servoPositions[servoNo] = servoStartPositions[servoNo];
    servoDegrees[servoNo] = servoStartDegrees[servoNo];
    
  }