/*
11011110010101000100111110111111-STOP-3730067391
10111010001011111000111100100100-UP-3123679012
10001001000100100001101010111101-Down-2299665085
10101101101100001001101000001001-Left-2914032137
00010101100100100000111100101110-Right-361893678

00010010001001101110010010000010-UPLEFT-304538754
11011001010001010010111101101011-UPRIGHT-3645189995
00001010111011100110010100111011-DOWNLEFT-183395643
01011000010100011111010111101001-DOWNRIGHT-1481766377

10111000011010110001001011110111-HIGHSTOP-3094024951
01010011011011111000110110100100-HIGHUP-1399819684
00010101000011001111001000000011-HIGHDOWN-353169923
11100010111111011001110101000011-HIGHLEFT-3808271683
10100001100011001110011001110000-HIGHRIGHT-2710365808

11111010110000001010011100000000-HIGHUPLEFT-4206929664
00100100011010010100000111110101-HIGHUPRIGHT-610877941
01101101101110111101100010100111-HIGHDOWNLEFT-1841027239
10111111000100011111011101101001-HIGHDOWNRIGHT-3205625705
 */

#include <IRremote.h>

int RECV_PIN = 11;
int ledPin = 13;

IRrecv irrecv(RECV_PIN);

decode_results irData;
int result;

int speedHIGH = 128;//-------------------------------
int speedLOW = 32;//-------------------------------

int leftDir=5;
int leftPWM=9;
int rightDir=4;
int rightPWM=10;

int leftAmpPin = A0;
int rightAmpPin = A1;
int leftAmpReading;
int rightAmpReading;
float leftAmps;
float rightAmps;
float conversionFactor;

int forward = 1;
int backward = 0;

int leftEncoderPin = 3;
int rightEncoderPin = 2;
unsigned long lPulse;
unsigned long rPulse;
unsigned long lTime;
unsigned long rTime;
unsigned long lCount = 0;
unsigned long rCount = 0;
unsigned long lCountLast = 0;
unsigned long rCountLast = 0;

int lPWM, rPWM, lSpeed, rSpeed;
unsigned long maxspeed = 520;//520 according to OB1
unsigned long actual;

void setup()
{
  Serial.begin(9600);
  irrecv.enableIRIn(); // Start the receiver

  analogReference(DEFAULT);

  conversionFactor = 5.0/1023.0;

  pinMode(leftDir, OUTPUT);
  pinMode(leftPWM, OUTPUT);
  pinMode(rightDir, OUTPUT);
  pinMode(rightPWM, OUTPUT);

  attachInterrupt(0, leftEncoderChange, CHANGE);
  attachInterrupt(1, rightEncoderChange, CHANGE);
}

void loop() {
  if (irrecv.decode(&irData)) {
    result = irData.value;
    //Serial.println(irData.value);

    switch(result) {
      case 3123679012:
        leftMotor(forward, speedLOW);
        rightMotor(forward, speedLOW);
        led(0);
        //slow forwards
        break;
      case 2299665085:
        leftMotor(backward, speedLOW);
        rightMotor(backward, speedLOW);
        led(0);
        //backwards
        //slow
        break;
      case 2914032137:
        leftMotor(backward, speedLOW);
        rightMotor(forward, speedLOW);
        led(0);
        //left
        //slow
        break;
      case 361893678:
        leftMotor(forward, speedLOW);
        rightMotor(backward, speedLOW);
        led(0);
        //right
        //slow
        break;
      case 304538754:
        //forward-left slow
        leftMotor(forward, 0);
        rightMotor(forward, speedLOW);
        break;
      case 3645189995:
        //forward-right slow
        leftMotor(forward, speedLOW);
        led(0);
        rightMotor(forward, 0);
        break;
      case 183395643:
        //backward left slow
        leftMotor(backward, speedLOW);
        rightMotor(backward, 0);
        led(0);
        break;
      case 1481766377:
        //backward right slow
        leftMotor(backward, 0);
        rightMotor(backward, speedLOW);
        led(0);
        break;

      case 1399819684:
        //forward fast
        leftMotor(forward, speedHIGH);
        rightMotor(forward, speedHIGH);
        led(1);
        break;
      case 353169923:
        //backward fast
        leftMotor(backward, speedHIGH);
        rightMotor(backward, speedHIGH);
        led(1);
        break;
      case 3808271683:
        //left fast
        leftMotor(backward, speedHIGH);
        rightMotor(forward, speedHIGH);
        led(1);
        break;
      case 2710365808:
        //right fast
        leftMotor(forward, speedHIGH);
        rightMotor(backward, speedHIGH);
        led(1);
        break;

      case 4206929664:
        //highspeed forwards left
        leftMotor(forward, speedLOW);
        rightMotor(forward, speedHIGH);
        led(1);
        break;
      case 610877941:
        //highspeed forwards right
        leftMotor(forward, speedHIGH);
        rightMotor(forward, speedLOW);
        led(1);
        break;
      case 1841027239:
        //high back left
        leftMotor(backward, speedLOW);
        rightMotor(backward, speedHIGH);
        led(1);
        break;
      case 3205625705:
        //high back right
        leftMotor(backward, speedLOW);
        rightMotor(backward, speedHIGH);
        led(1);
        break;


      case 3730067391:
        leftMotor(forward, 0);
        rightMotor(forward, 0);

      case 3094024951:
        leftMotor(forward, 0);
        rightMotor(forward, 0);


      default:
        //stop

        break;
    }
    leftAmpReading = analogRead(leftAmpPin);
    leftAmps = leftAmpReading * conversionFactor;
    rightAmpReading = analogRead(rightAmpPin);
    rightAmps = rightAmpReading * conversionFactor;

    Serial.print("Left:  ");
    Serial.print(leftAmps);
    Serial.print("A ");
    Serial.println(lCount);
    Serial.print("Right: ");
    Serial.print(rightAmps);
    Serial.print("A ");
    Serial.println(rCount);

    irrecv.resume(); // Receive the next value
  }
  else {
    //stop

  }

  //===================================== Motor Speed Control ============================
  if (micros()-lTime > 50000ul && lSpeed != 0) lPWM += 4;
  if (micros()-rTime > 50000ul && rSpeed != 0) rPWM += 4;

  actual = 20000/(abs(lSpeed)*maxspeed/255);
  if (actual>lPulse && lPWM>0) lPWM--;
  if (actual<lPulse && lPWM<2549) lPWM++;

  actual = 20000/(abs(rSpeed)*maxspeed/255);
  if (actual>rPulse && rPWM>0) rPWM--;
  if (actual<rPulse && rPWM<2549) rPWM++;

  if (lSpeed > 0) {
  analogWrite(leftPWM, lPWM);
  }
  else {
    analogWrite(leftPWM, 0);
  }
  if (rSpeed > 0) {
    analogWrite(rightPWM, rPWM);
  }
  else {
    analogWrite(rightPWM, 0);
  }
}

void leftMotor(int motorDirection, int motorPWM){
  digitalWrite(leftDir, motorDirection);
  //analogWrite(leftPWM, motorPWM);
  motorPWM = map(motorPWM, 0, 255, 0, maxspeed);
  lSpeed = motorPWM;
}
void rightMotor(int motorDirection, int motorPWM){
  digitalWrite(rightDir, motorDirection);
  //analogWrite(rightPWM, motorPWM);
  motorPWM = map(motorPWM, 0, 255, 0, maxspeed);
  rSpeed = motorPWM;
}

void led(int state) {
  digitalWrite(ledPin, state);
}

void leftEncoderChange() {
  lPulse = int((micros() - lTime)/50);
  lTime = micros();
  lCount++;
}
void rightEncoderChange() {
  rPulse = int((micros() - rTime)/50);
  rTime = micros();
  rCount++;
}