// IDE 0022
//-------------
// nicoo
// april 2013
//-------------

/*
 * Arduino pin 0 (RX) is connected to Roomba TXD 4
 * Arduino pin 1 (TX) is connected to Roomba RXD 3
 * Arduino pin 2      is connected to Roomba DD 5
 */

int useless_variable_to_force_ide_to_insert_its_lines_here = 0;
 
#include <NewSoftSerial.h>
#include <Wire.h>

//-- Nunchuk ---------------------
#include <Wire.h>
#include <ArduinoNunchuk.h>
#include <NicoArduinoNunchuk.h>
NicoArduinoNunchuk nunchuk;
//----------

int forward = -1;
const int vitnorm = 500;
const int vitmax = 100;
const int topspeed = 500;

// debug Nunchuk
#define LED_HIGH 13
#define LED_UP 12
#define LED_DOWN 11
#define LED_RIGHT 10
#define LED_LEFT 9
//----------

#define PIN_INIT 6
#define PIN_RELAY 7 
#define PIN_BUT1 8
#define ANA_BUT2 3

#define ROOMBA_DELAY 50
#define ROOMBA_MDELAY 1

//#define USE_FORWARD
//#define USE_CANON
//#define DBG_DRIVE

/// serial ports
// 
int rxPin_nss = 3;
int txPin_nss = 4;
int ddPin = 5;
NewSoftSerial roombaSerial(rxPin_nss,txPin_nss);
#define termSerial Serial

void dance();
void roombaDrive(int velocity, int radius=0x8000);

void roomba_wakeup()
{
  digitalWrite(ddPin, HIGH);
  delay(100);
  digitalWrite(ddPin, LOW);
  delay(500);
  digitalWrite(ddPin, HIGH);
  delay(2000);
}

#define ROOMBA_WRITE1(A) roombaSerial.print(A, BYTE); delay(ROOMBA_MDELAY);
#define ROOMBA_WRITE2(A,B) ROOMBA_WRITE1(A); ROOMBA_WRITE1(B); 

void roomba_init_sequence()
{
  roombaSerial.begin(57600); // ROBOT1
  // wake up the robot
  roomba_wakeup();

  //  "Start" command [128]- Changes Roomba mode from every mode to "Passive" and tells Roomba to be prepared to start accepting commands;
  //  "Control" command [130]- Changes Roomba mode from "Passive" to "Safe" mode (for Roomba 4XX, Create and Dirt Dog models) or from "Passive" or "Full" mode to "Safe" mode for the other Roomba models;
  //  "Safe" command [131]- Changes Roomba mode from "Full" mode to "Safe" mode (for Roomba 4XX, Create and Dirt Dog models) or from "Passive" or "Full" mode to "Safe" mode for the other Roomba models;
  //  "Full" command [132]- Changes Roomba mode from "Safe" to "Full" (for Roomba 4XX, Create and Dirt Dog models) or from "Passive" or "Safe" mode to "Full" mode for the other Roomba models;

  // set up ROI to receive commands  
  roombaSerial.print(128, BYTE); delay(ROOMBA_DELAY); // START
  roombaSerial.print(130, BYTE); delay(ROOMBA_DELAY); // CONTROL
  roombaSerial.print(132, BYTE); delay(ROOMBA_DELAY); // FULL
}

void setup() {
  pinMode(ddPin,  OUTPUT);   // sets the pins as output

  pinMode(LED_UP, OUTPUT);
  pinMode(LED_DOWN, OUTPUT);
  pinMode(LED_RIGHT, OUTPUT);
  pinMode(LED_LEFT, OUTPUT);
  pinMode(LED_HIGH, OUTPUT);
  digitalWrite(LED_HIGH, HIGH);
  digitalWrite(LED_UP, LOW);
  digitalWrite(LED_DOWN, LOW);
  digitalWrite(LED_RIGHT, LOW);
  digitalWrite(LED_LEFT, LOW);

  pinMode(PIN_INIT, INPUT);
  pinMode(PIN_BUT1, INPUT);
  pinMode(PIN_RELAY, OUTPUT);
//ANA_BUT2 3

  termSerial.begin(57600);
  termSerial.print("starting up... ");

  roomba_init_sequence();

  termSerial.println("ready!");

  // [re]set baud rate to 57600
  //roombaSerial.print(129, BYTE);  // BAUD
  //roombaSerial.print(10, BYTE);   // 57600
  //delay(100);

  dance(); //  demonstrate that controls work

  nunchuk.init();
//  nunchuck_setpowerpins();
//  nunchuck_init(); // send the initilization handshake

  delay(2000);

  digitalWrite(LED_UP, HIGH);
  digitalWrite(LED_DOWN, HIGH);
  digitalWrite(LED_RIGHT, HIGH);
  digitalWrite(LED_LEFT, HIGH);

  roombaSerial.print(132, BYTE); delay(ROOMBA_DELAY); // FULL
}

void dance() {
  termSerial.println("top of dance()");
  roombaDrive(100);
  delay(1000);
  roombaDrive(-100);
  delay(1000);
  roombaDrive(0);
}

void roombaDriveDistance(int velocity, int length)
{
    int time = int(1000.0 * double(abs(length)) / double(abs(velocity)/10) );
    roombaDrive(velocity*forward);
    delay(time);
    roombaDrive(0);
}


void flashled(int numled, int msec)
{
  digitalWrite(numled, LOW);
  delay(msec);
  digitalWrite(numled, HIGH);
}

void loop() 
{
  nunchuk.update();
  nunchuk.prt(Serial);
  int up = nunchuk.up;
  int down = nunchuk.down;
  int left = nunchuk.left;
  int right = nunchuk.right;
  int zbut = nunchuk.zButton;
  int cbut = nunchuk.cButton;

  digitalWrite(LED_UP, HIGH);
  digitalWrite(LED_DOWN, HIGH);
  digitalWrite(LED_RIGHT, HIGH);
  digitalWrite(LED_LEFT, HIGH);
  if (nunchuk.up && forward>0 || nunchuk.down && forward<0) digitalWrite(LED_UP, LOW);
  if (nunchuk.down && forward>0 || nunchuk.up && forward<0) digitalWrite(LED_DOWN, LOW);
  if (nunchuk.left && forward>0 || nunchuk.right && forward<0) digitalWrite(LED_RIGHT, LOW);
  if (nunchuk.right && forward>0 || nunchuk.left && forward<0) digitalWrite(LED_LEFT, LOW);

  int vitesse = zbut ? vitmax : vitnorm;
  int radius = 600;

  int analog3 = analogRead(3);
  if (analog3 > 255)
    {
    digitalWrite(PIN_RELAY, HIGH);
    delay(12000);
    digitalWrite(PIN_RELAY, LOW);
    }
    
  if (digitalRead(PIN_INIT))
    {
    int index;
    for(index=0; index<4; index++)
      {
      digitalWrite(LED_UP, LOW);
      digitalWrite(LED_DOWN, LOW);
      delay(100);
      digitalWrite(LED_UP, HIGH);
      digitalWrite(LED_DOWN, HIGH);
      digitalWrite(LED_RIGHT, HIGH);
      digitalWrite(LED_LEFT, HIGH);
      digitalWrite(LED_RIGHT, LOW);
      digitalWrite(LED_LEFT, LOW);
      delay(100);
      digitalWrite(LED_UP, HIGH);
      digitalWrite(LED_DOWN, HIGH);
      digitalWrite(LED_RIGHT, HIGH);
      digitalWrite(LED_LEFT, HIGH);
      }
    roomba_init_sequence();
    for(index=0; index<2; index++)
      {
      digitalWrite(LED_UP, LOW);
      digitalWrite(LED_DOWN, LOW);
      digitalWrite(LED_RIGHT, LOW);
      digitalWrite(LED_LEFT, LOW);
      delay(100);
      digitalWrite(LED_UP, HIGH);
      digitalWrite(LED_DOWN, HIGH);
      digitalWrite(LED_RIGHT, HIGH);
      digitalWrite(LED_LEFT, HIGH);
      }
    }
  
  vitesse *= forward;
  radius *= forward;

/*
  term.print("accx: "); Serial.print((byte)accx,DEC);
  termSerial.print("\taccy: "); termSerial.print((byte)accy,DEC);
  termSerial.print("\taccz: "); termSerial.print((byte)accz,DEC);
  termSerial.print("\tjoyx: "); termSerial.print((byte)joyx,DEC);
  termSerial.print("\tjoyy: "); termSerial.print((byte)joyy,DEC);
  termSerial.print("\n");  
*/
  if (up && !down && !left && !right)
    roombaDrive(vitesse);
  else if (up && !down && left && !right) 
    roombaDrive(vitesse,radius);
  else if (!up && !down && left && !right) 
    roombaDrive(vitesse,forward*1);
  else if (!up && down && left && !right) 
    roombaDrive(-vitesse,radius);
  else if (!up && down && !left && !right) 
    roombaDrive(-vitesse);
  else if (!up && down && !left && right) 
    roombaDrive(-vitesse,-radius);
  else if (!up && !down && !left && right) 
    roombaDrive(vitesse,forward*-1);
  else if (up && !down && !left && right) 
    roombaDrive(vitesse,-radius);
  else
    roombaDrive(0);
  
  delay(10);
}

// velocity: beetween -500 and 500 mm/s
// radius: between -2000 and 2000 mm
int gvelocity=0;
int gradius=0;
void roombaDrive(int velocity, int radius)
{
  if (velocity == gvelocity && radius == gradius)
    return;
  gvelocity = velocity;
  gradius = radius;

  byte vel1 = (velocity & 0xFF00) >> 8;
  byte vel2 = (velocity & 0x00FF) >> 0;
  byte rad1 = (radius & 0xFF00) >> 8;
  byte rad2 = (radius & 0x00FF) >> 0;
  if (radius == 0) { rad1=0x80; rad2=0x00; }
  
#ifdef DBG_DRIVE
  not used
  char buf[255];
  sprintf(buf, "%d 0x%x 0x%x 0x%x 0x%x", 137,vel1, vel2, rad1, rad2);
  if (velocity)
    termSerial.println(buf);
#endif

//  roombaSerial.print(132, BYTE); delay(ROOMBA_DELAY); // FULL
  roombaSerial.print(137, BYTE); delay(ROOMBA_MDELAY);  // DRIVE
  roombaSerial.print(vel1,BYTE); delay(ROOMBA_MDELAY);  // velocity
  roombaSerial.print(vel2,BYTE); delay(ROOMBA_MDELAY);
  roombaSerial.print(rad1,BYTE); delay(ROOMBA_MDELAY);  // radius
  roombaSerial.print(rad2,BYTE); delay(ROOMBA_MDELAY);
//  roombaSerial.print(130, BYTE); delay(DELAY); // CONTROL
//  roombaSerial.print(131, BYTE); delay(DELAY); // SAFE
  delay(ROOMBA_DELAY);
}