#include <IRremote.h>
int IRPin = 7;
int greenLED = 3;
int yellowLED = 4;
int redLED = 5;
int rightForward = 13;
int rightBackward = 12;
int leftForward = 11;
int leftBackward = 10;
int leftFan = 2;
int rightFan = 8;

IRrecv irrecv(IRPin);
decode_results results;


void setup() {
  pinMode(greenLED, OUTPUT);
  pinMode(yellowLED, OUTPUT);
  pinMode(redLED, OUTPUT);
  pinMode(rightForward, OUTPUT);
  pinMode(rightBackward, OUTPUT);
  pinMode(leftBackward, OUTPUT);
  pinMode(leftForward, OUTPUT);
  pinMode(leftFan, OUTPUT);
  pinMode(rightFan, OUTPUT);
  ledDebug(-2, 4, true, 0.5);
  //Inital Check of LED
  irrecv.enableIRIn();
  activateLeftFan(2);
  activateRightFan(2);

  /*
  moveLeft(2.0);
  moveRight(2.0);
  moveForward(2.0);
  moveBackward(2.0);
  tiltLeft(2.0);
  tiltRight(2.0);
  */
}


void loop() {
  digitalWrite(greenLED, HIGH);
  if (irrecv.decode(&results)) {
  }
}


void activateLeftFan(double duration) {
  long startTime = millis();
  ledDebug(2, duration, true, 0.7);
  while ((duration == -1) ? true : millis() < startTime + duration * 1000) {
    digitalWrite(leftFan, HIGH);
  }
}

void activateRightFan(double duration) {
  long startTime = millis();
  ledDebug(2, duration, true, 0.2);
  while ((duration == -1) ? true : millis() < startTime + duration * 1000) {
    digitalWrite(rightFan, HIGH);
  }
}


void moveLeft(double duration) {
  long startTime = millis();
  ledDebug(1, duration, true, 0.7);
  while ((duration == -1) ? true : millis() < startTime + duration * 1000) {
    digitalWrite(rightForward, HIGH);
  }

}
void moveRight(double duration) {
  long startTime = millis();
  ledDebug(1, duration, true, 0.7);
  while ((duration == -1) ? true : millis() < startTime + duration * 1000) {
    digitalWrite(leftForward, HIGH);
  }
}
void moveForward(double duration) {

  long startTime = millis();
  ledDebug(1, duration, true, 0.2);
  while ((duration == -1) ? true : millis() < startTime + duration * 1000) {
    digitalWrite(rightForward, HIGH);
    digitalWrite(leftForward, HIGH);
  }
}
void moveBackward(double duration) {

  long startTime = millis();
  ledDebug(1, duration, true, 0.7);
  while ((duration == -1) ? true : millis() < startTime + duration * 1000) {
    digitalWrite(rightForward, HIGH);
  }

}

void tiltLeft(double duration) {
  long startTime = millis();
  ledDebug(1, duration, true, 0.2);
  while ((duration == -1) ? true : millis() < startTime + duration * 1000) {
    digitalWrite(rightForward, HIGH);
    digitalWrite(leftForward, LOW);
  }

}

void tiltRight(double duration) {
  long startTime = millis();
  ledDebug(1, duration, true, 0.2);
  while ((duration == -1) ? true : millis() < startTime + duration * 1000) {
    digitalWrite(rightForward, LOW);
    digitalWrite(leftForward, HIGH);
  }
}

void ledDebug(int code, double duration) {
  long startTime = millis();
  while ((duration == -1) ? true : millis() < startTime + duration * 1000) {
    Serial.println("Running");
    switch (code) {
      case 0:
        digitalWrite(greenLED, HIGH);
        break;
      case 1:
        digitalWrite(yellowLED, HIGH);
        break;
      case 2:
        digitalWrite(redLED, HIGH);
        break;
      case -1:
        digitalWrite(greenLED, LOW);
        digitalWrite(yellowLED, LOW);
        digitalWrite(redLED, LOW);
        break;
      case -2:
        digitalWrite(greenLED, HIGH);
        digitalWrite(yellowLED, HIGH);
        digitalWrite(redLED, HIGH);
        break;
      default:
        Serial.println("Unknown debug code.");
        return;
    }
  } //Reset to off.
  digitalWrite(greenLED, LOW);
  digitalWrite(yellowLED, LOW);
  digitalWrite(redLED, LOW);
}

void ledDebug(int code, double duration, boolean toBlink, double delay) {
  long startTime = millis();
  long endTime = millis() + duration * 1000;
  int index = 0;
  while (millis() < endTime) {
    if (millis() - delay * 1000 != startTime) {
      continue;
    }
    if (index == 1) {
      index = 0;
    } else {
      index = 1;
    }
    switch (code) {
      case 0:
        if (toBlink) {
          if (index == 1) {
            digitalWrite(greenLED, HIGH);
          }
          else {
            digitalWrite(greenLED, LOW);
          }
          break;
        }
        digitalWrite(greenLED, HIGH);
        break;
      case 1:
        if (toBlink) {
          if (index == 1) {
            digitalWrite(yellowLED, HIGH);
          }
          else {
            digitalWrite(yellowLED, LOW);
          }
        }
        digitalWrite(yellowLED, HIGH);
        break;
      case 2:
        if (toBlink) {
          if (index == 1) {
            digitalWrite(redLED, HIGH);
          }
          else {
            digitalWrite(redLED, LOW);
          }
          break;
        }
        digitalWrite(redLED, HIGH);
        break;
      case -2:
        digitalWrite(greenLED, HIGH);
        digitalWrite(yellowLED, HIGH);
        digitalWrite(redLED, HIGH);
        break;
      case -1:
        digitalWrite(greenLED, LOW);
        digitalWrite(yellowLED, LOW);
        digitalWrite(redLED, LOW);
        break;
      default:
        Serial.println("Unknown debug code.");
        break;
    }
  }
  digitalWrite(greenLED, LOW);
  digitalWrite(yellowLED, LOW);
  digitalWrite(redLED, LOW);
}