#include <Arduino_CAN.h>


/*
 Message spec

 CAN_ID is set to row number (master / main controller is 0)

 angles are encoded as int8_t in 0.5 degree increments

 Goto:  [G] [device] [angle]
 Stop:  [S]
 Query: [Q] [device]
 State: [A] [Moving|Error|Finished|Stopped] [mid_angle] [front_angle] [rear_angle]

 normal operation:
 - on a change, goto (G) is sent by master
 - devices heartbeat (A) with position periodically
 - master may command a system shutdown (S), takes a (G) to resume
 - master may force a query with (Q), a (A) is expected

 */

/*
 Note on CAN msg library
 [020] (4) : DEADBEEF
 [id] (len) : message_in_hexadecimal
 */

/*
if (int const rc = CAN.write(msg); rc < 0) {
    lcd.print("sendCAN ERR: ");
    Serial.println(rc);
  }
*/

int CAN_reset_row_ids() {
  uint8_t msg_data[] = { 'R' };
  CanMsg const msg(CanStandardId(CAN_ID), sizeof(msg_data), msg_data);
  int rc = -60003;
  // Apparently, the TX buffer can get full. Keep trying until the message sends. I wish there were a more elegant way to do this.
  for (uint16_t iters=0; rc == -60003 && iters<256; iters++ ) {
    rc = CAN.write(msg);
  }
  return debugCAN(rc);
}
int CAN_claim_row_id() {
  uint8_t msg_data[] = { 'C' };
  CanMsg const msg(CanStandardId(CAN_ID), sizeof(msg_data), msg_data);
  int rc = -60003;
  // Apparently, the TX buffer can get full. Keep trying until the message sends. I wish there were a more elegant way to do this.
  for (uint16_t iters=0; rc == -60003 && iters<256; iters++ ) {
    rc = CAN.write(msg);
  }
  return debugCAN(rc);  
}

int CAN_goto(uint8_t device, int8_t angle) {
  uint8_t msg_data[] = { 'G', device, angle };
  CanMsg const msg(CanStandardId(CAN_ID), sizeof(msg_data), msg_data);
  int rc = -60003;
  // Apparently, the TX buffer can get full. Keep trying until the message sends. I wish there were a more elegant way to do this.
  for (uint16_t iters=0; rc == -60003 && iters<256; iters++ ) {
    rc = CAN.write(msg);
  }
  //for(uint8_t i = 0; (rc=CAN.write(msg)) < 0 && i < 32; i++);;
  return debugCAN(rc);
}
int CAN_stop() {
  uint8_t msg_data[] = { 'S' };
  CanMsg const msg(CanStandardId(CAN_ID), sizeof(msg_data), msg_data);
  int rc = -60003;
  // Apparently, the TX buffer can get full. Keep trying until the message sends. I wish there were a more elegant way to do this.
  for (uint16_t iters=0; rc == -60003 && iters<256; iters++ ) {
    rc = CAN.write(msg);
  }
  //for(uint8_t i = 0; (rc=CAN.write(msg)) < 0 && i < 32; i++);;
  return debugCAN(rc);
}
int CAN_query(uint8_t device) {
  uint8_t msg_data[] = { 'Q', device };
  CanMsg const msg(CanStandardId(CAN_ID), sizeof(msg_data), msg_data);
  int rc = -60003;
  // Apparently, the TX buffer can get full. Keep trying until the message sends. I wish there were a more elegant way to do this.
  for (uint16_t iters=0; rc == -60003 && iters<256; iters++ ) {
    rc = CAN.write(msg);
  }
  //for(uint8_t i = 0; (rc=CAN.write(msg)) < 0 && i < 32; i++);;
  return debugCAN(rc);
}
int CAN_status(byte code, byte a, byte b, byte c, byte d) {
  uint8_t msg_data[] = { 'A', code, a, b, c, d};
  CanMsg const msg(CanStandardId(CAN_ID), sizeof(msg_data), msg_data);
  int rc = -60003;
  // Apparently, the TX buffer can get full. Keep trying until the message sends. I wish there were a more elegant way to do this.
  for (uint16_t iters=0; rc == -60003 && iters<256; iters++ ) {
    rc = CAN.write(msg);
  }
  //for(uint8_t i = 0; (rc=CAN.write(msg)) < 0 && i < 32; i++);;
  return debugCAN(rc);
}