/*
 * charger_gbt.c
 *
 *  Created on: Apr 18, 2024
 *      Author: colorbass
 */


#include "charger_gbt.h"
#include "main.h"
#include "board.h"
#include "stdio.h"
#include "j1939.h"
#include "string.h"
#include "edcan.h"
#include "connector.h"
#include "soft_rtc.h"
#include "debug.h"

uint8_t GBT_CC_GetStateRaw();

gbtState_t GBT_State;
uint32_t GBT_state_tick; //Tick after state switch
uint32_t GBT_delay_start;
uint32_t GBT_delay;
/* Флаги приёма пакетов от EVSE (EV получает) */
uint8_t GBT_CHM_recv;   // CHM получен (0x2600) → отвечаем BHM
uint8_t GBT_CRM_recv;   // CRM получен (0x0100) → отвечаем BRM или BCP
uint8_t GBT_CML_recv;   // CML получен (0x0800) → отвечаем BRO
uint8_t GBT_CST_recv;   // CST получен (0x1A00) → EVSE запросил стоп, переходим в STOP
uint8_t GBT_CSD_recv;   // CSD получен (0x1D00) → финальный отчёт EVSE, переходим в STOP_CSD

uint8_t GBT_CRO_val;    // последнее значение CRO от EVSE (0x0A00)

GBT_CML_t GBT_MaxLoad;
GBT_CRM_t GBT_ChargerInfo;

GBT_BHM_t GBT_MaxVoltage;
GBT_BRM_t GBT_EVInfo;
GBT_BCP_t GBT_BATStat;
GBT_BCL_t GBT_ReqPower;
GBT_BCL_t GBT_CurrPower;

GBT_BCS_t GBT_ChargingStatus;

GBT_BSM_t GBT_BatteryStatus;

GBT_CCS_t GBT_ChargerCurrentStatus;

GBT_CSD_t GBT_ChargerStop;

uint8_t GBT_BRO;
uint8_t cc_enable;

uint32_t GBT_TimeChargingStarted;

uint32_t GBT_StopCauseCode;
uint32_t GBT_ErrorCode;

GBT_StopSource_t GBT_StopSource;

static uint32_t GBT_EVSE_last_rx_tick;

#define GBT_EV_LINK_LOSS_TIMEOUT_MS 5000U
#define GBT_EV_WAIT_READY_TIMEOUT_MS 10000U
#define GBT_EV_CHARGING_RX_TIMEOUT_MS 3000U
#define GBT_EV_ENABLE_LOAD_DELAY_MS 10000U

static void GBT_HandleEvseRxPacket(const j_receive_t *rx){
	GBT_EVSE_last_rx_tick = HAL_GetTick();
	switch (rx->PGN){
		case 0x2600: // CHM EVSE->EV (старт/версия GB/T)
			GBT_CHM_recv = 1;
			break;

		case 0x0100: // CRM EVSE->EV (идентификация зарядника, 0x00 или 0xAA)
			memcpy(&GBT_ChargerInfo, rx->data, sizeof(GBT_ChargerInfo));
			GBT_CRM_recv = 1;
			break;

		case 0x0800: // CML EVSE->EV (макс. параметры зарядника)
			memcpy(&GBT_MaxLoad, rx->data, sizeof(GBT_MaxLoad));
			GBT_CML_recv = 1;
			break;

		case 0x0700: // CTS EVSE->EV (time sync)
			break;

		case 0x0A00: // CRO EVSE->EV (0x00 = инициализация, 0xAA = готов к зарядке)
			GBT_CRO_val = (rx->data[0] & 0xFF);
			break;

		case 0x1200: // CCS EVSE->EV (текущий статус зарядника)
			memcpy(&GBT_ChargerCurrentStatus, rx->data, sizeof(GBT_ChargerCurrentStatus));
			CONN[0].ChargingTime = GBT_ChargerCurrentStatus.chargingTime;
			CONN[0].MeasuredVoltageSE = GBT_ChargerCurrentStatus.outputVoltage / 10;
			CONN[0].MeasuredCurrentSE = 4000 - GBT_ChargerCurrentStatus.outputCurrent;
			break;

		case 0x1A00: // CST EVSE->EV (остановка зарядки по инициативе EVSE)
			GBT_CST_recv = 1;
			break;

		case 0x1D00: // CSD EVSE->EV (финальный отчёт зарядника)
			memcpy(&GBT_ChargerStop, rx->data, sizeof(GBT_ChargerStop));
			GBT_CSD_recv = 1;
			break;

		case 0x1F00: // CEM EVSE->EV (сообщение об ошибке)
			memcpy(&GBT_ErrorCode, rx->data, sizeof(GBT_ErrorCode));
			break;

		default:
			break;
	}
}

void GBT_ApplyShortPacket(uint32_t pgn, const uint8_t *data, uint8_t dlc){
	j_receive_t rx;
	memset(&rx, 0, sizeof(rx));
	rx.PGN = pgn;
	rx.size = dlc;
	if(data != NULL){
		uint8_t n = (dlc > 8U) ? 8U : dlc;
		memcpy(rx.data, data, n);
	}
	GBT_HandleEvseRxPacket(&rx);
}

void GBT_Init(){
	GBT_State = GBT_DISABLED;
	CONN[0].connControl = CMD_NONE;
	cc_enable = 0U;
	/* BRM defaults are initialized once after boot and kept unchanged. */
	GBT_EVInfo.version[0] = 2;
	GBT_EVInfo.version[1] = 0;
	GBT_EVInfo.version[2] = 0;
	GBT_EVInfo.batteryType = 1;
	GBT_EVInfo.batteryCapacity = 700;
	GBT_EVInfo.batteryVoltage = 3990;
	memcpy(GBT_EVInfo.batteryVendor, "TEST", 4);
	GBT_EVInfo.batterySN = 666666;
	GBT_EVInfo.batteryManuY = 30;
	GBT_EVInfo.batteryManuM = 2;
	GBT_EVInfo.batteryManuD = 20;
	GBT_EVInfo.batteryCycleCount = 666;
	GBT_EVInfo.ownAuto = 1;
	GBT_EVInfo.rsvd0 = 0;
	GBT_BATStat.maxCellVoltage = 320;
	GBT_BATStat.maxChargingCurrent = 40;
	GBT_BATStat.totalEnergy = 6;
	GBT_BATStat.maxChargingVoltage = 500;
	GBT_BATStat.maxTemp = 70;
	GBT_BATStat.SOC = CONN[0].SOC;
	GBT_BATStat.measVoltage = CONN[0].MeasuredVoltage;
	memcpy(GBT_EVInfo.EVIN, "EDISON_TEST_EVIN_", 17);
	memcpy(GBT_EVInfo.EV_SW_VER, "1.0.0", 8);
	GBT_Reset();
}


void GBT_ChargerTask(){
	RELAY_Write(RELAY_CC, cc_enable);

	//GBT_LockTask();
	if(j_rx.state == 2){
		GBT_HandleEvseRxPacket(&j_rx);
		j_rx.state = 0;
	}

	if((connectorState == Unplugged) && (GBT_State != GBT_DISABLED)){
		log_printf(LOG_INFO, "Car unplugged, resetting charge session\n");
		CONN[0].enableLoad = 0;
		GBT_Reset();
		return;
	}

	if((HAL_GetTick() - GBT_delay_start) < GBT_delay){
		//waiting
	}else switch (GBT_State){
		case GBT_DISABLED:
			CONN[0].enableLoad = 0;
			// if(connectorState == Preparing){
			// 	GBT_Reset();
			// 	GBT_SwitchState(GBT_EV_CONNECTING);
			// }
			break;

		/* --- Строгая последовательность GB/T EV: приём от EVSE → ответ EV --- */

		case GBT_EV_CONNECTING:
			// 1) Ждём CHM (0x2600), ничего не шлём. Получили → переход в GBT_EV_HANDSHAKE
			GBT_Delay(250);

			if (GBT_CHM_recv) {
				log_printf(LOG_INFO, "CHM received, starting EV handshake\n");
				GBT_EVSE_last_rx_tick = HAL_GetTick();
				GBT_SwitchState(GBT_EV_HANDSHAKE);
				break;
			}
			if (GBT_StateTick() > 10000) {
				GBT_Error(0xFCF0C0FC);
				log_printf(LOG_WARN, "CHM timeout\n");
			}
			break;

		case GBT_EV_HANDSHAKE:
			// 2) Постоянно шлём BHM, ждём CRM (0x0100, первый раз 0x00)
			GBT_MaxVoltage.maxOutputVoltage = CONN[0].RequestedVoltage * 10;
			if (j_rx.state == 0) GBT_SendBHM();
			GBT_Delay(250);

			if (GBT_CRM_recv) {
				log_printf(LOG_INFO, "CRM received, sending BRM (EV identification)\n");
				GBT_SwitchState(GBT_EV_RECOGNITION);
				break;
			}
			if ((HAL_GetTick() - GBT_EVSE_last_rx_tick) > GBT_EV_LINK_LOSS_TIMEOUT_MS) {
				GBT_Error(0xFCF0C0FD);
				log_printf(LOG_WARN, "EVSE link loss in EV_HANDSHAKE (no RX for %u ms)\n",
						(unsigned)GBT_EV_LINK_LOSS_TIMEOUT_MS);
			}
			break;


		case GBT_EV_RECOGNITION:
			// 3) Постоянно шлём BRM, ждём CRM (0x0100, 0xAA)
			if (j_rx.state == 0) GBT_SendBRM(); // TODO CHUNKED SEND
			GBT_Delay(250);

			if ((GBT_CRM_recv) && (GBT_ChargerInfo.bmsIdentified == 0xAA)) {
				log_printf(LOG_INFO, "EV identified by charger, sending BCP\n");
				GBT_SwitchState(GBT_EV_CHARGING_PARAMETERS);
				break;
			}
			if (GBT_StateTick() > 5000) {
				GBT_Error(0xFCF1C0FC);
				log_printf(LOG_WARN, "CRM(0xAA) timeout (wait BCP)\n");
			}
			break;

		case GBT_EV_CHARGING_PARAMETERS:
			// 4) Постоянно шлём BCP, ждём CML (0x0800) → шлём BRO(0xAA)
			if (j_rx.state == 0) GBT_SendBCP();
			GBT_Delay(250);

			if (GBT_CML_recv) {
				log_printf(LOG_INFO, "CML received, starting BMS initialization\n");
				GBT_SwitchState(GBT_EV_BMS_INIT);
				break;
			}
			if (GBT_StateTick() > 5000) {
				GBT_Error(0xFCF4C0FC);
				log_printf(LOG_WARN, "CML timeout\n");
			}
			break;
		case GBT_EV_BMS_INIT:
			// 5) Постоянно шлём BRO(0x00) и ждем инициализацию CRO(0xAA)
			if (j_rx.state == 0) GBT_SendBRO(0x00);
			GBT_Delay(250);
			if (GBT_StateTick() > 1500) {
				log_printf(LOG_INFO, "BMS initialized, waiting charger ready signal\n");
				GBT_SwitchState(GBT_EV_WAIT_CHARGER_READY);
				break;
			}

			break;

		case GBT_EV_WAIT_CHARGER_READY:
			// 5) Постоянно шлём BRO(0xAA) и ждем инициализацию CRO(0xAA)
			if (j_rx.state == 0) GBT_SendBRO(0xAA);

			GBT_Delay(250);

			if (GBT_CRO_val == 0xAA) {
				log_printf(LOG_INFO, "Charger ready, entering active charging\n");
				GBT_SwitchState(GBT_EV_CHARGING);
				GBT_TimeChargingStarted = get_Current_Time();
				GBT_EVSE_last_rx_tick = HAL_GetTick();
				break;
			}
			if (GBT_StateTick() > GBT_EV_WAIT_READY_TIMEOUT_MS) {
				GBT_Error(0xFCF2C0FD);
				log_printf(LOG_WARN, "CRO(0xAA) timeout in EV_WAIT_CHARGER_READY\n");
			}

			break;

		case GBT_EV_CHARGING:
			// Основной режим зарядки: EV периодически шлёт BCS/BSM.
			GBT_ReqPower.requestedVoltage = CONN[0].RequestedVoltage * 10;
			GBT_ReqPower.requestedCurrent = 4000 - CONN[0].RequestedCurrent;
			GBT_ReqPower.chargingMode = 1;

			GBT_BATStat.SOC = CONN[0].SOC;
			GBT_BATStat.measVoltage = CONN[0].MeasuredVoltage;

			/* Enable load only after delayed start in charging state. */
			if (GBT_StateTick() >= GBT_EV_ENABLE_LOAD_DELAY_MS) {
				CONN[0].enableLoad = GBT_ChargerCurrentStatus.chargingPermissible;
			} else {
				CONN[0].enableLoad = 0;
			}
			
			// Стоп по инициативе EVSE (получили CST)
			if (GBT_CST_recv) {
				log_printf(LOG_INFO, "Charging stop requested by EVSE (CST)\n");
				GBT_StopEVSE(GBT_CST_SUDDENSTOP);
				break;
			}
			// Стоп по команде с машины (EDCAN)
			if (CONN[0].connControl == CMD_STOP) {
				CONN[0].connControl = CMD_NONE;
				log_printf(LOG_INFO, "Charging stop requested by EV command\n");
				GBT_StopEV(GBT_CST_BMS_ACTIVELY_SUSPENDS);
				break;
			}
			if (IN_ReadInput(IN_ESTOP)) {
				log_printf(LOG_INFO, "Charging stop requested by emergency input\n");
				GBT_StopEV(GBT_CST_BMS_ACTIVELY_SUSPENDS);
				break;
			}
			if ((HAL_GetTick() - GBT_EVSE_last_rx_tick) > GBT_EV_CHARGING_RX_TIMEOUT_MS) {
				log_printf(LOG_WARN, "EVSE RX timeout in EV_CHARGING\n");
				GBT_StopEVSE(GBT_CST_SUDDENSTOP);
				break;
			}

			GBT_SendBCS();
			GBT_SendBCL();
			GBT_SendBSM();
			GBT_Delay(250);
			break;

		case GBT_STOP:
			GBT_Delay(10);
			CONN[0].enableLoad = 0;
			if (GBT_StopSource == GBT_STOP_EV) {
				// EV-инициированный stop: сначала BST, ждём ответный CST, потом BSD.
				GBT_SendBST(GBT_StopCauseCode);
				if (GBT_CST_recv) {
					GBT_SendBSD();
				}
			} else {
				// EVSE-инициированный stop: EV сразу отвечает BSD.
				GBT_SendBSD();
			}
			if (GBT_StateTick() > 10000) {
				log_printf(LOG_WARN, "CSD Timeout\n");
				GBT_Error(0xFCF0C0FD); // CSD timeout
			}
			if (GBT_CSD_recv) {
				log_printf(LOG_INFO, "CSD received, finalizing charge session\n");
				GBT_SwitchState(GBT_STOP_CSD);
			}
			break;

		case GBT_STOP_CSD:
			// EV не шлёт CSD (финальный отчёт шлёт EVSE). Ждём 2.5 с и завершаем.
			GBT_Delay(250);
			if (GBT_StateTick() > 2500) {
				log_printf(LOG_INFO, "Charge session completed\n");
				GBT_SwitchState(GBT_COMPLETE);
			}
			break;


		case GBT_ERROR:
			//GBT_SendBEM(GBT_ErrorCode); //2.5S TODO
			GBT_SwitchState(GBT_COMPLETE);
//			GBT_Reset();
			//
			break;

		case GBT_COMPLETE:
			if(connectorState != Finished) {
				GBT_SwitchState(GBT_DISABLED);
				GBT_Reset();//CHECK
			}
			break;

		default:
			GBT_SwitchState(GBT_DISABLED);
	}
	if (CONN_CC_GetState()==GBT_CC_4V) CONN[0].EvseConnected = 1;
	else CONN[0].EvseConnected = 0;
}



void GBT_SwitchState(gbtState_t state){
	GBT_State = state;
	GBT_state_tick = HAL_GetTick();

	if(GBT_State == GBT_DISABLED) 				log_printf(LOG_DEBUG, "DBG_STATE: GBT_DISABLED\n");
	if(GBT_State == GBT_EV_CONNECTING) 			log_printf(LOG_DEBUG, "DBG_STATE: GBT_EV_CONNECTING\n");
	if(GBT_State == GBT_EV_HANDSHAKE) 			log_printf(LOG_DEBUG, "DBG_STATE: GBT_EV_HANDSHAKE\n");
	if(GBT_State == GBT_EV_RECOGNITION) 		log_printf(LOG_DEBUG, "DBG_STATE: GBT_EV_RECOGNITION\n");
	if(GBT_State == GBT_EV_CHARGING_PARAMETERS) log_printf(LOG_DEBUG, "DBG_STATE: GBT_EV_CHARGING_PARAMETERS\n");
	if(GBT_State == GBT_EV_BMS_INIT) 			log_printf(LOG_DEBUG, "DBG_STATE: GBT_EV_BMS_INIT\n");
	if(GBT_State == GBT_EV_WAIT_CHARGER_READY) 	log_printf(LOG_DEBUG, "DBG_STATE: GBT_EV_WAIT_CHARGER_READY\n");
	if(GBT_State == GBT_EV_CHARGING) 			log_printf(LOG_DEBUG, "DBG_STATE: GBT_EV_CHARGING\n");
	if(GBT_State == GBT_STOP) 					log_printf(LOG_DEBUG, "DBG_STATE: GBT_STOP\n");
	if(GBT_State == GBT_STOP_CSD) 				log_printf(LOG_DEBUG, "DBG_STATE: GBT_STOP_CSD\n");
	if(GBT_State == GBT_COMPLETE) 				log_printf(LOG_DEBUG, "DBG_STATE: GBT_COMPLETE\n");
	if(GBT_State == GBT_ERROR) 					log_printf(LOG_WARN, "State machine entered ERROR state\n");


}

uint32_t GBT_StateTick(){
	return HAL_GetTick() - GBT_state_tick;
}

void GBT_Delay(uint32_t delay){
	GBT_delay_start = HAL_GetTick();
	GBT_delay = delay;
}

void GBT_StopEV(uint32_t causecode){ // --> Suspend EV
	if (CONN[0].chargingError){
		GBT_StopSource = GBT_STOP_EVSE;
	}else{
		GBT_StopSource = GBT_STOP_EV;
	}
	GBT_StopCauseCode = causecode;
	if(GBT_State != GBT_STOP) GBT_SwitchState(GBT_STOP);
}

void GBT_StopEVSE(uint32_t causecode){ // --> Suspend EVSE
	GBT_StopSource = GBT_STOP_EVSE;
	GBT_StopCauseCode = causecode;
	if(GBT_State != GBT_STOP) GBT_SwitchState(GBT_STOP);
}

void GBT_StopOCPP(uint32_t causecode){ // --> Finished
	GBT_StopSource = GBT_STOP_OCPP;
	GBT_StopCauseCode = causecode;
	if(GBT_State != GBT_STOP) GBT_SwitchState(GBT_STOP);
}

void GBT_ForceStop(){ // --> Suspend EV
	GBT_StopSource = GBT_STOP_EV;
	// Отключаем силовой контактор батареи со стороны EV
	CONN[0].enableLoad = 0;
	GBT_SwitchState(GBT_COMPLETE);
}

void GBT_Error(uint32_t errorcode){ // --> Suspend EV
	GBT_StopSource = GBT_STOP_EV;
	log_printf(LOG_WARN, "GBT Error code: 0x%X\n", errorcode);
	GBT_ErrorCode = errorcode;
	GBT_SwitchState(GBT_ERROR);
}


void GBT_Reset(){
	GBT_SwitchState(GBT_DISABLED);
	GBT_CHM_recv = 0;
	GBT_CRM_recv = 0;
	GBT_CML_recv = 0;
	GBT_CST_recv = 0;
	GBT_CSD_recv = 0;
	GBT_CRO_val = 0x00;
	CONN[0].enableLoad = 0;
	CONN[0].chargingError = 0;
	memset(&GBT_ReqPower, 0, sizeof (GBT_ReqPower));
	memset(&GBT_CurrPower, 0, sizeof (GBT_CurrPower));
	memset(&GBT_MaxVoltage, 0, sizeof (GBT_MaxVoltage));
	memset(&GBT_ChargingStatus, 0, sizeof (GBT_ChargingStatus));
	memset(&GBT_BatteryStatus, 0, sizeof (GBT_BatteryStatus));
	memset(&GBT_ChargerCurrentStatus, 0, sizeof (GBT_ChargerCurrentStatus));
	memset(&GBT_ChargerStop, 0, sizeof (GBT_ChargerStop));
	GBT_CurrPower.requestedCurrent = 4000; //0A
	GBT_CurrPower.requestedVoltage = 500;  //50V
	GBT_TimeChargingStarted = 0;
	GBT_BRO = 0x00;
	GBT_EVSE_last_rx_tick = HAL_GetTick();
}
