#include "factory.h"
#include "bsp/bsp_driver.h"
#include "prot/can_message.h"
#include "prot/can_foc_msg.h"
#include "foc/samples.h"
#include "foc/motor/current.h"
#include "libs/utils.h"
#include "libs/logger.h"
#include "os/os_task.h"

static u8 factory_mode = 0;

static void stop_pwm_adc(void);

static bool start_pwm_adc(void) {
	pwm_turn_on_low_side();
	delay_ms(10);
	phase_current_offset_calibrate();
	pwm_start();
	delay_us(10); //wait for ebrake error
	adc_start_convert();
	phase_current_calibrate_wait();
	if (phase_curr_offset_check()) {
		stop_pwm_adc();
		return false;
	}
	return true;
}

static void stop_pwm_adc(void) {
	u32 mask = cpu_enter_critical();
	adc_stop_convert();
	pwm_stop();
	pwm_up_enable(true);
	cpu_exit_critical(mask);
}

void can_process_factory_message(can_message_t *can_message){
	uint8_t response[32];
	uint8_t rsplen;
	encoder_can_key(response, can_message->key);
	response[2] = 0;
	rsplen = 3;
	switch(can_message->key) {
		case BUILD_CMD_KEY(0xE0):
			factory_mode = decode_u8(can_message->data);
			break;
		case BUILD_CMD_KEY(0xE1):
		{
			if (factory_mode == 0) {
				response[2] = 1;
				break;
			}
			u8 item = decode_u8(can_message->data);
			if (item == 1) { //3相驱动测试
				u8 mask = decode_u8((u8 *)can_message->data + 1);
				pwm_3phase_sides(false, mask);
			}else if (item == 2) {//获取所有电压的采集值
				can_response_vols(can_message->src, can_message->key);
				return;
			}else if (item == 3) { //读取gpio状态
				encode_u16(response + 3, GPIOA_VALUE);
				encode_u16(response + 5, GPIOB_VALUE);
				encode_u16(response + 7, GPIOC_VALUE);
				encode_u16(response + 9, GPIOD_VALUE);
				encode_u16(response + 11, GPIOE_VALUE);
				rsplen += 10;
			}else if (item == 4) { // u phase detect
				int count = 20;
				float uvw[3] = {0, 0, 0};
				s16 uvw_total[3] = {0, 0, 0};
				if (can_message->len < 2) {
					response[2] = 2; //长度错误
					break;
				}
				u8 detect = decode_u8((u8 *)can_message->data + 1);
				gpio_phase_u_detect(detect?true:false);
				while(count-- > 0) {
					delay_us(100);
					get_uvw_phases_raw(uvw);
					uvw_total[0] += (s16)(uvw[0] * 100);
					uvw_total[1] += (s16)(uvw[1] * 100);
					uvw_total[2] += (s16)(uvw[2] * 100);
				}
				encode_u16(response + 3, uvw_total[0]/20);
				encode_u16(response + 5, uvw_total[1]/20);
				encode_u16(response + 7, uvw_total[2]/20);
				gpio_phase_u_detect(false);
				rsplen += 6;
			}else if (item == 5) { //phase current test
				u8 start = decode_u8((u8 *)can_message->data + 1);
				if (start) {
					pwm_3phase_sides(true, 0); //use pwm output, disable timer break in
					if (!start_pwm_adc()) {
						response[2] = 1;
						break;
					}
				}else {
					stop_pwm_adc();
					pwm_3phase_init();
				}
			}
			break;
		}
		default:
			rsplen = 0;
			break;
	}
	if (rsplen > 0) {
		can_send_response(can_message->src, response, rsplen);
	}
}

u8 factory_get_mode(void) {
	return factory_mode;
}

bool factory_is_running(void) {
	return (factory_mode == 0);
}