#include "VESC_controller.h"
extern mbed::CAN can1; 

int32_t VESC_controller::setDutyCycle(int32_t duty) {
  mbed::CANMessage canMsg;
  // VESC uses extended message IDs
  // The message ID is the combination of the command and the VESC ID
  // Set duty cycle = 0x0000, vescID = 0x5A for example, than
  // combined ID = 0x0000005A
  canMsg.id      = 0x000000000 | vescId;  
  canMsg.format  = CANExtended;
  canMsg.len     = 4;       // Message length
  // Convert duty cycle to 4 bytes
  // 32-bit big endian signed number
  canMsg.data[3] = (duty >> 0) & 0xFF;
  canMsg.data[2] = (duty >> 8) & 0xFF;
  canMsg.data[1] = (duty >> 16) & 0xFF;
  canMsg.data[0] = (duty >> 24) & 0xFF;

  // Send message on CAN bus
  can1.write(canMsg);

  current_duty_cycle = duty;
  return current_duty_cycle;
}

void VESC_controller::getStatus1(const mbed::CANMessage &canMsg) {
  // For debug:
  // // Serial.print("CAN Message Data: ");
  // for (int i = 0; i < canMsg.len; i++) {
  //   if (canMsg.data[i] < 0x10) // Serial.print("0"); // Print leading zero for single-digit hex
  //   // Serial.print(canMsg.data[i], HEX);
  //   // Serial.print(" ");
  // }
  // // Serial.println();

  // Extract Current (bytes 4-5)
  int16_t current_raw = (canMsg.data[4] << 8) | canMsg.data[5];
  float current = current_raw / 10.0f; // Scale by 10

  // Extract Duty Cycle (bytes 6-7)
  int16_t duty_raw = (canMsg.data[6] << 8) | canMsg.data[7];
  float duty = duty_raw / 1000.0f; // Scale by 1000

  // Print results
  // Serial.print("Current: ");
  // Serial.print(current);
  readout_motor_current = current;
  // Serial.print(" A, Duty: ");
  // Serial.print(duty*100);
  readout_duty_cycle = duty*100;
  // Serial.println("%");
}

bool VESC_controller::verifyDutyCycle(){
    return readout_duty_cycle == current_duty_cycle;
}

void VESC_controller::getDataFromVesc(){
  getStatus1(lastCanMessage);
}

uint8_t VESC_controller::getVescID(){

  return vescId;
}