#include "libs/utils.h"
#include "prot/can_message.h"
#include "prot/can_foc_msg.h"
#include "foc/commands.h"
#include "foc/samples.h"
#include "foc/motor/motor.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) {
	u8 data[8];
	s16 rpm = (s16)motor_encoder_get_speed();
	float vDC = get_vbus_float();
	float iDC = PMSM_FOC_GetVbusCurrent();	
	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) {
	u8 data[14];
	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Phase_Current));
	encode_float(data + 2, PMSM_FOC_Get()->in.s_iABCFilter[0]);
	encode_float(data + 6, PMSM_FOC_Get()->in.s_iABCFilter[1]);
	encode_float(data + 10, PMSM_FOC_Get()->in.s_iABCFilter[2]);
	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
}

void can_report_phase_voltage(u8 can) {
	u8 data[14];
	float s_vABC[3];
	get_phase_vols(s_vABC);
	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Phase_Vol));
	encode_float(data + 2, s_vABC[0]);
	encode_float(data + 6, s_vABC[1]);
	encode_float(data + 10, s_vABC[2]);
	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
}

void can_report_dq_current(u8 can) {
	u8 data[10];
	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Dq_Current));
	float id = PMSM_FOC_GetDQCurrent()->d;
	float iq = PMSM_FOC_GetDQCurrent()->q;
	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);
}

void can_report_foc_status(u8 can) {
	u8 data[16];
	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Status));
	mc_get_running_status(data+2);
	u32 errMask = PMSM_FOC_GetCriticalError();
	encode_u32(data + 3, errMask);
	encode_s16(data + 7, get_motor_temp());
	encode_s16(data + 9, get_mos_temp());
	encode_u32(data + 11, shark_get_seconds());
	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
}


void can_report_mpta_values(u8 can) {
	u8 data[8];
	if (!PMSM_FOC_Get()->in.b_MTPA_calibrate) {
		return;
	}
	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_MTPA_DQ_Angle));
	encode_s16(data + 2, (s16)(PMSM_FOC_Get()->in.s_targetCurrent * 10.0f));
	encode_s16(data + 4, (s16)(PMSM_FOC_Get()->in.s_targetIdq.d * 10.0f));
	encode_s16(data + 6, (s16)(PMSM_FOC_Get()->in.s_targetIdq.q * 10.0f));
	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
}