#include "libs/utils.h"
#include "prot/can_message.h"
#include "prot/can_foc_msg.h"
#include "foc/commands.h"
#include "foc/core/PMSM_FOC_Core.h"

void can_report_speed(u8 can, s16 rpm) {
	u8 data[6];
	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Speed));
	encode_s16(data + 2, rpm);
	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
}

void can_report_power(u8 can, s16 rpm, float vDC, float iDC) {
	u8 data[8];
	s16 v = (s16)(vDC * 10.0f);
	s16 i = (s16)(iDC * 10.0f);
	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Power));
	encode_s16(data + 2, rpm);
	encode_s16(data + 4, v);
	encode_s16(data + 6, i);
	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
}


void can_report_phase_current(u8 can, float iA, float iB, float iC) {
	u8 data[14];
	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Phase_Current));
	encode_float(data + 2, iA);
	encode_float(data + 6, iB);
	encode_float(data + 10, iC);
	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
}

void can_report_phase_voltage(u8 can, float vA, float vB, float vC) {
	u8 data[14];
	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Phase_Vol));
	encode_float(data + 2, vA);
	encode_float(data + 6, vB);
	encode_float(data + 10, vC);
	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);

}

void can_report_dq_current(u8 can, float id, float iq) {
	u8 data[10];
	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Dq_Current));
	encode_float(data + 2, id);
	encode_float(data + 6, iq);
	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
}

void can_response_hall_offset(u8 can, int offset) {
	u8 data[7];
	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Cali_Hall_Offset));
	encode_u8(data + 2, 1);
	encode_u32(data + 3, offset);
	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
}

void can_report_pid_value(u8 can, u8 id) {
	float kp, ki, kb;
	PMSM_FOC_GetPid(id, &kp, &ki, &kb);
	u8 data[15];
	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Pid));
	data[2] = id;
	encode_float(data + 3, kp);
	encode_float(data + 7, ki);
	encode_float(data + 11, kb);
	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
}