#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include "mode_pwmp_hd.h"
#include "mode.h"
#include "entity.h"
#include "board.h"
#include "pdctrl.h"
#include "pid.h"
#include "convert.h"
#include "at_hc24.h"

mode_minor_t *mode_minor;

static execute_rsp_t *execute_dac_plan1(void); // 执行方案1
static execute_rsp_t *execute_dac_plan2(void); // 执行方案2
static execute_rsp_t *execute_dac_plan3(void); // 执行方案3
static execute_rsp_t *execute_dac_plan4(void); // 执行方案4

/*********************************** 控制任务 ***********************************/
// 算法方案初始化
void minor_pid_init(execute_plan_minor_e plan)
{
	switch (plan)
	{
	case EXECUTE_PLAN_1:
		break;
	case EXECUTE_PLAN_2:

		break;
	case EXECUTE_PLAN_3:

		break;
	case EXECUTE_PLAN_4:

	default:
		break;
	}
}

// 算法方案执行
void execute_dac(execute_plan_minor_e plan)
{
	switch (plan)
	{
	case EXECUTE_PLAN_1:
		execute_dac_plan1();
	case EXECUTE_PLAN_2:
		execute_dac_plan2();
	case EXECUTE_PLAN_3:
		execute_dac_plan3();
	case EXECUTE_PLAN_4:
		execute_dac_plan4();
	default:
		mode_minor->execute.code = EXECUTE_NONE;
	}
}

// 执行方案1
static execute_rsp_t *execute_dac_plan1(void)
{
	return 0;
}

// 执行方案2
static execute_rsp_t *execute_dac_plan2(void)
{
	return 0;
}

// 执行方案3
static execute_rsp_t *execute_dac_plan3(void)
{
	return 0;
}

// 执行方案4
static execute_rsp_t *execute_dac_plan4(void)
{
	return 0;
}

// 控制进程
static void mode_minor_control(void)
{
}

/*********************************** 整定任务 ***********************************/
// 同步整定参数
static void minor_public_params_update(void)
{
}

// 启动整定
static void minor_adjust_start(void)
{
	// 设置整定阶段
	mode_minor->adjust.step = MINOR_ADJUST_IDEL;
	// 使能整定
	mode_minor->adjust.enable = TRUE;
	// 初始化整定状态
	mode_minor->adjust_state = ADJUST_ONGING;
}

// 停止整定
static void minor_adjust_stop(void)
{
	// 输出最小值
	pdctrl_out(CON_MIN);
	// 清除整定任务
	osel_memset((uint8_t *)&mode_minor->adjust, 0, sizeof(minor_adjust_t));
	// 切换控制状态
	minor_process_state_set(MINOR_PROCESS_CONTROL);
}

// 整定进程
static void mode_minor_adjust(void)
{
	uint8_t ts = 0;

	if (mode_minor->adjust.enable != TRUE)
	{
		minor_adjust_start();
	}

	ts = mode_minor->adjust.execute_step;
	BIT_CLR(ts, BIT7);

	switch (ts)
	{
	default:
		minor_adjust_stop();
		break;
	}
}

/*********************************** 停止任务 ***********************************/
static void mode_minor_stop(void)
{
	minor_adjust_stop();
}

/*********************************** 模式管理 ***********************************/
// 设置任务
void minor_process_state_set(mode_minor_process_state_e state)
{
	if (mode_minor->process_state == state)
	{
		return;
	}
	// 如果当前正在整定，切换任务前要求停止整定
	if (mode_minor->process_state == MINOR_PROCESS_ADJUST)
	{
		minor_adjust_stop();
	}
	// 切换任务
	mode_minor->process_state = state;
}

// 模式进程
void mode_minor_process(void)
{
	switch (mode_minor->process_state)
	{
	case MINOR_PROCESS_CONTROL:
		mode_minor_control();
		break;
	case MINOR_PROCESS_ADJUST:
		mode_minor_adjust();
		break;
	case MINOR_PROCESS_STOP:
		mode_minor_stop();
		break;
	case MINOR_PROCESS_TEST:
	{
		pdctrl_out(mode_minor->test.output);
		mode_minor->test.travel_adc = adc_result_median(ADCS_1, ADC_PSB_CHANNEL);
		mode_minor->test.minor_adc = adc_result_median(ADCS_1, ADC_IPSB_CHANNEL);
		mode_minor->test.drive_current = ip2current();
		break;
	}
	default:
		break;
	}
}

// 模式初始化
void mode_minor_init(uint16_t positioner_model, mode_pwmp_hd_params_u *params, void (*params_save_cb)(void))
{
	DBG_ASSERT(params != NULL __DBG_LINE);
	DBG_ASSERT(params_save_cb != NULL __DBG_LINE);

	// 使能大电流
	VIP_H_EN_ENABLE();
	// 分配空间
	if (mode_minor == NULL)
	{
		mode_minor = (mode_minor_t *)osel_mem_alloc(sizeof(mode_minor_t));
	}
	osel_memset((uint8_t *)mode_minor, 0, sizeof(mode_minor_t));
	// 初始化滤波器
	mode_minor->target_lpf.handle = lpf_window_init(20);
	mode_minor->actual_lpf.handle = lpf_window_init(20);
	// 初始化控制器
	minor_pid_init(EXECUTE_PLAN);
	// 绑定存储参数
	mode_minor->save = params;
	mode_minor->params_save_cb = params_save_cb;
	// 保存模式参数
	mode_minor->params_save_cb();
	// 更新公共参数
	minor_public_params_update();
	// 初始化任务
	minor_process_state_set(MINOR_PROCESS_CONTROL);
}

// 模式反初始化
void mode_minor_dinit(void)
{
	if (mode_minor != NULL)
	{
		minor_process_state_set(MINOR_PROCESS_STOP);
		osel_mem_free(mode_minor);
		mode_minor = NULL;
	}
}