#include <MicroPlaneSystem.h>
MicroPlaneSystem rx;


MicroPlaneSystem servo1;
MicroPlaneSystem servo2;
MicroPlaneSystem servo3;
MicroPlaneSystem servo4;
MicroPlaneSystem servo5;


//MicroPlaneSystem motorA;
//MicroPlaneSystem motorB;


int rud;
int ele;
int thro;
int ail;
int aux1;
int aux2;

int iswitch = 0;

float fBattV;

unsigned long timeFirst;

bool debugging = false;

void setup() {
  Serial.begin(115200);
  Serial.println("-----------");
  Serial.println("Hello World");

  servo1.attach(7);
  servo2.attach(8);
  servo3.attach(9);
  servo4.attach(10);
  servo5.attach(11);

  
  // motorA.setMotorPin(6, A1, A0);
  //motorB.setMotorPin(3, 5, 13);


 //Motor A
  pinMode(6, OUTPUT);
  digitalWrite(6, LOW);

//Motor B
  pinMode(5, OUTPUT);
  digitalWrite(5, LOW);


  delay(4000);

}


void loop() {

  updateChannels();


  servo1.write(ail);
  //servo2.write(ele);
  //servo3.write(ail);
  //servo4.write(rud);
  //servo5.write(aux1);
  //motorA.setThrust(0,thro);
  //motorB.setThrust(1,thro);


//2050
 /* if (thro > 1500) {
    digitalWrite(6, HIGH);
    Serial.println("An");
  } else {
  */
    //digitalWrite(6, LOW);
    //Serial.println("Aus");
    int value = map(thro, 800, 2102, 0, 255);

    analogWrite(6, value);
 // }




  if (aux1 >= 1500) {
    digitalWrite(A2, HIGH);
  } else {
    digitalWrite(A2, LOW);
  }

  if (aux2 >= 1500) {
    posLight();
    //digitalWrite(4, HIGH);

  } else {
    //digitalWrite(4, LOW);
  }

}

void channelMapping() {
  ail = rx.getRxChannel(0);
  rud = rx.getRxChannel(1);
  ele = rx.getRxChannel(2);
  thro = rx.getRxChannel(3);
  aux1 = rx.getRxChannel(4);
  aux2 = rx.getRxChannel(5);
}

void updateChannels() {

  if (Serial.available()) {
    //Serial.println("Serial ist da");
    if (rx.update()) {
      channelMapping();
      if (debugging) {
        for (int i = 0; i < 6; i++) {
          Serial.print(" Ch0");
          Serial.print(i);
          Serial.print(": ");
          Serial.print(rx.getRxChannel(i));
        }
        Serial.println();
      }
    }
  } else {
    //No connection or Failsafe
    // Serial.println("\nNo Connection");
  }
}

void posLight() {

  //Serial.println(millis());



  switch (iswitch) {
    case 0:
      if (everyXMills(100)) {
        iswitch++;
        digitalWrite(4, HIGH);
      }
      break;
    case 1:
      if (everyXMills(50)) {
        iswitch++;
        digitalWrite(4, LOW);
      }
      break;
    case 2:
      if (everyXMills(100)) {
        iswitch++;
        digitalWrite(4, HIGH);
      }
      break;
    case 3:
      if (everyXMills(50)) {
        iswitch++;
        digitalWrite(4, LOW);
      }
      break;
    case 4:
      if (everyXMills(800)) {
        iswitch = 0;
      }
      break;
  }


}


bool everyXMills(int mills) {
  bool bStatus;
  if ((millis() - timeFirst) >= mills) {
    bStatus = true;
    timeFirst = millis();
  } else {
    bStatus = false;
  }
  return bStatus;
}