#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 = true;


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);


}


void loop() {
 
  updateChannels();








  motorA.setThrust(0,thro);
  
  //motorB.setThrust(1,thro);

  servo1.write(rud);
  servo2.write(ele);
  servo3.write(ail);

  //servo4.write(rud);
  //servo5.write(aux1);


  //Do fancy stuff

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

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

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

/*

fBattV =  analogRead(A3);
//Serial.print("BattV: ");  

fBattV = (129*fBattV/33800);

Serial.println(fBattV);

*/
}

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
  }
}

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;
}