#include <psu_control.h>

#include "can.h"
#include "string.h"
#include "stdio.h"
#include "charger_config.h"
#include "charger_control.h"
#include "charger_gbt.h"
#include "board.h"
#include "debug.h"

PSU_02_t PSU_02;
PSU_04_t PSU_04;
PSU_06_t PSU_06;
PSU_08_t PSU_08;
PSU_09_t PSU_09;

PSU_1A_t PSU_1A;
PSU_1B_t PSU_1B;
PSU_1C_t PSU_1C;

PSU_t PSU0;

#define CAN_DELAY 20
#define PSU_VOLTAGE_THRESHOLD 20 // Порог напряжения для определения состояния (В)
#define PSU_ONLINE_TIMEOUT 500 // Таймаут для определения состояния (мс)
#define PSU_STARTUP_DELAY 4000 // Задержка 2 секунды перед включением

uint32_t can_lastpacket;

extern CAN_HandleTypeDef hcan2;

static void PSU_SwitchState(PSU_State_t state){
	PSU0.state = state;
	PSU0.statetick = HAL_GetTick();
}

static uint32_t PSU_StateTime(void){
	return HAL_GetTick() - PSU0.statetick;
}

void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef *hcan){

	static CAN_RxHeaderTypeDef RxHeader;
	static uint8_t RxData[8] = {0,};
	CanId_t CanId;

    if(HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO1, &RxHeader, RxData) == HAL_OK)
    {
    	memcpy(&CanId, &RxHeader.ExtId, sizeof(CanId_t));

		/* Для DC30 поддерживается только один силовой модуль (source == 0) */
		if(CanId.source != 0) return;
		can_lastpacket = HAL_GetTick();

    	if(CanId.command==0x02){
    		memcpy(&PSU_02, RxData, 8);
    	}
    	if(CanId.command==0x04){
    		memcpy(&PSU_04, RxData, 8);

			PSU0.tempAmbient = PSU_04.moduleTemperature;
			PSU0.status0.raw = PSU_04.modularForm0;
			PSU0.status1.raw = PSU_04.modularForm1;
			PSU0.status2.raw = PSU_04.modularForm2;
    	}
    	if(CanId.command==0x06){
    		memcpy(&PSU_06, RxData, 8);

    		PSU_06.VAB = PSU_06.VABLo+(PSU_06.VABHi<<8);
    		PSU_06.VBC = PSU_06.VBCLo+(PSU_06.VBCHi<<8);
    		PSU_06.VCA = PSU_06.VCALo+(PSU_06.VCAHi<<8);

    	}
    	if(CanId.command==0x08){
    		memcpy(&PSU_08, RxData, 8);
    	}
    	if(CanId.command==0x09){

    		memcpy(&PSU_09, RxData, 8);
    		PSU_09.moduleNCurrent  = PSU_09.moduleNCurrent_[3];
    		PSU_09.moduleNCurrent |= PSU_09.moduleNCurrent_[2]<<8;
    		PSU_09.moduleNCurrent |= PSU_09.moduleNCurrent_[1]<<16;
    		PSU_09.moduleNCurrent |= PSU_09.moduleNCurrent_[0]<<24;

    		PSU_09.moduleNVoltage  = PSU_09.moduleNVoltage_[3];
    		PSU_09.moduleNVoltage |= PSU_09.moduleNVoltage_[2]<<8;
    		PSU_09.moduleNVoltage |= PSU_09.moduleNVoltage_[1]<<16;
    		PSU_09.moduleNVoltage |= PSU_09.moduleNVoltage_[0]<<24;

			// PSU_09 -> PSU -> CONN (один модуль)
			{
				uint16_t v = PSU_09.moduleNVoltage / 1000;
				int16_t  i = PSU_09.moduleNCurrent / 100;

				// Обновляем модель PSU0 по телеметрии
				PSU0.outputVoltage = v;
				PSU0.outputCurrent = i;
				PSU0.PSU_enabled   = (v >= PSU_VOLTAGE_THRESHOLD);
				PSU0.online        = 1;
				PSU0.temperature   = PSU_04.moduleTemperature;

				// Экспортируем значения из PSU0 в CONN только,
				// когда модуль хотя бы в состоянии READY и выше
				if(PSU0.state >= PSU_READY){
					CONN.MeasuredVoltage = PSU0.outputVoltage;
					CONN.MeasuredCurrent = PSU0.outputCurrent;
					CONN.Power = CONN.MeasuredCurrent * CONN.MeasuredVoltage / 10;
					CONN.outputEnabled = PSU0.PSU_enabled;
				}
			}
    	}
    }
}

void PSU_CAN_FilterInit(){
	CAN_FilterTypeDef  sFilterConfig;

	sFilterConfig.FilterBank = 14;
	sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK;
	sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT;
	sFilterConfig.FilterIdHigh = 0x0000;
	sFilterConfig.FilterIdLow = 0x0000;
	sFilterConfig.FilterMaskIdHigh = 0x0000;
	sFilterConfig.FilterMaskIdLow = 0x0000;
	sFilterConfig.FilterFIFOAssignment = CAN_RX_FIFO0;
	sFilterConfig.FilterActivation = ENABLE;

	sFilterConfig.FilterFIFOAssignment = CAN_RX_FIFO1;
	sFilterConfig.SlaveStartFilterBank = 14;


	if(HAL_CAN_ConfigFilter(&hcan2, &sFilterConfig) != HAL_OK)
	{
		Error_Handler();
	}
}

void PSU_Init(){

	HAL_CAN_Stop(&hcan2);
	MX_CAN2_Init();
	PSU_CAN_FilterInit();
	HAL_CAN_Start(&hcan2);
	HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO1_MSG_PENDING /* | CAN_IT_ERROR | CAN_IT_BUSOFF | CAN_IT_LAST_ERROR_CODE | CAN_IT_TX_MAILBOX_EMPTY*/);
	memset(&PSU0, 0, sizeof(PSU0));
	PSU0.state = PSU_UNREADY;
	PSU0.statetick = HAL_GetTick();

	PSU0.power_limit = PSU_MAX_POWER; // kW
	PSU0.hv_mode     = 0;

	PSU_Enable(0, 0);
}

void PSU_Enable(uint8_t addr, uint8_t enable){
	PSU_1A_t data;
	memset(&data, 0, sizeof(data));
	/* Для DC30 поддерживается только один модуль с адресом 0 */
	if(addr != 0) return;
	if(PSU0.online == 0) return;

	data.enable = !enable;
	PSU_SendCmd(0xF0, addr, 0x1A, &data);
	ED_Delay(CAN_DELAY);
}

void PSU_SetHVMode(uint8_t addr, uint8_t enable){
	PSU_1D_t data;
	memset(&data, 0, sizeof(data));
	data.enable = !enable;
	if(addr != 0) return;
	PSU_SendCmd(0xF0, addr, 0x1D, &data);
}
void PSU_SetVoltageCurrent(uint8_t addr, uint16_t voltage, uint16_t current){
	PSU_1C_t data;
	memset(&data, 0, sizeof(data));

	if(addr != 0) return;

	if(voltage<PSU_MIN_VOLTAGE) voltage = PSU_MIN_VOLTAGE;

	if((PSU0.hv_mode==0) && voltage>499) voltage = 499;

	uint32_t current_ma = current * 100;
	uint32_t voltage_mv = voltage * 1000;

	data.moduleCurrentTotal[0] = (current_ma >> 24) & 0xFF;
	data.moduleCurrentTotal[1] = (current_ma >> 16) & 0xFF;
	data.moduleCurrentTotal[2] = (current_ma >>  8) & 0xFF;
	data.moduleCurrentTotal[3] = (current_ma >>  0) & 0xFF;

	data.moduleVoltage[0] = (voltage_mv >>  24) & 0xFF;
	data.moduleVoltage[1] = (voltage_mv >>  16) & 0xFF;
	data.moduleVoltage[2] = (voltage_mv >>   8) & 0xFF;
	data.moduleVoltage[3] = (voltage_mv >>   0) & 0xFF;

	PSU_SendCmd(0xF0, addr, 0x1C, &data);

}

void PSU_SendCmd(uint8_t source, uint8_t destination, uint8_t cmd, void *data){
	CanId_t CanId;
	CanId.source = source;
	CanId.destination = destination;
	CanId.command = cmd;
	CanId.device = 0x0A;

	int8_t retry_counter = 10;
    CAN_TxHeaderTypeDef tx_header;
    uint32_t tx_mailbox;
    HAL_StatusTypeDef CAN_result;

    memcpy(&tx_header.ExtId, &CanId, sizeof(CanId_t));

    tx_header.RTR = CAN_RTR_DATA;
    tx_header.IDE = CAN_ID_EXT;
    tx_header.DLC = 8;

	while(retry_counter>0){ //если буфер полон, ждем пока он освободится
		if (HAL_CAN_GetTxMailboxesFreeLevel(&hcan2) > 0){
			/* отправка сообщения */
			CAN_result = HAL_CAN_AddTxMessage(&hcan2, &tx_header, (uint8_t*)data, &tx_mailbox);

			/* если отправка удалась, выход */
			if(CAN_result == HAL_OK) {
				return;
				retry_counter = 0;
			}
		}
		ED_Delay(1);

		retry_counter--;
	}

}

uint32_t max(uint32_t a, uint32_t b){
	if(a>b) return a;
	else return b;
}

void PSU_ReadWrite(){

	uint8_t zero_data[8] = {0,0,0,0,0,0,0,0};

	PSU_SendCmd(0xF0, 0, 0x04, zero_data);ED_Delay(CAN_DELAY);
	PSU_SendCmd(0xF0, 0, 0x06, zero_data);ED_Delay(CAN_DELAY);
	// PSU_SendCmd(0xF0, 0, 0x08, zero_data);ED_Delay(CAN_DELAY);
	PSU_SendCmd(0xF0, 0, 0x09, zero_data);ED_Delay(CAN_DELAY);

	// Power Limit
	if ((CONN.WantedCurrent/10) * CONN.MeasuredVoltage > PSU0.power_limit){
		CONN.RequestedCurrent = PSU0.power_limit * 10 / CONN.MeasuredVoltage;
	}else{
		CONN.RequestedCurrent = CONN.WantedCurrent;
	}

	if(CONN.RequestedCurrent > (PSU_MAX_CURRENT*10)){
		CONN.RequestedCurrent = PSU_MAX_CURRENT*10;
	}
	CONN.RequestedPower = CONN.RequestedCurrent * CONN.RequestedVoltage / 10;


	if(PSU0.ready){
		PSU_SetVoltageCurrent(0, CONN.RequestedVoltage, CONN.RequestedCurrent); // Normal mode
		ED_Delay(CAN_DELAY);
		if(CONN.MeasuredVoltage>490) PSU0.hv_mode = 1;
	}

	// PSU_SetHVMode(0, PSU0.hv_mode); // auto set, no need
	// ED_Delay(CAN_DELAY);

}

void PSU_Task(void){
	static uint32_t psu_on_tick = 0;
	static uint32_t dc_on_tick = 0;
	static uint32_t cont_ok_tick = 0;

	// Обновляем ONLINE/READY по таймауту
	if((HAL_GetTick() - can_lastpacket) > PSU_ONLINE_TIMEOUT){
		PSU0.online = 0;
		PSU0.PSU_enabled = 0;
		PSU_04.moduleTemperature = 0;
		PSU_04.modularForm0 = 0;
		PSU_04.modularForm1 = 0;
		PSU_04.modularForm2 = 0;
		PSU_06.VAB = 0;
		PSU_06.VBC = 0;
		PSU_06.VCA = 0;
		PSU_09.moduleNCurrent = 0;
		PSU_09.moduleNVoltage = 0;
	}
	if(!PSU0.online || !PSU0.enableAC){
		CONN.MeasuredVoltage = 0;
		CONN.MeasuredCurrent = 0;
		CONN.outputEnabled = 0;
	}

	// Управление AC-контактором с задержкой отключения 1 минута
	if(CONN.EvConnected){
		RELAY_Write(RELAY_AC, 1);
		psu_on_tick = HAL_GetTick();
		PSU0.enableAC = 1;
	}else{
		if((HAL_GetTick() - psu_on_tick) > 1 * 60000){
			RELAY_Write(RELAY_AC, 0);
			PSU0.enableAC = 0;
		}
	}

	// Текущее состояние DC-контактора по обратной связи
	PSU0.CONT_enabled = IN_ReadInput(IN_CONT_FB_DC);

	// Обновляем ready с учётом ошибок
	if(PSU0.online && !PSU0.cont_fault && PSU0.enableAC){
		// PSU0.ready = 1;
	}else{
		PSU0.ready = 0;
	}

	switch(PSU0.state){
	case PSU_UNREADY:
		PSU0.enableOutput = 0;
		RELAY_Write(RELAY_DC, 0);
		if(PSU0.online && PSU0.enableAC && !PSU0.cont_fault){
			PSU_SwitchState(PSU_INITIALIZING);
		}
		break;

	case PSU_INITIALIZING:
		if(PSU_StateTime() > 4000){ // Wait 4s for PSU to initialize
			PSU0.ready = 1;
			PSU_SwitchState(PSU_READY);
		}
		break;

	case PSU_READY:
		// модуль готов, но выключен
		PSU0.hv_mode = 0;

		RELAY_Write(RELAY_DC, 0);
		if(!PSU0.ready){
			PSU_SwitchState(PSU_UNREADY);
			break;
		}
		if(CONN.EnableOutput){
			PSU_Enable(0, 1);
			PSU_SwitchState(PSU_WAIT_ACK_ON);
		}
		break;

	case PSU_WAIT_ACK_ON:

		if(PSU0.PSU_enabled && PSU0.ready){
			dc_on_tick = HAL_GetTick();
			PSU_SwitchState(PSU_CONT_WAIT_ACK_ON);
		}else if(PSU_StateTime() > 10000){
			PSU0.psu_fault = 1;
			CONN.chargingError = CONN_ERR_PSU_FAULT;
			PSU_SwitchState(PSU_UNREADY);
			log_printf(LOG_ERR, "PSU on timeout\n");
		}
		break;

	case PSU_CONT_WAIT_ACK_ON:
		// замыкаем DC-контактор и ждём подтверждение
		RELAY_Write(RELAY_DC, 1);
		if(PSU0.CONT_enabled){
			PSU_SwitchState(PSU_CONNECTED);
		}else if(PSU_StateTime() > 1000){
			PSU0.cont_fault = 1;
			CONN.chargingError = CONN_ERR_CONTACTOR;
			PSU_SwitchState(PSU_CURRENT_DROP);
			log_printf(LOG_ERR, "Contactor error, stopping...\n");
		}
		break;

	case PSU_CONNECTED:
		// Основное рабочее состояние
		if(!CONN.EnableOutput || !PSU0.ready){
			PSU_SwitchState(PSU_CURRENT_DROP);
			break;
		}
		// контроль контактора: 1 c таймаут
		if (IN_ReadInput(IN_CONT_FB_DC) != RELAY_Read(RELAY_DC)){
			if((HAL_GetTick() - cont_ok_tick) > 1000){
				CONN.chargingError = CONN_ERR_CONTACTOR;
				PSU0.cont_fault = 1;
				PSU_SwitchState(PSU_CURRENT_DROP);
				log_printf(LOG_ERR, "Contactor error, stopping...\n");
			}
		}else{
			cont_ok_tick = HAL_GetTick();
		}
		break;

	case PSU_CURRENT_DROP:
		// снижаем ток до нуля перед отключением DC
		CONN.RequestedCurrent = 0;

		// если ток действительно упал или вышло время, отключаем DC
		if((CONN.MeasuredCurrent < 30) || (PSU_StateTime() > 5000)){
			PSU_SwitchState(PSU_CONT_WAIT_ACK_OFF);
		}
		break;

	case PSU_CONT_WAIT_ACK_OFF:
		RELAY_Write(RELAY_DC, 0);
		if(!PSU0.CONT_enabled){
			PSU_Enable(0, 0);
			PSU_SwitchState(PSU_WAIT_ACK_OFF);
		}else if(PSU_StateTime() > 1000){
			PSU0.cont_fault = 1;
			CONN.chargingError = CONN_ERR_CONTACTOR;
			PSU_Enable(0, 0);
			PSU_SwitchState(PSU_WAIT_ACK_OFF);
			log_printf(LOG_ERR, "Contactor error, stopping...\n");
		}
		break;

	case PSU_WAIT_ACK_OFF:
		if(!PSU0.PSU_enabled){
			PSU_SwitchState(PSU_OFF_PAUSE);
		}else if(PSU_StateTime() > 10000){
			PSU0.psu_fault = 1;
			CONN.chargingError = CONN_ERR_PSU_FAULT;
			PSU_SwitchState(PSU_UNREADY);
			log_printf(LOG_ERR, "PSU off timeout\n");
		}
		break;
	case PSU_OFF_PAUSE:
		if(PSU_StateTime() > 4000){
			PSU_SwitchState(PSU_READY);
		}
		break;
	


	default:
		PSU_SwitchState(PSU_UNREADY);
		break;
	}
}