#include "libs/utils.h"
#include "prot/can_message.h"
#include "prot/can_foc_msg.h"
#include "foc/foc_cmd.h"

void can_report_speed(int can, int rpm) {
	u8 data[6];
	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Speed));
	encode_u32(data + 2, rpm);
	can_send_message(get_request_can_id(can, 1), data, sizeof(data), 100);
}

void can_report_phase_current(int can, int iA, int iB, int iC) {
	u8 data[14];
	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Phase_Current));
	encode_u32(data + 2, iA);
	encode_u32(data + 6, iB);
	encode_u32(data + 10, iC);
	can_send_message(get_request_can_id(can, 1), data, sizeof(data), 100);
}

void can_report_phase_voltage(int can, int vA, int vB, int vC) {
	u8 data[14];
	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Phase_Vol));
	encode_u32(data + 2, vA);
	encode_u32(data + 6, vB);
	encode_u32(data + 10, vC);
	can_send_message(get_request_can_id(can, 1), data, sizeof(data), 100);

}

void can_report_dq_current(int can, int id, int iq) {
	u8 data[10];
	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Dq_Current));
	encode_u32(data + 2, id);
	encode_u32(data + 6, iq);
	can_send_message(get_request_can_id(can, 1), data, sizeof(data), 100);
}

void can_response_hall_offset(int 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_reponse_can_id(can), data, sizeof(data), 100);
}