#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, ABS(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 = mc_get_critical_error();
	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);
}

void can_report_ext_status(u8 can) {
	u8 data[8] = {0x0, 0x0, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
	data[0] = (get_vbus_int()>60?1:0) << 5;
	data[0] |= (mc_is_epm()?1:0) << 6;
	data[0] |= (mc_is_epm()?1:0) << 7;
	data[1] = mc_is_start()?0:1;
	data[1] |= (PMSM_FOC_Is_CruiseEnabled()?1:0) << 3;
	data[1] |= mc_get_gear() << 6;
	encode_s16(data + 2, ABS((s16)(motor_encoder_get_speed()/4.0f)));
	float vDC = get_vbus_float();
	encode_s16(data + 4, (s16)(vDC*10));
	float iDC = PMSM_FOC_GetVbusCurrent();
	encode_s16(data + 6, (s16)(iDC*10));
	shark_can0_send_ext_message(0x1A014D43, data, sizeof(data));
}

void can_report_plot_values(u8 can) {
    s16 trq_ref = eCtrl_get_RefTorque() * 10;
	s16 real_ref = PMSM_FOC_Get_Real_Torque() * 10;
	can_plot2(trq_ref, real_ref);
}

void can_plot1(s16 v1) {
	u8 data[4];
	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Plot));
	encode_s16(data + 2, v1);
	can_send_message(get_indicator_can_id(0x45), data, sizeof(data), 0);
}

void can_plot2(s16 v1, s16 v2) {
	u8 data[6];
	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Plot));
	encode_s16(data + 2, v1);
	encode_s16(data + 4, v2);
	can_send_message(get_indicator_can_id(0x45), data, sizeof(data), 0);
}

void can_plot3(s16 v1, s16 v2, s16 v3) {
	u8 data[8];
	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Plot));
	encode_s16(data + 2, v1);
	encode_s16(data + 4, v2);
	encode_s16(data + 6, v3);
	can_send_message(get_indicator_can_id(0x45), data, sizeof(data), 0);
}