#include "app/nv_storage.h"
#include "bsp/bsp_driver.h"
#include "libs/crc16.h"
#include "libs/logger.h"
#include "foc/motor/motor_param.h"
#include "foc/foc_config.h"

#define motorParam_idx_0 (sn_page_index + 1)
#define motorParam_idx_1 (motorParam_idx_0 + 1)
#define focParam_idx_0 (motorParam_idx_1 + 1)
#define focParam_idx_1 (focParam_idx_0 + 1)

#define trq_Tbl_size (4)
#define trq_Tbl_idx0 (focParam_idx_1 + 1)
#define trq_Tbl_idx1 (trq_Tbl_idx0 + trq_Tbl_size)

#define gear_config_idx_0 (trq_Tbl_idx1 + trq_Tbl_size)
#define gear_config_idx_1 (gear_config_idx_0 + 1)

#define sn_idx_back       (gear_config_idx_1 + 1)

#define limiter_cfg_idx_0 (sn_idx_back + 1)
#define limiter_cfg_idx_1 (limiter_cfg_idx_0 + 1)

#define mc_runtime_red_idx (limiter_cfg_idx_1 + 1)
#define mc_crit_err_idx (mc_runtime_red_idx + 1)

static motor_params_t m_params;
static foc_params_t   foc_params;
static mc_gear_config_t gear_config;
static mc_limit_t limiter;
#define NV_MAGIC 0x5AA5

motor_params_t *nv_get_motor_params(void) {
	return &m_params;
}
foc_params_t *nv_get_foc_params(void) {
	return &foc_params;
}

mc_gear_config_t *nv_get_gear_configs(void) {
	return &gear_config;
}

mc_limit_t *nv_get_limter(void) {
	return &limiter;
}

void nv_save_hall_table(s32 *hall_table) {
//	memcpy((char *)m_params.hall_table, (char *)hall_table, sizeof(m_params.hall_table));
	nv_save_motor_params();
}

void nv_save_angle_offset(float offset) {
	m_params.offset = offset;
	nv_save_motor_params();
}


static void nv_default_motor_params(void) {
	m_params.mot_nr = MOTOR_NR;
	m_params.poles = MOTOR_POLES;
	m_params.r = MOTOR_R;
	m_params.ld = MOTOR_Ld;
	m_params.lq = MOTOR_Lq;
#ifdef MOTOR_Flux
	m_params.flux_linkage = MOTOR_Flux;
#else
	m_params.flux_linkage = 0;
#endif
	m_params.offset = MOTOR_ENC_OFFSET;
	m_params.est_pll_band = 200;
	m_params.epm_pll_band = 400;
	m_params.pos_lock_pll_band = 500;
	m_params.velocity_weight = 190;
	m_params.velocity_C = 145;
	m_params.gear_ratio = 6250;//6.25:1
}

static void nv_default_limter(void) {
	limiter.motor[0].enter_pointer = 120;
	limiter.motor[0].exit_pointer = 110;
	limiter.motor[0].limit_value = 0;
	limiter.motor[1].enter_pointer = 110;
	limiter.motor[1].exit_pointer = 97;
	limiter.motor[1].limit_value = 50;
	limiter.motor[2].enter_pointer = 97;
	limiter.motor[2].exit_pointer = 85;
	limiter.motor[2].limit_value = 80;

	limiter.mos[0].enter_pointer = 110;
	limiter.mos[0].exit_pointer = 100;
	limiter.mos[0].limit_value = 0;
	limiter.mos[1].enter_pointer = 100;
	limiter.mos[1].exit_pointer = 90;
	limiter.mos[1].limit_value = 60;
	limiter.mos[2].enter_pointer = 90;
	limiter.mos[2].exit_pointer = 80;
	limiter.mos[2].limit_value = 80;

	limiter.vbus.enter_pointer = 76;
	limiter.vbus.exit_pointer = 80;
	limiter.vbus.limit_value = 20;
}


static void nv_default_foc_params(void) {
	foc_params.s_maxDCVol = CONFIG_MAX_DC_VOL;
	foc_params.s_minDCVol = CONFIG_MIN_DC_VOL;
	foc_params.s_PhaseCurrLim = CONFIG_DEFAULT_PHASE_CURR_LIM;
	foc_params.s_maxRPM = CONFIG_DEFAULT_RPM_LIM;
	foc_params.s_maxEpmRPM = CONFIG_DEFAULT_EPM_RPM;
	foc_params.s_maxEpmTorqueLim = CONFIG_DEFAULT_EPM_PHASE_CURR;
	foc_params.s_maxTorque = CONFIG_MAX_MOTOR_TORQUE;
	foc_params.s_TorqueBrkLim = CONFIG_DEFAULT_EBRK_TORQUE;
	foc_params.n_currentBand = CONFIG_CURRENT_BANDWITH;
	foc_params.n_brkShutPower = CONFIG_BRK_SHUT_POWER_ENABLE;
	foc_params.s_LimitiDC = CONFIG_DEFAULT_IDC_LIM;
	foc_params.s_iDCeBrkLim = CONFIG_DEFAULT_EBRK_IDC_LIM;
	foc_params.n_startThroVol = CONFIG_THROTTLE_START_VALUE;
	foc_params.n_endThroVol = CONFIG_THROTTLE_END_VALUE;
	foc_params.n_autoHold = CONFIG_AUTOHOLD_ENABLE;
	foc_params.n_dec_time = CONFIG_DEC_TIME;
	foc_params.n_ebrk_time = CONFIG_EBRK_RAMP_TIME;
	foc_params.n_FwEnable  = CONFIG_DEFAULT_FW_ENABLE;
	foc_params.f_minThroVol = CONFIG_THROTTLE_MIN_VALUE;
	foc_params.f_maxThroVol = CONFIG_THROTTLE_MAX_VALUE;

	foc_params.s_maxEpmRPMBck = CONFIG_DEFAULT_EPM_BK_RPM;
	foc_params.s_maxEpmTorqueLimBck = CONFIG_DEFAULT_EPM_BK_PHASE_CURR;

	foc_params.pid_conf[PID_D_id].kp = (foc_params.n_currentBand * MOTOR_Ld);
	foc_params.pid_conf[PID_D_id].ki = (MOTOR_R/MOTOR_Ld);
	foc_params.pid_conf[PID_D_id].kd = 0;
	
	foc_params.pid_conf[PID_Q_id].kp = (foc_params.n_currentBand * MOTOR_Lq);
	foc_params.pid_conf[PID_Q_id].ki = (MOTOR_R/MOTOR_Lq);
	foc_params.pid_conf[PID_Q_id].kd = 0;

	foc_params.pid_conf[PID_TRQ_id].kp = PI_VEL_LIM_KP;
	foc_params.pid_conf[PID_TRQ_id].ki = PI_VEL_LIM_KI;
	foc_params.pid_conf[PID_TRQ_id].kd = PI_VEL_LIM_KD;

	foc_params.pid_conf[PID_Spd_id].kp = PI_VEL_KP;
	foc_params.pid_conf[PID_Spd_id].ki = PI_VEL_KI;
	foc_params.pid_conf[PID_Spd_id].kd = PI_VEL_KD;

	foc_params.pid_conf[PID_Pow_id].kp = 5.0f;
	foc_params.pid_conf[PID_Pow_id].ki = 15.0f;
	foc_params.pid_conf[PID_Pow_id].kd = 0;

	foc_params.pid_conf[PID_Lock_id].kp = (0.01f);
	foc_params.pid_conf[PID_Lock_id].ki = (0.20f);
	foc_params.pid_conf[PID_Lock_id].kd = 0;
#ifdef CONFIG_SPEED_LADRC
	foc_params.f_adrc_vel_lim_Wo = CONFIG_LADRC_Wo;
	foc_params.f_adrc_vel_lim_Wcv = CONFIG_LADRC_Wcv;
	foc_params.f_adrc_vel_lim_B0 = CONFIG_LADRC_B0;
	foc_params.f_adrc_vel_Wo = CONFIG_LADRC_Wo;
	foc_params.f_adrc_vel_Wcv = CONFIG_LADRC_Wcv;
	foc_params.f_adrc_vel_B0 = CONFIG_LADRC_B0;
#endif
}

static void nv_default_gear_config(void) {
	for (int i = 0; i < CONFIG_MAX_GEAR_NUM; i++) {
		gear_config.gears_48[i].n_max_speed = 4000;
		gear_config.gears_48[i].n_max_trq = CONFIG_MAX_MOTOR_TORQUE/2;
		gear_config.gears_48[i].n_max_idc = 60;
		gear_config.gears_48[i].n_zero_accl = 500;
		gear_config.gears_48[i].n_accl_time = 10;

		gear_config.gears_96[i].n_max_speed = 8000;
		gear_config.gears_96[i].n_max_trq = CONFIG_MAX_MOTOR_TORQUE;
		gear_config.gears_96[i].n_max_idc = 100;
		gear_config.gears_96[i].n_zero_accl = 500;
		gear_config.gears_96[i].n_accl_time = 10;

		for (int j = 0; j < GEAR_SPEED_TRQ_NUM; j++) {
			gear_config.gears_48[i].n_torque[j] = 60+j;
			gear_config.gears_96[i].n_torque[j] = 80+j;
		}
	}

}


void nv_write_crit_errblock(u8 *data, int len) {
	fmc_write_data(mc_crit_err_idx, data, len);
}

void nv_read_crit_errblock(u8 *data, int len) {
	fmc_read_data(mc_crit_err_idx, data, len);
}

void nv_write_runtime_block(u8 *data, int len) {
	fmc_write_data(mc_runtime_red_idx, data, len);
}

void nv_read_runtime_block(u8 *data, int len) {
	fmc_read_data(mc_runtime_red_idx, data, len);
}

void nv_save_motor_params(void) {
	u16 crc = crc16_get((u8 *)&m_params, sizeof(m_params) - 2);
	m_params.crc16 = crc;
	fmc_write_data(motorParam_idx_0, (u8 *)&m_params, sizeof(m_params));
	fmc_write_data(motorParam_idx_1, (u8 *)&m_params, sizeof(m_params));
}
void nv_read_motor_params(void) {
	fmc_read_data(motorParam_idx_0, (u8 *)&m_params, sizeof(m_params));
	u16 crc0 = crc16_get((u8 *)&m_params, sizeof(m_params) - 2);
	if (crc0 != m_params.crc16) {
		sys_debug("mp 0 error\n");
		fmc_read_data(motorParam_idx_1, (u8 *)&m_params, sizeof(m_params));
		crc0 = crc16_get((u8 *)&m_params, sizeof(m_params) - 2);
		if (crc0 != m_params.crc16) {
			nv_default_motor_params();
			nv_save_motor_params();
			return;
		}
		fmc_write_data(motorParam_idx_0, (u8 *)&m_params, sizeof(m_params));
	}else {
		fmc_read_data(motorParam_idx_1, (u8 *)&m_params, sizeof(m_params));
		crc0 = crc16_get((u8 *)&m_params, sizeof(m_params) - 2);
		if (crc0 != m_params.crc16) {
			sys_debug("mp 1 error\n");
			fmc_read_data(motorParam_idx_0, (u8 *)&m_params, sizeof(m_params));
			fmc_write_data(motorParam_idx_1, (u8 *)&m_params, sizeof(m_params));
		}
	}
}

bool nv_set_limit_config(u8 *config, int len) {
	int config_len = sizeof(nv_limter_t) * (2 * TEMP_LIMITER_NUM + 1);
	if (len < config_len) {
		return false;
	}
	memcpy(&limiter.motor[0], config, len);
	sys_debug("lim %d %d %d\n", limiter.motor[0].enter_pointer, limiter.motor[0].exit_pointer, limiter.motor[0].limit_value);
	nv_save_limit_config();
	return true;
}

void* nv_get_limit_config(int *len) {
	if (len) {
		*len = sizeof(nv_limter_t) * (2 * TEMP_LIMITER_NUM + 1);
	}
	return &limiter.motor[0];
}

void nv_save_limit_config(void) {
	limiter.magic = NV_MAGIC;
	u16 crc = crc16_get((u8 *)&limiter, sizeof(limiter) - 2);
	limiter.crc16 = crc;
	fmc_write_data(limiter_cfg_idx_0, (u8 *)&limiter, sizeof(limiter));
	fmc_write_data(limiter_cfg_idx_1, (u8 *)&limiter, sizeof(limiter));
}

void nv_read_limit_config(void) {
	fmc_read_data(limiter_cfg_idx_0, (u8 *)&limiter, sizeof(limiter));
	u16 crc16 = crc16_get((u8 *)&limiter, sizeof(limiter) - 2);
	if (limiter.magic != NV_MAGIC || limiter.crc16 != crc16) {
		sys_debug("lim 0 error\n");
		fmc_read_data(limiter_cfg_idx_1, (u8 *)&limiter, sizeof(limiter));
		crc16 = crc16_get((u8 *)&limiter, sizeof(limiter) - 2);
		if (limiter.magic == NV_MAGIC && limiter.crc16 == crc16) {
			fmc_write_data(limiter_cfg_idx_0, (u8 *)&limiter, sizeof(limiter));
			return;
		}else {
			sys_debug("lim 1 error\n");
			nv_default_limter();
			nv_save_limit_config();
		}
	}else {
		fmc_read_data(limiter_cfg_idx_1, (u8 *)&limiter, sizeof(limiter));
		crc16 = crc16_get((u8 *)&limiter, sizeof(limiter) - 2);
		if (limiter.magic != NV_MAGIC || limiter.crc16 != crc16) {
			sys_debug("lim 1 error\n");
			fmc_read_data(limiter_cfg_idx_0, (u8 *)&limiter, sizeof(limiter));
			fmc_write_data(limiter_cfg_idx_1, (u8 *)&limiter, sizeof(limiter));
		}
	}
}

void nv_save_foc_params(void) {
	u16 crc = crc16_get((u8 *)&foc_params, sizeof(foc_params) - 2);
	foc_params.crc16 = crc;
	fmc_write_data(focParam_idx_0, (u8 *)&foc_params, sizeof(foc_params));
	fmc_write_data(focParam_idx_1, (u8 *)&foc_params, sizeof(foc_params));
}

void nv_read_foc_params(void) {
	fmc_read_data(focParam_idx_0, (u8 *)&foc_params, sizeof(foc_params));
	u16 crc0 = crc16_get((u8 *)&foc_params, sizeof(foc_params) - 2);
	if (crc0 != foc_params.crc16) {
		sys_debug("fp 0 error\n");
		fmc_read_data(focParam_idx_1, (u8 *)&foc_params, sizeof(foc_params));
		crc0 = crc16_get((u8 *)&foc_params, sizeof(foc_params) - 2);
		if (crc0 != foc_params.crc16) {
			nv_default_foc_params();
			nv_save_foc_params();
			return;
		}
		fmc_write_data(focParam_idx_0, (u8 *)&foc_params, sizeof(foc_params));
	}else {
		fmc_read_data(focParam_idx_1, (u8 *)&foc_params, sizeof(foc_params));
		crc0 = crc16_get((u8 *)&foc_params, sizeof(foc_params) - 2);
		if (crc0 != foc_params.crc16) {
			sys_debug("fp 1 error\n");
			fmc_read_data(focParam_idx_0, (u8 *)&foc_params, sizeof(foc_params));
			fmc_write_data(focParam_idx_1, (u8 *)&foc_params, sizeof(foc_params));
		}
	}
	sys_debug("maxTorque=%f\n", foc_params.s_maxTorque);
}

void nv_save_gear_configs(void) {
	gear_config.magic = NV_MAGIC;
	u16 crc = crc16_get((u8 *)&gear_config, sizeof(gear_config) - 2);
	gear_config.crc16 = crc;
	fmc_write_data(gear_config_idx_0, (u8 *)&gear_config, sizeof(gear_config));
	fmc_write_data(gear_config_idx_1, (u8 *)&gear_config, sizeof(gear_config));
}

void nv_read_gear_configs(void) {
	fmc_read_data(gear_config_idx_0, (u8 *)&gear_config, sizeof(gear_config));
	u16 crc0 = crc16_get((u8 *)&gear_config, sizeof(gear_config) - 2);
	if (crc0 != gear_config.crc16) {
		sys_debug("gear 0 error\n");
		fmc_read_data(gear_config_idx_1, (u8 *)&gear_config, sizeof(gear_config));
		crc0 = crc16_get((u8 *)&gear_config, sizeof(gear_config) - 2);
		if (crc0 != gear_config.crc16) {
			sys_debug("default gear\n");
			nv_default_gear_config();
			nv_save_gear_configs();
			return;
		}
		fmc_write_data(gear_config_idx_0, (u8 *)&gear_config, sizeof(gear_config));
	}else {
		fmc_read_data(gear_config_idx_1, (u8 *)&gear_config, sizeof(gear_config));
		crc0 = crc16_get((u8 *)&gear_config, sizeof(gear_config) - 2);
		if (crc0 != gear_config.crc16) {
			sys_debug("gear 1 error\n");
			fmc_read_data(gear_config_idx_0, (u8 *)&gear_config, sizeof(gear_config));
			fmc_write_data(gear_config_idx_1, (u8 *)&gear_config, sizeof(gear_config));
		}
	}
}

#if 0
bool nv_set_gear_config(u8 mode4896, u8 gear, u16 rpm, u16 torque, u16 idc, u16 acc) {
	mc_gear_t *gear_cfg;
	if (gear >= CONFIG_MAX_GEAR_NUM) {
		return false;
	}

	if (mode4896 == 0) { //48vmode
		gear_cfg = &gear_config.gears_48[gear];
	}else {
		gear_cfg = &gear_config.gears_96[gear];
	}
	gear_cfg->u_maxRPM = rpm;
	gear_cfg->u_maxTorque = torque;
	gear_cfg->u_maxIdc = idc;
	gear_cfg->u_accTime = acc;

	nv_save_gear_configs();
	return true;
}

bool nv_get_gear_config(u8 mode4896, u8 gear, u16 *rpm, u16 *torque, u16 *idc, u16 *acc) {
	mc_gear_t *gear_cfg;
	if (gear >= CONFIG_MAX_GEAR_NUM) {
		return false;
	}

	if (mode4896 == 0) { //48vmode
		gear_cfg = &gear_config.gears_48[gear];
	}else {
		gear_cfg = &gear_config.gears_96[gear];
	}
	*rpm = gear_cfg->u_maxRPM;
	*torque = gear_cfg->u_maxTorque;
	*idc = gear_cfg->u_maxIdc;
	*acc = gear_cfg->u_accTime;

	return true;
}
#endif


bool nv_set_gear_config(u8 mode96, u8 *config, int len) {
	int config_len = sizeof(mc_gear_t) * CONFIG_MAX_GEAR_NUM;
	if (len < config_len) {
		return false;
	}
	if (mode96) {
		memcpy(&gear_config.gears_96[0], config, len);
	}else {
		memcpy(&gear_config.gears_48[0], config, len);
	}
	for (int i = 0; i < CONFIG_MAX_GEAR_NUM; i++) {
		if (gear_config.gears_48[i].n_max_speed > CONFIG_MAX_MOT_RPM) {
			gear_config.gears_48[i].n_max_speed = CONFIG_MAX_MOT_RPM;
		}

		if (gear_config.gears_48[i].n_max_trq > CONFIG_MAX_TORQUE) {
			gear_config.gears_48[i].n_max_trq = CONFIG_MAX_TORQUE;
		}

		if (gear_config.gears_48[i].n_max_idc > CONFIG_MAX_VBUS_CURRENT) {
			gear_config.gears_48[i].n_max_idc = CONFIG_MAX_VBUS_CURRENT;
		}
		if (gear_config.gears_48[i].n_zero_accl == 0) {
			gear_config.gears_48[i].n_zero_accl = 100;
		}
		if (gear_config.gears_48[i].n_accl_time == 0) {
			gear_config.gears_48[i].n_accl_time = 1;
		}

		if (gear_config.gears_96[i].n_max_speed > CONFIG_MAX_MOT_RPM) {
			gear_config.gears_96[i].n_max_speed = CONFIG_MAX_MOT_RPM;
		}

		if (gear_config.gears_96[i].n_max_trq > CONFIG_MAX_TORQUE) {
			gear_config.gears_96[i].n_max_trq = CONFIG_MAX_TORQUE;
		}

		if (gear_config.gears_96[i].n_max_idc > CONFIG_MAX_VBUS_CURRENT) {
			gear_config.gears_96[i].n_max_idc = CONFIG_MAX_VBUS_CURRENT;
		}
		if (gear_config.gears_96[i].n_zero_accl == 0) {
			gear_config.gears_96[i].n_zero_accl = 100;
		}
		if (gear_config.gears_96[i].n_accl_time == 0) {
			gear_config.gears_96[i].n_accl_time = 1;
		}
		for (int j = 0; j < GEAR_SPEED_TRQ_NUM; j++) {
			if (gear_config.gears_48[i].n_torque[j] > 100) {
				gear_config.gears_48[i].n_torque[j] = 100;
			}

			if (gear_config.gears_96[i].n_torque[j] > 100) {
				gear_config.gears_96[i].n_torque[j] = 100;
			}
		}
	}

	nv_save_gear_configs();
	return true;
}

void* nv_get_gear_config(u8 mode96, int *len) {
	if (len) {
		*len = sizeof(mc_gear_t) * CONFIG_MAX_GEAR_NUM;
	}
	sys_debug("gear0 %d %d\n", mode96, gear_config.gears_96[0].n_max_speed);
	if (mode96) {
		return &gear_config.gears_96[0];
	}else {
		return &gear_config.gears_48[0];
	}
}


void nv_set_pid(u8 id, pid_conf_t *pid) {
	foc_params.pid_conf[id] = *pid;
	nv_save_foc_params();
}

void nv_get_pid(u8 id, pid_conf_t *pid) {
	*pid = foc_params.pid_conf[id];
}

void nv_set_hwbrake_mode(u8 mode) {
	foc_params.n_brkShutPower = mode;
}

void nv_set_throttle_vol(float min, float max) {
	foc_params.n_startThroVol = min;
	foc_params.n_endThroVol = max;
}


int nv_write_sn(u8 *data, int len) {
	mc_sn_t sn;
	memset(&sn, 0, sizeof(sn));
	len = min(32, len);
	memcpy(sn.sn, data, len);
	sn.len = len;
	sn.crc = crc16_get(data, len);
	fmc_write_data(sn_page_index, (u8 *)&sn, sizeof(sn));
	fmc_write_data(sn_idx_back, (u8 *)&sn, sizeof(sn));
	return len;
}
int nv_read_sn(u8 *data, int len) {
	mc_sn_t *sn;
	memset(&sn, 0, sizeof(sn));
	len = min(ARRAY_SIZE(sn->sn), len);
	
	sn = (mc_sn_t *)fmc_get_addr(sn_page_index);
	u16 crc16 = crc16_get(sn->sn, min(32, sn->len));
	if (crc16 == sn->crc) {
		memcpy(data, sn->sn, len);
		return len;
	}
	sn = (mc_sn_t *)fmc_get_addr(sn_idx_back);
	crc16 = crc16_get(sn->sn, min(32, sn->len));
	if (crc16 == sn->crc) {
		memcpy(data, sn->sn, len);
		return len;
	}
	return 0;
}

void nv_storage_init(void) {
	nv_read_motor_params();
	nv_read_foc_params();
	nv_read_gear_configs();
	nv_read_limit_config();
	sys_debug("encoder_off = %f\n", m_params.offset);
	if (m_params.mot_nr != MOTOR_NR) {
		nv_default_motor_params();
		nv_default_foc_params();
		nv_save_foc_params();
		nv_save_motor_params();
		nv_default_gear_config();
		nv_save_gear_configs();
		nv_default_limter();
		nv_save_limit_config();
		sys_debug("change motor %x\n", m_params.mot_nr);
	}
#if CONFIG_MOT_TYPE==MOTOR_BLUESHARK_NEW1
	m_params.offset = 0.0f; //编码器做了零位置校准
#endif
#if CONFIG_MOT_TYPE==MOTOR_BLUESHARK_ZD_100
	m_params.offset = 0.0f; //编码器做了零位置校准
	m_params.est_pll_band = 200;
	m_params.flux_linkage = MOTOR_Flux;
#endif
#if CONFIG_MOT_TYPE==MOTOR_BLUESHARK_A1
	m_params.offset = 0.0f; //编码器做了零位置校准
#endif
	sys_debug("%f -- %f, flux: %f, %d\n", foc_params.n_currentBand, m_params.ld, m_params.flux_linkage, sizeof(m_params));
}