#include "Robot_Base_main.h"
#include <BMSerial/BMSerial.h>
#include <BMSerial/icrmacros.h>
#include <RoboClaw/RoboClaw.h>

#define address 0x80
#define LED_PIN 13
#define RC_RX_PIN 12
#define RC_TX_PIN 11

// serial input //
#define MAX_COMMAND 10
#define BUF_SIZE 32
byte buf[BUF_SIZE];
byte cmd = 0;
byte bytes = 0;

// RoboClaw //
RoboClaw roboclaw(RC_RX_PIN, RC_TX_PIN);

// common vars //
unsigned long lastCommandTime;
unsigned long commandTime;
unsigned long cmdTimeout = 2500;
boolean ledCmdOn = false;
boolean IsConnected = false;

//=======================================

void setup() 
{
  pinMode(LED_PIN, OUTPUT);
  roboclaw.begin(38400);
  Serial.begin(38400);
}

void loop()
{
  ReadSerial();
  if (IsConnected) CheckCommandTimeout();
}

//=======================================

// read serial data into buffer. execute command
void ReadSerial()
{
  while (Serial.available())
  {
    buf[bytes] = Serial.read();
    if (buf[bytes] == 10 || buf[bytes] == 13 || bytes >= MAX_COMMAND)
    {
      if (bytes > 0) Execute();
      return;
    }
    bytes++;
  }
}

void CheckCommandTimeout()
{
  commandTime = millis();
  if (commandTime >= lastCommandTime) commandTime -= lastCommandTime; else lastCommandTime = 0;
  if (commandTime > cmdTimeout) OnCommandTimeout();
}

//=======================================

void OnCommandTimeout()
{
  OnDisconnected();
}

void OnDisconnected()
{
  driveStop();
  IsConnected = false;
  if (ledCmdOn) switchCommandLed();
}

void OnValidCommand()
{
  IsConnected = true;
  lastCommandTime = millis();
  switchCommandLed();
}

void switchCommandLed()
{
  ledCmdOn = !ledCmdOn;
  if (ledCmdOn) digitalWrite(LED_PIN, HIGH); else digitalWrite(LED_PIN, LOW);
}

//=======================================

// check that CRC is okay
boolean CRC_OK()
{
  byte crc = 0;
  for (byte i = 0; i < bytes - 1; i++) crc += buf[i];
  crc = crc & 0x7F; // 7 bit CRC
  boolean result = crc == buf[bytes - 1];
  if (!result)
  {
    bytes = 0; // empty the buf
    driveStop(); // disable motors
    Serial.println("CRC"); // report CRC error
  }
  return result;
}

// execute command in buffer //
void Execute()
{
  if (!CRC_OK()) return;
  
  bytes = 0; // empty input buffer (only one command at a time)
  cmd = buf[0]; // first character is a command identifier
  if (cmd == 0 || cmd == 32) return;
  
  if (cmd == 'D' || cmd == 'd') drive();
  else if (cmd == 'B' || cmd == 'b') battery();
  else if (cmd == 'R' || cmd == 'r') dread();
  else if (cmd == 'W' || cmd == 'w') dwrite();
  else if (cmd == 'V' || cmd == 'v') ver();
  else if (cmd == 'Z' || cmd == 'z')
  {
    OnDisconnected();
    return;
  }
  else
  {
    Serial.println("NAK"); // unknown command. report negative acknowledge
    return;
  }
  
  OnValidCommand();
}

//=======================================

// 'drive' command. both motors in one command
void drive()
{
  byte m1 = bctoi(1, 3);
  byte m2 = bctoi(4, 3);
  roboclaw.ForwardBackwardM1(address, m1);
  roboclaw.ForwardBackwardM2(address, m2);
}

// stop both motors immediately
void driveStop()
{
  roboclaw.ForwardBackwardM1(address, 64);
  roboclaw.ForwardBackwardM2(address, 64);
  roboclaw.flush();
}

// main battery 'voltage' command
void battery()
{
  unsigned int voltage = (unsigned int)roboclaw.ReadMainBatteryVoltage(address, 0);
  buf[2] = 0;
  buf[3] = 0;
  buf[4] = 0;
  buf[5] = 0;
  buf[6] = 0;
  utoa(voltage, (char*)buf, 10);
  Serial.write('B');
  Serial.println((char*)buf);
}

// 'version' command
void ver()
{
  Serial.write('V');
  Serial.write('1');
  if (roboclaw.ReadVersion(address, (char*)buf)) { Serial.write(';'); Serial.println((char*)buf); }
  else Serial.println("");
}

// 'digitalWrite' command
void dwrite()
{
  byte pin = bctoi(1, 2);
  pinMode(pin, OUTPUT);
  if (buf[3] == '0') digitalWrite(pin, 0); else digitalWrite(pin, 1);
}

// 'digitalRead' command
void dread()
{
  byte pin = bctoi(1, 2);
  pinMode(pin, INPUT_PULLUP);
  if (digitalRead(pin) == HIGH) buf[3] = '1'; else buf[3] = '0';
  buf[0] = 'R';
  buf[4] = 0;
  Serial.println((char*)buf);
}

// converts string to number. reads count chars starting at index position in buffer
byte bctoi(byte index, byte count)
{
  byte i = 0;
  byte fin = index + count;
  if (fin > BUF_SIZE) fin = BUF_SIZE;
  while (buf[index] >= '0' && buf[index] <= '9' && index < fin)
  {
    i *= 10;
    i += buf[index] - '0';
    index++;
  }
  return i;
}