#include <Servo.h>

// --- LEDs --------------------------------

#define LED_GEAR_UP_PIN 14
#define LED_RED_PIN 15
#define LED_GEAR_DOWN_PIN 16

// --- Switch ------------------------------

#define GEAR_UP_SWITCH_PIN 0
#define GEAR_DOWN_SWITCH_PIN 1
#define SWTICH_PRESSED 0

// --- Gear States -------------------------

#define GEAR_STATE_UP 0
#define GEAR_STATE_MOVING 1
#define GEAR_STATE_DOWN 2

unsigned int currentGearState;
unsigned int targetGearState;

// --- Servos ------------------------------

#define GEAR_DOORS_CLOSED 0
#define GEAR_DOORS_OPENING 1
#define GEAR_DOORS_OPENED 2
#define GEAR_DOORS_CLOSING 3

unsigned int gearDoorsState;

#define STRUT_EXTENDED 0
#define STRUT_RETRACTING 1
#define STRUT_RETRACTED 2
#define STRUT_EXTENDING 3

unsigned int strutState;

#define SERVO_MAX_POSITION 180
#define SERVO_STEP_DELAY_MS 60

#define SERVO_GEAR_DOOR_LEFT_PIN 13
#define SERVO_GEAR_DOOR_RIGHT_PIN 12

Servo servoGearDoorLeft;
Servo servoGearDoorRight;

unsigned int servoGearDoorLeftPosition;
unsigned int servoGearDoorRightPosition;

unsigned int lastLeftDoorTimestamp;
unsigned int lastRightDoorTimestamp;

// --- Setup -------------------------------

void setup() {
  pinMode(LED_GEAR_UP_PIN, OUTPUT);
  pinMode(LED_RED_PIN, OUTPUT);
  pinMode(LED_GEAR_DOWN_PIN, OUTPUT);

  pinMode(GEAR_UP_SWITCH_PIN, INPUT);
  pinMode(GEAR_DOWN_SWITCH_PIN, INPUT);

  Serial.begin(9600);
  
  currentGearState = GEAR_STATE_DOWN;
  targetGearState = GEAR_STATE_DOWN;

  gearDoorsState = GEAR_DOORS_CLOSED;
  strutState = STRUT_EXTENDED;

  servoGearDoorLeft.attach(SERVO_GEAR_DOOR_LEFT_PIN);
  servoGearDoorRight.attach(SERVO_GEAR_DOOR_RIGHT_PIN);

  servoGearDoorLeftPosition = 0;
  servoGearDoorRightPosition = 0;

  lastLeftDoorTimestamp = 0;
  lastRightDoorTimestamp = 0;

  initServoPosition();
}


void initServoPosition() {
  servoGearDoorLeft.write(servoGearDoorLeftPosition);
  delay(500);

  servoGearDoorRight.write(servoGearDoorRightPosition);
  delay(500);
}

// --- Running ------------------------------------

void loop() {
  checkSwitch();

  if(targetGearState != currentGearState) {
    currentGearState = GEAR_STATE_MOVING;
  }

  updateLed();
  updateServos();
}

void updateServos() {
  updateGearDoorServos();
}

void updateGearDoorServos() {
  updateLeftGearDoorServo();
  updateRightGearDoorServo();
  updateGearDoorState();
}

void updateGearDoorState() {
  Serial.print("left: ");
  Serial.print(servoGearDoorLeftPosition);
  Serial.print(", right: ");
  Serial.print(servoGearDoorRightPosition);
  Serial.print(", gearDoorsState: ");
  Serial.println(gearDoorsState);

  if(servoGearDoorLeftPosition == 0 && servoGearDoorRightPosition == 0) {
    gearDoorsState = GEAR_DOORS_CLOSED;
  }
  if(servoGearDoorLeftPosition == SERVO_MAX_POSITION && servoGearDoorRightPosition == SERVO_MAX_POSITION) {
    gearDoorsState = GEAR_DOORS_OPENED;
  }
}

void updateLeftGearDoorServo() {
  unsigned int now = millis();

  if(now - lastLeftDoorTimestamp > SERVO_STEP_DELAY_MS //
      && currentGearState != targetGearState
      && servoGearDoorLeftPosition <= SERVO_MAX_POSITION) {

    servoGearDoorLeft.write(servoGearDoorLeftPosition);
    servoGearDoorLeftPosition += (servoGearDoorLeftPosition < SERVO_MAX_POSITION) ? 1 : SERVO_MAX_POSITION;

    lastLeftDoorTimestamp = now;
  }
}

void updateRightGearDoorServo() {
  unsigned int now = millis();

  if(now - lastRightDoorTimestamp > SERVO_STEP_DELAY_MS //
      && currentGearState != targetGearState
      && servoGearDoorRightPosition <= SERVO_MAX_POSITION //
      && servoGearDoorLeftPosition > 35) {

    servoGearDoorRight.write(servoGearDoorRightPosition);
    servoGearDoorRightPosition += (servoGearDoorRightPosition < SERVO_MAX_POSITION) ? 1 : SERVO_MAX_POSITION;

    lastRightDoorTimestamp = now;
  }
}

void updateLed() {
  if(currentGearState == GEAR_STATE_UP) {
    digitalWrite(LED_GEAR_UP_PIN, HIGH);
    digitalWrite(LED_RED_PIN, LOW);
    digitalWrite(LED_GEAR_DOWN_PIN, LOW);
  } else if(currentGearState == GEAR_STATE_MOVING) {
    digitalWrite(LED_GEAR_UP_PIN, LOW);
    digitalWrite(LED_RED_PIN, HIGH);
    digitalWrite(LED_GEAR_DOWN_PIN, LOW);
  } else if(currentGearState == GEAR_STATE_DOWN) {
    digitalWrite(LED_GEAR_UP_PIN, LOW);
    digitalWrite(LED_RED_PIN, LOW);
    digitalWrite(LED_GEAR_DOWN_PIN, HIGH);
  }
}

void checkSwitch() {
  //Serial.print(digitalRead(GEAR_UP_SWITCH_PIN));
  //Serial.println(targetGearState);

  if(digitalRead(GEAR_UP_SWITCH_PIN) == SWTICH_PRESSED) {
    targetGearState = GEAR_STATE_UP;
  }
  //if(digitalRead(GEAR_DOWN_SWITCH_PIN) == SWTICH_PRESSED) {
  //  targetGearState = GEAR_STATE_DOWN;
  //}
}