#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 "foc/motor/motor.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 phase_adc_start = false;
static bool start_pwm_adc(void) {
	if (phase_adc_start) {
		return true;
	}
	pwm_turn_on_low_side();
	delay_ms(10);
	phase_current_offset_calibrate();
	pwm_start();
	delay_us(10);
	get_motor()->b_start = true;
	adc_start_convert();
	phase_current_calibrate_wait();
	if (phase_curr_offset_check()) {
		stop_pwm_adc();
		return false;
	}
	phase_adc_start = true;
	return true;
}

static void stop_pwm_adc(void) {
	if (!phase_adc_start) {
		return;
	}
	phase_adc_start = false;
	u32 mask = cpu_enter_critical();
	adc_stop_convert();
	pwm_stop();
	pwm_up_enable(true);
	get_motor()->b_start = false;
	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_is_running()) {
				response[2] = 1;
				break;
			}
			u8 item = decode_u8(can_message->data);
			if (item == 1) { //3相驱动测试
				u8 duty = decode_u8((u8 *)can_message->data + 1);
				if (duty != 0) {
					pwm_3phase_test();
					pwm_start();
					u16 duty_time = (u16)((float)duty * FOC_PWM_Half_Period / 100.0f);
					pwm_update_duty(duty_time, duty_time, duty_time);
				}else {
					pwm_stop();
					pwm_3phase_init();
				}
				sys_debug("phase test duty %d\n", duty);
			}else if (item == 2) {//获取所有电压的采集值
				can_response_vols(can_message->src, can_message->key);
				return;
			}else if (item == 3) { //读取gpio状态
				encode_u16(response + 3, gpio_get_pin_values());
				rsplen += 2;
			}else if (item == 4) { // u phase detect
				int count = 200;
				float uvw[3] = {0, 0, 0};
				s16 uvw_total[3] = {0, 0, 0};
				u8 detect = decode_u8((u8 *)can_message->data + 1);
				gpio_phase_u_detect(detect?true:false);
				delay_ms(50);
				while(count-- > 0) {
					delay_us(100);
					get_uvw_phases_raw(uvw);
					uvw_total[0] += S16Q5(uvw[0]);
					uvw_total[1] += S16Q5(uvw[1]);
					uvw_total[2] += S16Q5(uvw[2]);
				}
				encode_s16(response + 3, uvw_total[0]/200);
				encode_s16(response + 5, uvw_total[1]/200);
				encode_s16(response + 7, uvw_total[2]/200);
				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 == 1) {
					pwm_3phase_test(); //use pwm output, disable timer break in
					if (!start_pwm_adc()) {
						response[2] = 1;
						break;
					}
				}else if (start == 0){
					stop_pwm_adc();
					pwm_3phase_init();
				}else {
					s16 ia = S16Q5(foc()->in.curr_abc[0]);
					s16 ib = S16Q5(foc()->in.curr_abc[1]);
					s16 ic = S16Q5(foc()->in.curr_abc[2]);
					encode_s16(response + 3, ia);
					encode_s16(response + 5, ib);
					encode_s16(response + 7, ic);
					rsplen += 6;
				}
			}
			break;
		}
		default:
			rsplen = 0;
			break;
	}
	if (rsplen > 0) {
		can_send_response(can_message->src, response, rsplen);
	}
}

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