/*
 * 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"

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;

uint32_t GBT_TimeChargingStarted;

uint32_t GBT_StopCauseCode;
uint32_t GBT_ErrorCode;

GBT_StopSource_t GBT_StopSource;

void GBT_Init(){
	GBT_State = GBT_DISABLED;
	CONN.connControl = CMD_NONE;
	GBT_Reset();
}


void GBT_ChargerTask(){

	//GBT_LockTask();
	if(j_rx.state == 2){
		switch (j_rx.PGN){
		case 0x2600: // CHM EVSE->EV (старт/версия GB/T)
			GBT_CHM_recv = 1;
			break;

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

		case 0x0800: // CML EVSE->EV (макс. параметры зарядника)
			memcpy(&GBT_MaxLoad, j_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 = (j_rx.data[0] & 0xFF);
			break;

		case 0x1200: // CCS EVSE->EV (текущий статус зарядника)
			memcpy(&GBT_ChargerCurrentStatus, j_rx.data, sizeof(GBT_ChargerCurrentStatus));
			CONN.enableLoad = GBT_ChargerCurrentStatus.chargingPermissible;
			CONN.ChargingTime = GBT_ChargerCurrentStatus.chargingTime;
			CONN.MeasuredVoltageSE = GBT_ChargerCurrentStatus.outputVoltage / 10;
			CONN.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, j_rx.data, sizeof(GBT_ChargerStop));
			GBT_CSD_recv = 1;
			break;

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

	if((HAL_GetTick() - GBT_delay_start) < GBT_delay){
		//waiting
	}else switch (GBT_State){
		case GBT_DISABLED:
			CONN.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) {
				GBT_SwitchState(GBT_EV_HANDSHAKE);
				break;
			}
			if (GBT_StateTick() > 10000) {
				GBT_Error(0xFCF0C0FC);
				EDCAN_printf(LOG_WARN, "CHM timeout\n");
			}
			break;

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

			if (GBT_CRM_recv) {
				GBT_SwitchState(GBT_EV_RECOGNITION);
				break;
			}
			break;


		case GBT_EV_RECOGNITION:
			// 3) Постоянно шлём BRM, ждём CRM (0x0100, 0xAA)
			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;
			memcpy(GBT_EVInfo.EVIN, "EDISON_TEST_EVIN_", 17);
			memcpy(GBT_EVInfo.EV_SW_VER, "1.0.0", 8);

			if (j_rx.state == 0) GBT_SendBRM(); // TODO CHUNKED SEND
			GBT_Delay(250);

			if ((GBT_CRM_recv) && (GBT_ChargerInfo.bmsIdentified == 0xAA)) {
				GBT_SwitchState(GBT_EV_CHARGING_PARAMETERS);
				break;
			}
			if (GBT_StateTick() > 5000) {
				GBT_Error(0xFCF1C0FC);
				EDCAN_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) {
				GBT_SwitchState(GBT_EV_BMS_INIT);
				break;
			}
			if (GBT_StateTick() > 5000) {
				GBT_Error(0xFCF4C0FC);
				EDCAN_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) {
				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) {
				GBT_SwitchState(GBT_EV_CHARGING);
				GBT_TimeChargingStarted = get_Current_Time();
				break;
			}

			break;

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

			GBT_BATStat.maxCellVoltage = 320;
			GBT_BATStat.maxChargingCurrent = 40;
			GBT_BATStat.totalEnergy = 6;
			GBT_BATStat.maxChargingVoltage = 500;
			GBT_BATStat.maxTemp = 70;
			GBT_BATStat.SOC = CONN.SOC;
			GBT_BATStat.measVoltage = CONN.MeasuredVoltage;
			
			// Стоп по инициативе EVSE (получили CST)
			if (GBT_CST_recv) {
				GBT_StopEVSE(GBT_CST_SUDDENSTOP);
				break;
			}
			// Стоп по команде с машины (EDCAN)
			if (CONN.connControl == CMD_STOP) {
				GBT_StopEV(GBT_CST_BMS_ACTIVELY_SUSPENDS);
				break;
			}
			if (IN_ReadInput(IN_ESTOP)) {
				GBT_StopEV(GBT_CST_BMS_ACTIVELY_SUSPENDS);
				break;
			}

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

		case GBT_STOP:
			GBT_Delay(10);
			CONN.enableLoad = 0;
			// EV шлёт BSD (Battery Stop Data), не CST (CST шлёт EVSE)
			GBT_SendBSD();
			if (GBT_StateTick() > 10000) {
				EDCAN_printf(LOG_WARN, "CSD Timeout\n");
				GBT_Error(0xFCF0C0FD); // CSD timeout
			}
			if (GBT_CSD_recv) {
				GBT_SwitchState(GBT_STOP_CSD);
			}
			break;

		case GBT_STOP_CSD:
			// EV не шлёт CSD (финальный отчёт шлёт EVSE). Ждём 2.5 с и завершаем.
			GBT_Delay(250);
			if (GBT_StateTick() > 2500) {
				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.EvseConnected = 1;
	else CONN.EvseConnected = 0;
}



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

	if(GBT_State == GBT_DISABLED) 				EDCAN_printf(LOG_INFO, "GBT_DISABLED\n");
	if(GBT_State == GBT_EV_CONNECTING) 			EDCAN_printf(LOG_INFO, "GBT_EV_CONNECTING\n");
	if(GBT_State == GBT_EV_HANDSHAKE) 		EDCAN_printf(LOG_INFO, "GBT_EV_HANDSHAKE\n");
	if(GBT_State == GBT_EV_RECOGNITION) 	EDCAN_printf(LOG_INFO, "GBT_EV_RECOGNITION\n");
	if(GBT_State == GBT_EV_CHARGING_PARAMETERS) 	EDCAN_printf(LOG_INFO, "GBT_EV_CHARGING_PARAMETERS\n");
	if(GBT_State == GBT_EV_BMS_INIT) 	EDCAN_printf(LOG_INFO, "GBT_EV_BMS_INIT\n");
	if(GBT_State == GBT_EV_WAIT_CHARGER_READY) 	EDCAN_printf(LOG_INFO, "GBT_EV_WAIT_CHARGER_READY\n");
	if(GBT_State == GBT_EV_CHARGING) 			EDCAN_printf(LOG_INFO, "GBT_EV_CHARGING\n");
	if(GBT_State == GBT_STOP) 					EDCAN_printf(LOG_INFO, "GBT_STOP\n");
	if(GBT_State == GBT_STOP_CSD) 				EDCAN_printf(LOG_INFO, "GBT_STOP_CSD\n");
	if(GBT_State == GBT_ERROR) 					EDCAN_printf(LOG_WARN, "GBT_ERROR\n");
	if(GBT_State == GBT_COMPLETE) 				EDCAN_printf(LOG_INFO, "GBT_COMPLETE\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.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.enableLoad = 0;
	GBT_SwitchState(GBT_COMPLETE);
}

void GBT_Error(uint32_t errorcode){ // --> Suspend EV
	GBT_StopSource = GBT_STOP_EV;
	EDCAN_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.SOC = 0;
	CONN.enableLoad = 0;
	CONN.RequestedCurrent = 1000;
	CONN.RequestedVoltage = 400;
	CONN.chargingError = 0;
	memset(&GBT_EVInfo, 0, sizeof (GBT_EVInfo));
	memset(&GBT_BATStat, 0, sizeof (GBT_BATStat));
	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;
}