//#include <boarddefs.h>
#include <IRremote.h>
//#include <IRremoteInt.h>
//#include <ir_Lego_PF_BitStreamEncoder.h>
#define IR 4
IRrecv irrecv(IR);

//GPIO Wheels
#define WHEEL_FRONT_LEFT_F A0
#define WHEEL_REAR_LEFT_R A1
#define WHEEL_REAR_RIGHT_F A2
#define WHEEL_FRONT_RIGHT_R A3
#define WHEEL_REAR_RIGHT_R 7
#define WHEEL_FRONT_RIGHT_F 8
#define WHEEL_REAR_LEFT_F 9
#define WHEEL_FRONT_LEFT_R 10

//Response from Remote
#define RFORWARD 4060792887
#define RBACKWARD 4060752087
#define RSTRAFE_LEFT 4060768407
#define RSTRAFE_RIGHT 4060784727
#define RSTOP 4060801047
#define RTURN_LEFT 4060750047
#define RTURN_RIGHT 4060790847
#define RDIAG_LEFT_FOR 4060786767
#define RDIAG_RIGHT_BACK 4060778607
#define RDIAG_RIGHT_FOR 4060782687
#define RDIAG_LEFT_BACK 4060766367

//direction bitmaps
#define DSTOP 0b00000000
#define DFORWARD 0b11110000
#define DBACKWARD 0b00001111
#define DSTRAFE_LEFT 0b01101001
#define DSTRAFE_RIGHT 0b10010110
#define DDIAG_LEFT_FOR 0b01100000
#define DDIAG_RIGHT_BACK 0b00000110
#define DDIAG_RIGHT_FOR 0b10010000
#define DDIAG_LEFT_BACK 0b00001001
#define DTURN_LEFT 0b10100101
#define DTURN_RIGHT 0b01011010

/* All Responses
 * 4060792887 ^
 * 4060752087 v
 * 4060768407 <
 * 4060784727 >
 * 4060801047 enter
 * 4060776567 menu
 * 4060774527 aspect
 * 4060745967 vol+
 * 4060794927 vol-
 * 4060803087 mute
 * 4060750047 source
 * 4060790847 video mode
 * 4060786767 keystone +
 * 4060778607 keystone -
 * 4060782687 mouse +
 * 4060766367 mouse -
 * 4060748007 auto adj
 * 4060743927 freeze
 * 4060754127 blank
 * 4060762287 zoom +
 * 4060770447 zoom -
 * 4060759227 info
 * 4060780647 vga
 * 4060742907 video
 * 4060775547 s-video
 */

void setup() {
  Serial.begin(115200);
  irrecv.enableIRIn();
  pinMode(WHEEL_REAR_RIGHT_R, OUTPUT);
  pinMode(WHEEL_REAR_RIGHT_F, OUTPUT);
  pinMode(WHEEL_REAR_LEFT_R, OUTPUT);
  pinMode(WHEEL_REAR_LEFT_F, OUTPUT);
  pinMode(WHEEL_FRONT_RIGHT_R, OUTPUT);
  pinMode(WHEEL_FRONT_RIGHT_F, OUTPUT);
  pinMode(WHEEL_FRONT_LEFT_R, OUTPUT);
  pinMode(WHEEL_FRONT_LEFT_F, OUTPUT);
}

void drive(int direction) {
    digitalWrite(WHEEL_FRONT_LEFT_F, direction & 0b10000000 ? LOW : HIGH);
    digitalWrite(WHEEL_FRONT_RIGHT_F, direction & 0b01000000 ? LOW : HIGH);
    digitalWrite(WHEEL_REAR_LEFT_F, direction & 0b00100000 ? LOW : HIGH);
    digitalWrite(WHEEL_REAR_RIGHT_F, direction & 0b00010000 ? LOW : HIGH);

    digitalWrite(WHEEL_FRONT_LEFT_R, direction & 0b00001000 ? LOW : HIGH);
    digitalWrite(WHEEL_FRONT_RIGHT_R, direction & 0b00000100 ? LOW : HIGH);
    digitalWrite(WHEEL_REAR_LEFT_R, direction & 0b00000010 ? LOW : HIGH);
    digitalWrite(WHEEL_REAR_RIGHT_R, direction & 0b00000001 ? LOW : HIGH);
}

//MIK IR Remote.pdf S.12 I. Kübler
unsigned long getIRremote(){
  static unsigned long lastValue = 0;
  decode_results irResults;
  if(irrecv.decode(&irResults) == 0){
    return 0;
  }
  if(!(irResults.decode_type == NEC && irResults.value == REPEAT)){
    lastValue = irResults.value;
  }
  irrecv.resume();
  return lastValue;
}

void loop() {
  
  unsigned long remote = getIRremote();
  static unsigned long mode;
  static unsigned long lastMode = 0;
  if(remote){
    Serial.println(remote);
    mode = remote;
    if(mode != lastMode){
      drive(DSTOP);
      delay(50);
      lastMode = mode;
    }
  }
  switch(mode){
    case RFORWARD:
      drive(DFORWARD);
      break;
    case RSTOP:
      drive(DSTOP);
      break;
    case RBACKWARD:
      drive(DBACKWARD);
      break;
    case RSTRAFE_LEFT:
      drive(DSTRAFE_LEFT);
      break;
    case RSTRAFE_RIGHT:
      drive(DSTRAFE_RIGHT);
      break;
    case RTURN_LEFT:
      drive(DTURN_LEFT);
      break;
    case RTURN_RIGHT:
      drive(DTURN_RIGHT);
      break;
    case RDIAG_LEFT_FOR:
      drive(DDIAG_LEFT_FOR);
      break;
    case RDIAG_LEFT_BACK:
      drive(DDIAG_LEFT_BACK);
      break;
    case RDIAG_RIGHT_FOR:
      drive(DDIAG_RIGHT_FOR);
      break;
    case RDIAG_RIGHT_BACK:
      drive(DDIAG_RIGHT_BACK);
      break;
  }
  /*
  if(mode == RFORWARD){
      drive(DFORWARD);
  }else if(mode == RSTOP){
    drive(DSTOP);
  }else if(mode == RBACKWARD){
    drive(DBACKWARD);
  }else if(mode == RSTRAFE_LEFT){
    drive(DSTRAFE_LEFT);
  }else if(mode == RSTRAFE_RIGHT){
    drive(DSTRAFE_RIGHT);
  }else if(mode == RTURN_LEFT){
    drive(DTURN_LEFT);
  }else if(mode == RTURN_RIGHT){
    drive(DTURN_RIGHT);
  }else if(mode == RDIAG_LEFT_FOR){
    drive(DDIAG_LEFT_FOR);
  }else if(mode == RDIAG_RIGHT_BACK){
    drive(DDIAG_RIGHT_BACK);
  }else if(mode == RDIAG_RIGHT_FOR){
    drive(DDIAG_RIGHT_FOR);
  }else if(mode == RDIAG_LEFT_BACK){
    drive(DDIAG_LEFT_BACK);
  }
  */
}