#include "version.h"
#include <SPI.h>
#include <Wire.h>
#include <Scout.h>
#include <GS.h>
#include <bitlash.h>
#include <lwm.h>
#include <js0n.h>

  // c0040b2a2f0fcb6f
  // http://www.pololu.com/docs/0J21/10.a
  enum {
    PI_SIGNATURE           = 0x81,
    PI_RAW_SENSORS         = 0x86,
    PI_CALIBRATED_SENSORS  = 0x87,
    PI_TRIMPOT             = 0xB0,
    PI_BATTERY_MV          = 0xB1,
    PI_PLAY_MUSIC          = 0xB3,
    PI_CALIBRATE           = 0xB4,
    PI_RESET_CALIBRATION   = 0xB5,
    PI_LINE_POSITION       = 0xB6,
    PI_CLEAR_LCD           = 0xB7,
    PI_PRINT               = 0xB8,
    PI_LCD_GOTO_XY         = 0xB9,
    PI_AUTOCALIBRATE       = 0xBA,
    PI_START_PID           = 0xBB,
    PI_STOP_PID            = 0xBC,
    PI_M1_FORWARD          = 0xC1,
    PI_M1_BACKWARD         = 0xC2,
    PI_M2_FORWARD          = 0xC5,
    PI_M2_BACKWARD         = 0xC6
  };

  uint32_t timer = millis();
  uint32_t safetyTimer = 0; // start in a disabled state
  bool stopped = true;
  
  void setup() {
    Scout.setup(SKETCH_NAME, SKETCH_REVISION, SKETCH_BUILD);
    
    addBitlashFunction("rover.forward", (bitlash_function)moveForward);
    addBitlashFunction("rover.left", (bitlash_function)turnLeft);
    addBitlashFunction("rover.right", (bitlash_function)turnRight);
    addBitlashFunction("rover.backward", (bitlash_function)moveBackward);
    addBitlashFunction("rover.stop", (bitlash_function)stop);
    
    initialize3Pi();
    Led.blinkGreen(200);
  }

  void loop() {
    Scout.loop();
  }

  void initialize3Pi() {
    Serial1.begin(115200);
    Serial1.write(PI_SIGNATURE);
    Serial1.write(PI_CLEAR_LCD);
    Serial1.write(PI_PRINT);
    Serial1.write(8);
    Serial1.print("Pinoccio");
    Serial1.write(PI_LCD_GOTO_XY);
    Serial1.write(0);
    Serial1.write(1);
    Serial1.write(PI_PRINT);
    Serial1.write(8);
    Serial1.print("Lets go!");
  }

  numvar moveForward() {
    Serial1.write(PI_M1_FORWARD);
    Serial1.write(32);
    Serial1.write(PI_M2_FORWARD);
    Serial1.write(32);
    safetyTimer = millis();
    stopped = false;
  }
  
  numvar turnLeft() {
    Serial1.write(PI_M1_BACKWARD);
    Serial1.write(32);
    Serial1.write(PI_M2_FORWARD);
    Serial1.write(32);
    safetyTimer = millis();
    stopped = false;
  }
  
  numvar turnRight() {
    Serial1.write(PI_M1_FORWARD);
    Serial1.write(32);
    Serial1.write(PI_M2_BACKWARD);
    Serial1.write(32);
    safetyTimer = millis();
    stopped = false;
  }
  
  numvar moveBackward() {
    Serial1.write(PI_M1_BACKWARD);
    Serial1.write(32);
    Serial1.write(PI_M2_BACKWARD);
    Serial1.write(32);
    safetyTimer = millis();
    stopped = false;
  }
  
  numvar stop() {
    Serial1.write(PI_STOP_PID);
    safetyTimer = millis();
    stopped = true;
  }