/*
 * connector.c
 *
 * All initialization before 12v_aux
 */

#include "connector.h"
#include "lock.h"
#include "board.h"
#include "debug.h"


CONN_State_t connectorState;
 
extern uint8_t config_initialized;
 
gbtCcState_t CC_STATE_FILTERED;

// name  - уникальный идентификатор таймера для данного места использования
// cond  - условие, которое должно непрерывно держаться delay_ms мс
// delay_ms - задержка в миллисекундах
#define DELAYED_HOLD_MS(name, cond, delay_ms)                       \
({                                                                  \
    static uint32_t name = 0;                                       \
    uint8_t __hold_result = 0;                                      \
    if (cond) {                                                     \
        if (name == 0) {                                            \
            name = HAL_GetTick();                                   \
        } else if ((HAL_GetTick() - name) >= (uint32_t)(delay_ms)) {\
            __hold_result = 1;                                      \
            name = 0;                                               \
        }                                                           \
    } else {                                                        \
        name = 0;                                                   \
    }                                                               \
    __hold_result;                                                  \
})
 
void CONN_Task(){

	switch (connectorState){
	case Unknown: // unlocked, waiting for config
		GBT_Lock(0);
		if (config_initialized) {
			CONN_SetState(Unplugged);
		}
		break;

	case Disabled: // faulted, unlocked
		GBT_Lock(0);
		if(CONN.chargingError == 0) CONN_SetState(Unplugged);
		if(CONN.connControl == CMD_FORCE_UNLOCK) GBT_ForceLock(0);
		break;

	case Unplugged: // unlocked, waiting to connect
		GBT_Lock(0);
		if(CONN.chargingError != 0) CONN_SetState(Disabled);
		if(CONN.connControl == CMD_FORCE_UNLOCK) GBT_ForceLock(0);

		if((CONN_CC_GetState()==GBT_CC_4V) && (CONN.connControl != CMD_FORCE_UNLOCK)){
			CONN_SetState(AuthRequired);
			GBT_Lock(0);
		}
		break;

	case AuthRequired: // plugged, waiting to start charge
		GBT_Lock(0);
		if(CONN.connControl == CMD_FORCE_UNLOCK) GBT_ForceLock(0);
		if(CONN_CC_GetState()==GBT_CC_4V){
			if(CONN.connControl == CMD_START){
				CONN_SetState(Preparing);
			}
			if(CONN.connControl == CMD_FORCE_UNLOCK){
				CONN_SetState(Unplugged);
			}
			// if CHARGING_NOT_ALLOWED — stay here
		}else{
			CONN_SetState(Unplugged);
		}
		break;

	case Preparing: // charging, locked
		GBT_Lock(1);

		if(GBT_State == GBT_COMPLETE){
			if(GBT_StopSource == GBT_STOP_EVSE){
				CONN_SetState(FinishedEVSE);
			}else if(GBT_StopSource == GBT_STOP_EV){
				CONN_SetState(FinishedEV);
			}else if(GBT_StopSource == GBT_STOP_OCPP){
				CONN_SetState(Finished);
			}else{
				CONN_SetState(FinishedEVSE);
			}
		}
		if(GBT_State == GBT_S10_CHARGING){
			CONN_SetState(Charging);
		}
		break;
	case Charging: // charging, locked
			GBT_Lock(1);
	
			if(GBT_State == GBT_COMPLETE){
				if(GBT_StopSource == GBT_STOP_EVSE){
					CONN_SetState(FinishedEVSE);
				}else if(GBT_StopSource == GBT_STOP_EV){
					CONN_SetState(FinishedEV);
				}else if(GBT_StopSource == GBT_STOP_OCPP){
					CONN_SetState(Finished);
				}else{
					CONN_SetState(FinishedEVSE);
				}
			}
			break;

	case FinishedEV: // charging completed by EV, waiting to transaction stop
		GBT_Lock(0);
		CONN_SetState(Finished);
		break;

	case FinishedEVSE: // charging completed by EVSE, waiting to transaction stop
		GBT_Lock(0);
		CONN_SetState(Finished);
		break;

	case Finished: // charging completed, waiting to disconnect, unlocked
		GBT_Lock(0);
 
		//TODO Force unlock time limit
		if(CONN.connControl == CMD_FORCE_UNLOCK) GBT_ForceLock(0);
 
		if(DELAYED_HOLD_MS(cc6v_hold_tick, CONN_CC_GetState()==GBT_CC_6V, 5000)){
			GBT_Lock(0);
			CONN_SetState(Unplugged);
		}
		break;

	default:
		CONN_SetState(Unknown);
	}

}
//external
//CONN_SetState(Disabled);

void CONN_SetState(CONN_State_t state){
	connectorState = state;

	if(connectorState == Unknown) 				log_printf(LOG_INFO, "Connector: Unknown\n");
	if(connectorState == Unplugged) 			log_printf(LOG_INFO, "Connector: Unplugged\n");
	if(connectorState == Disabled) 				log_printf(LOG_INFO, "Connector: Disabled\n");
	if(connectorState == Preparing) 			log_printf(LOG_INFO, "Connector: Preparing\n");
	if(connectorState == AuthRequired) 			log_printf(LOG_INFO, "Connector: AuthRequired\n");
	if(connectorState == WaitingForEnergy) 		log_printf(LOG_INFO, "Connector: WaitingForEnergy\n");
	if(connectorState == ChargingPausedEV) 		log_printf(LOG_INFO, "Connector: ChargingPausedEV\n");
	if(connectorState == ChargingPausedEVSE)	log_printf(LOG_INFO, "Connector: ChargingPausedEVSE\n");
	if(connectorState == Charging)				log_printf(LOG_INFO, "Connector: Charging\n");
	if(connectorState == AuthTimeout) 			log_printf(LOG_INFO, "Connector: AuthTimeout\n");
	if(connectorState == Finished)				log_printf(LOG_INFO, "Connector: Finished\n");
	if(connectorState == FinishedEVSE)			log_printf(LOG_INFO, "Connector: FinishedEVSE\n");
	if(connectorState == FinishedEV)			log_printf(LOG_INFO, "Connector: FinishedEV\n");
	if(connectorState == Replugging)			log_printf(LOG_INFO, "Connector: Replugging\n");

	CONN.connState = state;
}

void CONN_CC_ReadStateFiltered() {
    static uint32_t last_change_time = 0;
    static uint32_t last_check_time = 0;
    static uint8_t prev_state = 0;

    if((HAL_GetTick()-last_check_time)<100) return;

    last_check_time = HAL_GetTick();

    uint8_t new_state = CONN_CC_GetStateRaw();

    if (new_state != prev_state) {
        last_change_time = HAL_GetTick();
        prev_state = new_state;
    } else if ((HAL_GetTick() - last_change_time) >= 300) {
        CC_STATE_FILTERED = prev_state;
    }
}

uint8_t CONN_CC_GetState(){
	return CC_STATE_FILTERED;
}
uint8_t CONN_CC_GetStateRaw(){
	float volt = CONN_CC_GetAdc();
//	if((volt<12.6f) && (volt>11.4f)) return GBT_CC_12V;
//	if((volt<6.8f) && (volt>5.2f)) return GBT_CC_6V;
//	if((volt<4.8f) && (volt>3.2f)) return GBT_CC_4V;
//	if((volt<2.8f) && (volt>1.2f)) return GBT_CC_2V;
	if((volt<13.0f) && (volt>11.0f)) return GBT_CC_12V;
	if((volt<7.2f) && (volt>4.8f)) return GBT_CC_6V;
	if((volt<4.8f) && (volt>3.0f)) return GBT_CC_4V;
	if((volt<3.0f) && (volt>1.0f)) return GBT_CC_2V;
	return GBT_CC_UNKNOWN;
}

float CONN_CC_GetAdc(){
	//TODO: Filters
	//Vref=3.3v = 4095
	//k=1/11
	//Vin = 12v
	//Vin*k= 1.09v
	//12vin = 1353 ADC

	uint32_t adc;
	float volt;
	ADC_Select_Channel(ADC_CHANNEL_3);
	HAL_ADC_Start(&hadc1);
	HAL_ADC_PollForConversion(&hadc1, 100);
	adc = HAL_ADC_GetValue(&hadc1);
	HAL_ADC_Stop(&hadc1);

	volt = (float)adc/113.4f;

	return volt;
}