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

static motor_params_t m_params;
static foc_params_t   foc_params;
static mc_gear_config_t gear_config;
static int _write_position = 0;

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;
}

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;
	m_params.offset = MOTOR_ENC_OFFSET;//180;//(69.0f);
	m_params.est_pll_band = 100;
	m_params.pos_lock_pll_band = 500;
	m_params.flux_linkage = 0.0f;
}

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_PhaseCurreBrkLim = CONFIG_DEFAULT_EBRK_PHASE_CURR;
	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_acc_time = CONFIG_ACC_TIME;
	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.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 = TRQ_PI_KP;
	foc_params.pid_conf[PID_TRQ_id].ki = TRQ_PI_KI;
	foc_params.pid_conf[PID_TRQ_id].kd = TRO_PI_KD;

	foc_params.pid_conf[PID_Spd_id].kp = 0.13f;
	foc_params.pid_conf[PID_Spd_id].ki = 0.08f;
	foc_params.pid_conf[PID_Spd_id].kd = 0.0f;

	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;

	foc_params.pid_conf[PID_FW_id].kp = 0;
	foc_params.pid_conf[PID_FW_id].ki = 0.01f;
	foc_params.pid_conf[PID_FW_id].kd = 0;
}

static void nv_default_gear_config(void) {
	/* 48v 模式 */
	gear_config.gears_48[0].u_maxRPM = 1430;
	gear_config.gears_48[0].u_maxTorque = CONFIG_DEFAULT_PHASE_CURR_LIM * 0.5f;
	gear_config.gears_48[0].u_maxIdc = 25;
	gear_config.gears_48[0].u_accTime = 50;

	gear_config.gears_48[1].u_maxRPM = 2100;
	gear_config.gears_48[1].u_maxTorque = CONFIG_DEFAULT_PHASE_CURR_LIM * 0.8f;
	gear_config.gears_48[1].u_maxIdc = 30;
	gear_config.gears_48[1].u_accTime = 80;

	gear_config.gears_48[2].u_maxRPM = 2850;
	gear_config.gears_48[2].u_maxTorque = CONFIG_DEFAULT_PHASE_CURR_LIM;
	gear_config.gears_48[2].u_maxIdc = 40;
	gear_config.gears_48[2].u_accTime = 80;

	gear_config.gears_48[3].u_maxRPM = 2850;
	gear_config.gears_48[3].u_maxTorque = CONFIG_DEFAULT_PHASE_CURR_LIM;
	gear_config.gears_48[3].u_maxIdc = 40;
	gear_config.gears_48[3].u_accTime = 80;

	gear_config.gears_48[4].u_maxRPM = 2850;
	gear_config.gears_48[4].u_maxTorque = CONFIG_DEFAULT_PHASE_CURR_LIM;
	gear_config.gears_48[4].u_maxIdc = 40;
	gear_config.gears_48[4].u_accTime = 80;

	/* 96v 模式 */
	gear_config.gears_96[0].u_maxRPM = 2850;
	gear_config.gears_96[0].u_maxTorque = CONFIG_DEFAULT_PHASE_CURR_LIM * 0.5f;
	gear_config.gears_96[0].u_maxIdc = 25;
	gear_config.gears_96[0].u_accTime = 50;

	gear_config.gears_96[1].u_maxRPM = 4300;
	gear_config.gears_96[1].u_maxTorque = CONFIG_DEFAULT_PHASE_CURR_LIM * 0.8f;
	gear_config.gears_96[1].u_maxIdc = 30;
	gear_config.gears_96[1].u_accTime = 80;

	gear_config.gears_96[2].u_maxRPM = 5500;
	gear_config.gears_96[2].u_maxTorque = CONFIG_DEFAULT_PHASE_CURR_LIM;
	gear_config.gears_96[2].u_maxIdc = 35;
	gear_config.gears_96[2].u_accTime = 100;

	gear_config.gears_96[3].u_maxRPM = 5500;
	gear_config.gears_96[3].u_maxTorque = CONFIG_DEFAULT_PHASE_CURR_LIM;
	gear_config.gears_96[3].u_maxIdc = 45;
	gear_config.gears_96[3].u_accTime = 100;

	gear_config.gears_96[4].u_maxRPM = 5500;
	gear_config.gears_96[4].u_maxTorque = CONFIG_DEFAULT_PHASE_CURR_LIM;
	gear_config.gears_96[4].u_maxIdc = 45;
	gear_config.gears_96[4].u_accTime = 100;
}



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));
		}
	}
}
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) {
	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) {
			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));
		}
	}
}

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;
}


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;
}

trq2dq_table_t *_trq2dq_table(int idx) {
	trq2dq_table_t *tbl = (trq2dq_table_t *)fmc_get_addr(idx);

	if (tbl->magic != 0x5AA5) {
		sys_error("trq tlb %d magic error, 0x%x\n", idx, tbl->magic);
		return NULL;
	}
	u16 crc = crc16_get((u8 *)tbl, sizeof(trq2dq_table_t) - 4);
	if (crc != tbl->crc16) {
		sys_error("trq tlb %d crc16 error\n", idx);
		return NULL;
	}
	return tbl;
}

trq2dq_table_t *nv_get_trq2dq_table(void) {
	trq2dq_table_t *tbl = _trq2dq_table(trq_Tbl_idx0);
	if (tbl == NULL) {
		tbl = _trq2dq_table(trq_Tbl_idx1);
	}
	return tbl;
}

void nv_write_trq_table_begin(int index) {
	fmc_write_trq_table_begin(fmc_get_addr(index==0?trq_Tbl_idx0:trq_Tbl_idx1));
	_write_position = 0;
}

int nv_write_trq_table_continue(uint8_t *data, int len){
	int w_pos = decode_u24(data);
	if ((_write_position + len - 3) > one_page_size * trq_Tbl_size) {
		return -1;
	}
	if (w_pos == _write_position && len > 3) {
		fmc_write_trq_table_continue(data + 3, len - 3);
		_write_position += len - 3;
	}

	return _write_position;
}

static int nv_write_trq_table1(void) {
	u8 cache[16];
	int remain = sizeof(trq2dq_table_t);
	u32 faddr = fmc_get_addr(trq_Tbl_idx0);
	fmc_write_trq_table_begin(fmc_get_addr(trq_Tbl_idx1));

	while(remain > 0) {
		int len = min(16, sizeof(trq2dq_table_t));
		memcpy(cache, (void *)faddr, len);
		fmc_write_trq_table_continue(cache, len);
		remain -= len;
		faddr  += len;
	}
	
	fmc_write_trq_table_end();

	return _trq2dq_table(trq_Tbl_idx1) == NULL ? 1: 0;
}

int nv_write_trq_table_check(u8 *data, int len, int index) {
	uint32_t size, checksum;
	size = decode_u24(data);
	checksum = decode_u32(data + 3);
	u16 magic = 0x5AA5;
	fmc_write_trq_table_continue((u8 *)&magic, sizeof(magic));
	fmc_write_trq_table_continue((u8 *)&checksum, sizeof(checksum));
	fmc_write_trq_table_end();

	u16 fmc_checksum = crc16_get((const u8 *)fmc_get_addr(index==0?trq_Tbl_idx0:trq_Tbl_idx1), size);
	if ((checksum == fmc_checksum) && (NULL != _trq2dq_table(index==0?trq_Tbl_idx0:trq_Tbl_idx1))) {
		if (nv_write_trq_table1()) { //write back error
			return 1;
		}
		return 0;
	}

	return 1;
}

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();
	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();
		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; //编码器做了零位置校准
#endif
#if CONFIG_MOT_TYPE==MOTOR_BLUESHARK_A1
	m_params.offset = 0.0f; //编码器做了零位置校准
#endif
	foc_params.s_maxDCVol = CONFIG_MAX_DC_VOL;
	foc_params.s_minDCVol = CONFIG_MIN_DC_VOL;
	sys_debug("current band %f -- %d\n", foc_params.n_currentBand, sizeof(foc_params_t));
}