#include <MicroPlaneSystem.h>

MicroPlaneSystem rx;
MicroPlaneSystem servo1;
MicroPlaneSystem servo2;
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);

    motorB.setMotorPin(6);
}


void loop() {

    updateChannels();

    servo1.write(ail);
    servo2.write(ele);
    motorB.setThrust(thro);

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

    if (aux2 >= 1500) {
        posLight();
    }

}

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");
        motorB.setThrust(0);
    }
}

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(unsigned int mills) {
    bool bStatus;
    if ((millis() - timeFirst) >= mills) {
        bStatus = true;
        timeFirst = millis();
    } else {
        bStatus = false;
    }
    return bStatus;
}