#include "process_can.h"


static CAN_message_t CAN_TX_msg;
static CAN_message_t CAN_inMsg;


template <typename T>
void send_can_with_id_crc(uint8_t id, uint8_t message_type, T* data) {
  // Create CAN message
  CAN_message_t msg_l;
  msg_l.id = id;
  // msg_l.len = 8;  // Protocol-defined message length
  memcpy(&msg_l.buf[0], &message_type, sizeof(uint8_t));
  memcpy(&msg_l.buf[1], data, sizeof(T));

  // Prepare CRC calculation buffer (ID + data)
  uint8_t crc_data[CAN_MSG_MAX_LEN];

  // Copy message ID 
  memcpy(crc_data, (uint8_t*)&msg_l.id, sizeof(T));
  // Copy all data bytes
  memcpy(crc_data + 1, msg_l.buf, 6);

  // Calculate CRC  
  uint16_t crc_value = validate_crc16(crc_data, CAN_MSG_MAX_LEN);
 
  // Insert CRC into buffer
//   memcpy(&msg_l.buf[6], &crc_value, sizeof(uint16_t));
    msg_l.buf[6] = crc_value & 0xFF;
    msg_l.buf[7] = (crc_value >> 8) & 0xFF;

  // Send message
  Can.write(msg_l);
}

void send_velocity() {  
  float current_velocity = motor.shaftVelocity();  
  if (flash_rec == nullptr) {  // Null check  
      // Error handling: logging, alerts, etc.  
      return;
  }
  float value = flash_rec[vel].value;
  uint8_t id = flash_rec[addr_id].value;
  send_can_with_id_crc(id,'V',&value);
}

void send_angle() {
  float current_angle = motor.shaftAngle();
  if (flash_rec == nullptr) {  // Null check
      // Error handling: logging, alerts, etc.
      return;
  }
  uint8_t id = flash_rec[addr_id].value;
  send_can_with_id_crc(id,'A',&current_angle);
}


void send_motor_enabled() {
  /* Firmware data reading */
  if (flash_rec == nullptr) {  // Null check
      // Error handling: logging, alerts, etc.
      return;
  }
  uint8_t value = motor_control_inputs.motor_enabled; //copy current motor state
  uint8_t id = flash_rec[addr_id].value;
  send_can_with_id_crc(id,'M',&value);
}

void send_id() {
  /* Firmware data reading */
  if (flash_rec == nullptr) {  // Null check
      // Error handling: logging, alerts, etc.
      return;
  }

  uint8_t id = flash_rec[addr_id].value;
  send_can_with_id_crc(id,'I',&id);
}

// void send_motor_torque() {
//   float i_q = motor.current.q;  // Q-axis current (A)
//   float torque = kt * i_q;      // Torque calculation
//   torque *= 100;
//   CAN_TX_msg.id = flash_rec->value;
//   CAN_TX_msg.buf[0] = 'T';
//   CAN_TX_msg.len = 5;
//   memcpy(&CAN_TX_msg.buf[1], &torque, sizeof(torque));
//   Can.write(CAN_TX_msg);
// }

void send_pid_angle(uint8_t param_pid){
  if (flash_rec == nullptr) {  // Null check
    return;
  }
  uint8_t id = flash_rec[addr_id].value;
  conv_float_to_int.i = flash_rec[param_pid].value;
  uint32_t data = conv_float_to_int.i;
  switch(param_pid){
    case pid_p:
      param_pid = REG_MOTOR_POSPID_Kp;
      break;

    case pid_i:
      param_pid = REG_MOTOR_POSPID_Ki;
      break;

    case pid_d:
      param_pid = REG_MOTOR_POSPID_Kd;
      break;
  }

  send_can_with_id_crc(id,param_pid,&data);
}

void setup_id(uint8_t my_id) {
  write_param(addr_id,my_id);
}

void firmware_update(){
  write_param(firmw,FIRMWARE_FLAG);
  NVIC_SystemReset();
}

void setup_angle(float target_angle) {
  motor.enable();  // Enable motor if disabled
  motor.controller = MotionControlType::angle;
  motor.move(target_angle);
}

// void setup_pid_angle(uint8_t param_pid, uint32_t data){
//   conv_float_to_int.f = data;
//   switch (param_pid) {
//     case pid_p:
//       motor.P_angle.P = conv_float_to_int.f;
//       break;
//     case pid_i:
//       motor.P_angle.I = conv_float_to_int.f;
//       break;
//     case pid_d:
//       motor.P_angle.D = conv_float_to_int.f;
//       break;
//     default:
//       break;
//   }

//   write_param(param_pid,data);
// }


void listen_can(const CAN_message_t &msg) {
  msg_id = msg.id;
  msg_ch = msg_id & 0xF;        // Extract message channel
  uint16_t id_x = (msg_id >> 4) & 0x7FF; // Extract device address

  /* CRC Calculation */
  uint16_t received_crc = (msg.buf[msg.len - 2]) | (msg.buf[msg.len - 1] << 8);
  uint8_t data[10] = {0}; // Message buffer for CRC verification

  // Copy message ID (2 bytes)
  memcpy(data, (uint8_t*)&msg_id, sizeof(msg_id));
  // Copy message data (excluding CRC bytes)
  memcpy(data + sizeof(msg_id), msg.buf, msg.len - 2);
  
  // Calculate CRC
  uint16_t calculated_crc = validate_crc16(data, sizeof(msg_id) + msg.len - 2);
  
  // Verify CRC match
  if (calculated_crc != received_crc) {
    return; // Ignore message on CRC mismatch
  }
  flash_rec = load_params();
  /* Message Structure: 0x691
     69 - Device address
     1 - Action code */
  if(id_x == flash_rec[addr_id].value){
    if(msg_ch == REG_WRITE){
      switch(msg.buf[0]) {
        case REG_ID:
          setup_id(msg.buf[1]); 
          break;
        case REG_LED_BLINK:
          for (int i = 0; i < 10; i++) {
            GPIOC->ODR ^= GPIO_ODR_OD10;
            delay(100);
          }
          break;
          
        case MOTOR_ANGLE:
          memcpy(&motor_control_inputs.target_angle, &msg.buf[1],
                sizeof(motor_control_inputs.target_angle));
          setup_angle(motor_control_inputs.target_angle);
          break;

        case REG_MOTOR_POSPID_Kp:
          memcpy(&motor.P_angle.P, &msg.buf[1], sizeof(float));
          conv_float_to_int.f = motor.P_angle.P;
          write_param(pid_p,conv_float_to_int.i);
          break;

        case REG_MOTOR_POSPID_Ki:
          memcpy(&motor.P_angle.I, &msg.buf[1], sizeof(float));
          conv_float_to_int.f = motor.P_angle.I;
          write_param(pid_i,conv_float_to_int.i);
          break;

        case REG_MOTOR_POSPID_Kd:
          memcpy(&motor.P_angle.D, &msg.buf[1], sizeof(float));
          conv_float_to_int.f = motor.P_angle.D;
          write_param(pid_d,conv_float_to_int.i);
          break;
        
        case FIRMWARE_UPDATE:
          firmware_update();
          break;

        case MOTOR_ENABLED:
          if (msg.buf[1] == 1) {
            motor.enable();
            motor_control_inputs.motor_enabled = 1;
          } else {
            motor.disable();
            motor_control_inputs.motor_enabled = 0;
          }
        default:
          break;
      }
    }
    else if (msg_ch == REG_READ) {
      switch (msg.buf[0]) {
        case REG_ID: send_id(); break;
        case MOTOR_VELOCITY: send_velocity(); break;
        case MOTOR_ANGLE: send_angle(); break;
        case MOTOR_ENABLED: send_motor_enabled(); break;
        // case MOTOR_TORQUE: send_motor_torque(); break;
        // case FOC_STATE: send_foc_state(); break;
        case REG_MOTOR_POSPID_Kp: send_pid_angle(pid_p); break;
        case REG_MOTOR_POSPID_Ki: send_pid_angle(pid_i); break;
        case REG_MOTOR_POSPID_Kd: send_pid_angle(pid_d); break;
        default: break;
      }
    }
  }
}