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

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


CONN_State_t connectorState;
extern GBT_EDCAN_Output_t GBT_EDCAN_Output;
extern GBT_EDCAN_Input_t GBT_EDCAN_Input;

uint8_t CC_STATE_FILTERED;

void CONN_Init(){
	CONN_SetState(CONN_Initializing);
}

void CONN_Task(){

	switch (connectorState){
	case CONN_Initializing: // unlocked
		GBT_Lock(0);
		CONN_SetState(CONN_Available);
		GBT_LockState.error = 0;
		break;
	case CONN_Faulted: //unlocked
		GBT_Lock(0);

		break;
	case CONN_Available: //unlocked, waiting to connect
		GBT_Lock(0);
		GBT_LockState.error = 0;
		if((CONN_CC_GetState()==GBT_CC_4V) && (GBT_EDCAN_Input.chargeControl != FORCE_UNLOCK)){ // Исправить
			CONN_SetState(CONN_Preparing);
			GBT_Lock(1);

		}
		break;

		// Выйти из двух состояний в Finished если force unlock
	case CONN_Preparing: //locked, waiting to charge
		GBT_Lock(1);
		if(CONN_CC_GetState()==GBT_CC_4V){
			if(GBT_EDCAN_Input.chargeControl == CHARGING_ALLOWED){
//				RELAY_Write(RELAY_AUX, 1);
//				GBT_Start();
				CONN_SetState(CONN_Charging);
			}
			if(GBT_EDCAN_Input.chargeControl == FORCE_UNLOCK){
				CONN_SetState(CONN_Available);//TODO: CONN_Occupied_complete
			}
			//if (CHARGING_NOT_ALLOWED) stay here
		}else{
			CONN_SetState(CONN_Available);
		}
		break;
	case CONN_Charging://charging, locked
		GBT_Lock(1);

		if(GBT_State == GBT_COMPLETE){
			CONN_SetState(CONN_Finishing);
		}
		//

		break;
	case CONN_Finishing://charging completed, waiting to disconnect, unlocked
		GBT_Lock(0);
//		RELAY_Write(RELAY_AUX, 0);
		//TODO: Reconnection
//		if(GBT_EDCAN_Input.chargeControl == CHARGING_NOT_ALLOWED){
//			CONN_SetState(CONN_Initializing);
//		}
		if(CONN_CC_GetState()==GBT_CC_6V){
			CONN_SetState(CONN_Initializing);
		}
		//Проблема, если нажать кнопку и не вынуть пистолет, то он снова блочится
		break;
	default:
		CONN_SetState(CONN_Initializing);

	}


}
//external
//CONN_SetState(CONN_Error);
//CONN_SetState(CONN_Occupied_charging);
//CONN_SetState(CONN_Occupied_Complete);

void CONN_SetState(CONN_State_t state){
	connectorState = state;
	if(connectorState == CONN_Initializing) 		printf ("CONN_Initializing\n");
	if(connectorState == CONN_Faulted) 			printf ("CONN_Error\n");
	if(connectorState == CONN_Available) 		printf ("CONN_Available\n");
	if(connectorState == CONN_Preparing) printf ("CONN_Occupied_waiting\n");
	if(connectorState == CONN_Charging)	printf ("CONN_Occupied_charging\n");
	if(connectorState == CONN_Finishing)	printf ("CONN_Occupied_complete\n");
	GBT_EDCAN_Output.connectorState = state;
}

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

//    if((last_check_time+100)>HAL_GetTick()) return;
    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;
    }

//	switch(new_state){
//	case GBT_CC_UNKNOWN:
//		printf("GBT_CC_UNKNOWN\n");
//		break;
//	case GBT_CC_12V:
//		printf("GBT_CC_12V\n");
//		break;
//	case GBT_CC_6V:
//		printf("GBT_CC_6V\n");
//		break;
//	case GBT_CC_4V:
//		printf("GBT_CC_4V\n");
//		break;
//	case GBT_CC_2V:
//		printf("GBT_CC_2V\n");
//		break;
//
//	}
//	switch(CONN_CC_GetState()){
//	case GBT_CC_UNKNOWN:
//		printf("FGBT_CC_UNKNOWN\n");
//		break;
//	case GBT_CC_12V:
//		printf("FGBT_CC_12V\n");
//		break;
//	case GBT_CC_6V:
//		printf("FGBT_CC_6V\n");
//		break;
//	case GBT_CC_4V:
//		printf("FGBT_CC_4V\n");
//		break;
//	case GBT_CC_2V:
//		printf("FGBT_CC_2V\n");
//		break;
//
//	}
}

uint8_t CONN_CC_GetState(){
	return CC_STATE_FILTERED;
}
uint8_t CONN_CC_GetStateRaw(){
	//Vref=3.3v = 4095
	//k=1/11
	//Vin = 12v
	//Vin*k= 1.09v
	//12vin = 1353 ADC
//TODO: Filter 100ms
	uint32_t adc;
	float volt;
	ADC_Select_Channel(ADC_CHANNEL_6);
	HAL_ADC_Start(&hadc1);
	HAL_ADC_PollForConversion(&hadc1, 100);
	adc = HAL_ADC_GetValue(&hadc1);
	HAL_ADC_Stop(&hadc1);

	volt = (float)adc/113.4f;
//	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_6);
	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;
}