#include "qt_pi.h"

enum COMM_STATE
{
  WAITING_FOR_NEW_COM,
  FIRST_BYTE_VERIFIED,
  SECOND_BYTE_VERIFIED,
  LENGTH_INPUT,
  READING_PAYLOAD,
  READING_COMPLETE,
  COM_RECEIVED
};

int curState = WAITING_FOR_NEW_COM;

int com_len;

uint8_t in_buf[buf_size];
int in_buf_i;

uint8_t ser_write_buf[buf_size];
uint8_t com_recv[buf_size];

int
init_comm ()
{

  ser_write_buf[MB1P] = magic_start1;
  ser_write_buf[MB2P] = magic_start2;

  Serial.begin (BAUDRATE);
  Serial.setTimeout (0);

  resetComm ();

  curState = WAITING_FOR_NEW_COM;
}

int
sendCommand (int16_t len)
{
  if ((len + net_extra_len) > buf_size)
    return -1;

  memcpy (ser_write_buf + start_magic_len, &len, len_len);

  ser_write_buf[preamble_len + len] = magic_end;
  
  return (Serial.write (ser_write_buf, len + net_extra_len));
}

int
sendData (int len, uint8_t *buf)
{

  if ((len + net_extra_len) > buf_size)
    return -1;

  memcpy (ser_write_buf + preamble_len, buf, len);

  return sendCommand (len);
}

int
runInpCommand ()
{

  int data_len = com_len - com_preamble_len;

  switch (com_recv[COMB])
    {

    case GET_BAUDRATE:

      if (data_len != GET_BAUDRATE_DATA_LEN)
        {
          data_len = -1;
          break;
        }

      memcpy (ser_write_buf + DATASTRBP, &BAUDRATE, 4);

      data_len = 4;

      break;

    case GET_ENCODERS:
      {

        if (data_len != GET_ENCODERS_DATA_LEN)
          {
            data_len = -1;
            break;
          }

        long l = readEncoder (LEFT_WHEEL);
        long r = readEncoder (RIGHT_WHEEL);

        memcpy (ser_write_buf + DATASTRBP, &l, 4);
        memcpy (ser_write_buf + DATASTRBP + 4, &r, 4);

        data_len = 8;

        break;
      }

    case RESET_ENCODERS:

      if (data_len != RESET_ENCODERS_DATA_LEN)
        {
          data_len = -1;
          break;
        }

      resetEncoders ();
      resetPID ();

      ser_write_buf[DATASTRBP] = 1;
      data_len = 1;

      break;

    case SET_SPEEDS:

      if (data_len != SET_SPEEDS_DATA_LEN)
        {
          data_len = -1;
          break;
        }

      move_cmd_timeout_reset ();
      
      setTicksPerFrame (com_recv + DATASTRB);

      ser_write_buf[DATASTRBP] = 1;
      data_len = 1;

      break;

    case SET_PID_L:

      if (data_len != SET_PID_DATA_LEN)
        {
          data_len = -1;
          break;
        }

      setPID_L (com_recv + DATASTRB);

      ser_write_buf[DATASTRBP] = 1;
      data_len = 1;

      break;

    case SET_PID_R:

      if (data_len != SET_PID_DATA_LEN)
        {
          data_len = -1;
          break;
        }

      setPID_R (com_recv + DATASTRB);

      ser_write_buf[DATASTRBP] = 1;
      data_len = 1;

      break;

    case GET_PID_P:

      if (data_len != GET_PID_P_DATA_LEN)
        {
          data_len = -1;
          break;
        }

      getPID (ser_write_buf + DATASTRBP);

      data_len = 8;

      break;

    case GET_BATTERY_LVL:

      if (data_len != GET_BATTERY_LVL_DATA_LEN)
        {
          data_len = -1;
          break;
        }

      getBattLvl (ser_write_buf + DATASTRBP);

      data_len = 4;

      break;

    case EN_IMU:

      if (data_len != EN_IMU_DATA_LEN)
        {
          data_len = -1;
          break;
        }

      enable_imu (com_recv[DATASTRB]);

      ser_write_buf[DATASTRBP] = 1;
      data_len = 1;

      break;

    case EN_ODOM:

      if (data_len != EN_ODOM_DATA_LEN)
        {
          data_len = -1;
          break;
        }

      enable_odom (com_recv[DATASTRB]);

      ser_write_buf[DATASTRBP] = 1;
      data_len = 1;

      break;

    default:

      ser_write_buf[DATASTRBP] = com_recv[COMB];
      com_recv[COMB] = INVALID;
      data_len = 1;

      break;
    }

  if (data_len == -1)
    {
      ser_write_buf[COMBP] = INVALID_LENGTH;
      ser_write_buf[CIDBP] = com_recv[CIDB];
      ser_write_buf[DATASTRBP] = com_recv[COMB];
      data_len = 1;
    }
  else
    {
      ser_write_buf[COMBP] = com_recv[COMB];
      ser_write_buf[CIDBP] = com_recv[CIDB];
    }

  sendCommand (data_len + com_preamble_len);
}

void
resetComm ()
{
  com_len = 1;
  in_buf_i = 0;
}

uint8_t temp_buf[64];
void
serCommTimeout ()
{
  resetComm();
  Serial.readBytes (temp_buf, 64);
}

void
proc_serial ()
{

  if (!Serial.available ())
    return;

  ser_comm_timeout_reset ();

  switch (curState)
    {

    case WAITING_FOR_NEW_COM:

      if (Serial.read () == magic_start1)
        {
          curState = FIRST_BYTE_VERIFIED;
        }
    
      break;

    case FIRST_BYTE_VERIFIED:

      if (Serial.read () == magic_start2)
        {
          curState = SECOND_BYTE_VERIFIED;
        }
      else
        {
          curState = WAITING_FOR_NEW_COM;
        }

      break;

    case SECOND_BYTE_VERIFIED:

      com_len = Serial.read ();
      curState = LENGTH_INPUT;

      break;

    case LENGTH_INPUT:

      com_len = com_len | (Serial.read () << 8);

      if (com_len < 20)
        {
          curState = READING_PAYLOAD;
        }
      else
        {
          curState = WAITING_FOR_NEW_COM;
        }

      break;

    case READING_PAYLOAD:

      in_buf_i += Serial.readBytes (&in_buf[in_buf_i], com_len - in_buf_i);

      if (in_buf_i == com_len)
        {
          curState = READING_COMPLETE;
        }
      break;

    case READING_COMPLETE:

      if (Serial.read () == magic_end)
        {

          memcpy (com_recv, in_buf, com_len);

          runInpCommand ();

          curState = WAITING_FOR_NEW_COM;
        }

      resetComm ();

      break;
    }

  if ((Serial.available () > 0) && curState != WAITING_FOR_NEW_COM)
    proc_serial ();
}
