#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/controller.h"
#include "foc/core/foc_observer.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)mot_contrl_get_speed(&motor.controller);
	float vDC = get_vbus_float();
	float iDC = mot_contrl_get_dc_current(&motor.controller);	
	s16 v = S16Q5(vDC);
	s16 i = S16Q5(iDC);
	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[8];
	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Phase_Current));
	encode_s16(data + 2, S16Q5(foc()->in.curr_abc[0]));
	encode_s16(data + 4, S16Q5(foc()->in.curr_abc[1]));
	encode_s16(data + 6, S16Q5(foc()->in.curr_abc[2]));
	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
}

void can_report_phase_voltage(u8 can) {
	u8 data[8];
	float s_vABC[3];
	get_uvw_phases_raw(s_vABC);
	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Phase_Vol));
	encode_s16(data + 2, S16Q5(s_vABC[0]));
	encode_s16(data + 4, S16Q5(s_vABC[1]));
	encode_s16(data + 6, S16Q5(s_vABC[2]));
	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
}

void can_report_dq_current(u8 can) {
	u8 data[6];
	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Dq_Current));
	float id = motor.controller.out_idq_filterd.d;
	float iq = motor.controller.out_idq_filterd.q;
	encode_s16(data + 2, S16Q5(id));
	encode_s16(data + 4, S16Q5(iq));
	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
}

void can_report_dq_voltage(u8 can) {
	u8 data[6];
	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Dq_Vol));
	float id = foc()->out.vol_dq.d;
	float iq = foc()->out.vol_dq.q;
	encode_s16(data + 2, S16Q5(id));
	encode_s16(data + 4, S16Q5(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_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_raw());
	encode_s16(data + 9, get_mos_temp_raw());
	encode_u24(data + 11, shark_get_seconds());
	encode_u8(data + 14, mc_get_gear());
	encode_u8(data + 15, motor.mos_lim | (motor.motor_lim<<4));
	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
}

/* 采用组播方式上报,目的7E */
void can_mcast_foc_status2(void) {
	u8 data[8];
	encode_u16(data, mc_get_running_status2());
	encode_u16(data + 2, ABS((s16)(mot_contrl_get_speed(&motor.controller))));
	float vDC = get_vbus_float();
	encode_s16(data + 4, (s16)(vDC*10));
	float iDC = mot_contrl_get_dc_current(&motor.controller);
	if (!mot_contrl_is_start(&motor.controller)) {
		iDC = 0;
	}
	encode_s16(data + 6, (s16)(iDC*10));
	can_send_message(get_indicator_can_id(0x7F), data, sizeof(data), 0);
}

void can_report_mpta_values(u8 can) {
	u8 data[8];
	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_MTPA_DQ_Angle));
	encode_s16(data + 2, S16Q5(mot_contrl_get_current_vector(&motor.controller)));
	encode_s16(data + 4, S16Q5(motor.controller.out_idq_filterd.d));
	encode_s16(data + 6, S16Q5(motor.controller.out_idq_filterd.q));
	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] |= (mc_is_cruise_enabled()?1:0) << 3;
	data[1] |= mc_get_gear() << 6;
	encode_s16(data + 2, ABS((s16)(mot_contrl_get_speed(&motor.controller)/4.0f)));
	float vDC = get_vbus_float();
	encode_s16(data + 4, (s16)(vDC*10));
	float iDC = mot_contrl_get_dc_current(&motor.controller);
	if (!mot_contrl_is_start(&motor.controller)) {
		iDC = 0;
	}
	encode_s16(data + 6, (s16)(iDC*10));
	shark_can0_send_ext_message(0x1A014D43, data, sizeof(data));
}

void can_report_mot_params_ested(float v, u8 type) {
	u8 data[8];
	encoder_can_key(data, CMD_2_CAN_KEY(Foc_MotPara_Report));
	encode_u8(data + 2, type);
	encode_float(data + 3, v);
	can_send_message(get_indicator_can_id(0x45), data, sizeof(data), 0);
}

void can_response_vols(u8 can, u8 key) {
	u8 data[16];
	int len = 0;
	encoder_can_key(data, CMD_2_CAN_KEY(key));
	len += 2;
	encode_s16(data + len, S16Q5(get_acc_vol()));
	len += 2;
	encode_s16(data + len, S16Q5(get_vbus_float()));
	len += 2;
	encode_s16(data + len, S16Q5(get_throttle_float()));
	len += 2;
	encode_s16(data + len, S16Q5(get_thro_5v_float()));
	len += 2;
	encode_s16(data + len, S16Q5(get_throttle2_float()));
	len += 2;
	encode_s16(data + len, S16Q5(get_thro2_5v_float()));
	len += 2;
	can_send_response(can, data, len);
}


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);
}