#include <Adafruit_PWMServoDriver.h>
#include <SPI.h>
#include <mcp_can.h>
#include "defs.hpp"

Adafruit_PWMServoDriver board1 = Adafruit_PWMServoDriver(0x40);

int angleToPulse(int theta, int motorIndex) {
  return map(theta, 0, servo_range[motorIndex], SERVO_PMIN, SERVO_PMAX);
}

// ----- UART command handling -----
String inString = "";

String handleUartComm() {
  if (Serial.available() > 0) {
    unsigned char inChar = Serial.read();
    // Read finished procedure
    if (inChar == '\n') {
      // Copy return value
      String retStr = inString;

      // Reset
      inString = "";
      return retStr;
    }
    // Store byte procedure
    else {
      inString += (char)inChar;
      return "";
    }
  } else {
    return "";
  }
}

// ----- CAN config -----
const int CAN_CS_PIN = 10;           // CS pin for MCP2515
MCP_CAN CAN0(CAN_CS_PIN);

// Choose depending on your module crystal:
//   MCP_16MHZ for most shields
//   MCP_8MHZ for many cheap blue boards
const byte CAN_CLOCK = MCP_8MHZ;    // change to MCP_8MHZ if needed

// CAN bitrate and ID we listen to
const unsigned long SERVO_CAN_ID = 0x100;  // must match sender

// ----- String utils -----
int splitByChar(String in, char c, String* out) {
  int start = 0;
  int end;
  int count = 0;

  while (true) {
    end = in.indexOf(c, start);

    if (end == -1) {
      out[count++] = in.substring(start);
      break;
    }

    out[count++] = in.substring(start, end);
    start = end + 1;
  }

  return count;
}

// Process a complete command like "M 0 150"
void processCommand(String command) {
  command.trim();  // remove any stray whitespace

  if (command.length() == 0) {
    return;
  }

  String argv[5];
  int argc = splitByChar(command, ' ', argv);

  // Expecting command like: MA 0 150
  // Motor Absolute sets absolute motor position
  if (argc >= 3 && argv[0] == "MA") {
    int motorIndex = argv[1].toInt();
    int theta      = argv[2].toInt();

    // Safety range check
    if (theta < min_safe_angles[motorIndex]) {
      // Serial.println("Refusing to move motor to " + String(theta));
    } 
    else if (theta > max_safe_angles[motorIndex]) {
      // Serial.println("Refusing to move motor to " + String(theta));
    }
    else {
      board1.setPWM(motorIndex, 0, angleToPulse(theta, motorIndex));
      currentPose[motorIndex] = theta;
    }
  }
  // Expecting command like: MR 0 150
  // Motor Relative sets relatove motor position, e.g. move by 10
  else if (argc >= 3 && argv[0] == "MR") {
    int motorIndex = argv[1].toInt();
    int delta      = argv[2].toInt();
    int targetPosition = constrain(currentPose[motorIndex] + delta, min_safe_angles[motorIndex], max_safe_angles[motorIndex]);

    if (targetPosition != currentPose[motorIndex]) {
      Serial.println("Moving motor " + String(argv[1]) + " to " + String(targetPosition));
      board1.setPWM(motorIndex, 0, angleToPulse(targetPosition, motorIndex));
      currentPose[motorIndex] = targetPosition;
    }
  }
  // Expecting command like: MR 0 150
  // Command to reset pose
  else if (argc == 1 && argv[0] == "R") {
  takeInitialPose();
  } else {
    // Serial.println("Received unknown or malformed command: " + command);
  }
}

void takeInitialPose() {
  for (int motorIndex = 0; motorIndex < 4; motorIndex++) {
    board1.setPWM(motorIndex, 0, angleToPulse(initial_pose[motorIndex], motorIndex));
    currentPose[motorIndex] = initial_pose[motorIndex];
    delay(500);
  }
}

void setup() {
  Serial.begin(9600);

  board1.begin();
  board1.setPWMFreq(60); // Servos operate at ~60 Hz

  takeInitialPose();

  while (!Serial) {
    ; // wait for serial port to connect (for native USB boards)
  }

  // ----- CAN init -----
  while (CAN0.begin(MCP_ANY, CAN_500KBPS, CAN_CLOCK) != CAN_OK) {
    Serial.println("CAN init failed, retrying...");
    delay(200);
  }
  CAN0.setMode(MCP_NORMAL);
  Serial.println("CAN init OK");
}

void loop() {
  // ----- UART handling -----
  String uartCommand = handleUartComm();
  if (uartCommand != "") {
    //Serial.println(uartCommand);
    processCommand(uartCommand);
  }

  // ----- CAN handling -----
  // Check if a CAN message is available
  if (CAN_MSGAVAIL == CAN0.checkReceive()) {
    unsigned long canId;
    byte len;
    byte buf[8];

    // Read CAN frame
    CAN0.readMsgBuf(&canId, &len, buf);

    // Only accept frames with the expected ID (optional but recommended)
    if (canId == SERVO_CAN_ID) {
      // Interpret payload as ASCII string, e.g. "M 0 150"
      String canCommand = "";

      for (byte i = 0; i < len; i++) {
        canCommand += (char)buf[i];
      }

      Serial.println(canCommand);
      processCommand(canCommand);
    }
  }

  // You can add a small delay if you want to chill the loop a bit
  // delay(1);
}