/*
 * lock.c
 *
 *  Created on: Jul 31, 2024
 *      Author: colorbass
 */
#include "lock.h"
#include "debug.h"
#include "edcan.h"


uint8_t LOCK_POLARITY = 0; // 1 for v1
uint8_t LOCK_MOTOR_POLARITY = 1;
uint16_t LOCK_DELAY = 100;

GBT_LockState_t GBT_LockState = {
	.demand = 0,
	.error = 0,
	.action_requested = 255,  // нет запрошенного действия
	.motor_state = 0,         // idle
	.last_action_time = 0,
	.retry_count = 0,
	.error_tick = 0
};


void GBT_ForceLock(uint8_t state){
	// Устанавливаем флаг для выполнения действия
	GBT_LockState.action_requested = state ? 1 : 0;
	GBT_LockState.retry_count = 0;
}

uint8_t GBT_LockGetState(){
	//1 = locked
	//0 = unlocked
	if(LOCK_POLARITY){
		return HAL_GPIO_ReadPin(LOCK_FB_GPIO_Port, LOCK_FB_Pin);
	}else{
		return !HAL_GPIO_ReadPin(LOCK_FB_GPIO_Port, LOCK_FB_Pin);

	}
}

void GBT_Lock(uint8_t state){
	GBT_LockState.demand = state;
}

void GBT_ManageLockSolenoid(){
	static uint32_t tick;

	if(HAL_GetTick() - tick < 50) return;
	tick = HAL_GetTick();

	HAL_GPIO_WritePin(LOCK_B_GPIO_Port, LOCK_B_Pin, GBT_LockState.demand ? 1 : 0);
}

void GBT_ManageLockMotor(){
	static const uint8_t MAX_RETRIES = 5;
	uint32_t current_tick = HAL_GetTick();
	
	// Проверяем таймаут сброса ошибки (до проверки error, чтобы можно было сбросить)
	GBT_ResetErrorTimeout();
	
	if (GBT_LockState.error) {
		return;
	}

	// Проверяем, нужно ли выполнить действие
	bool lock_is_open = GBT_LockGetState() == 0;
	bool lock_should_be_open = GBT_LockState.demand == 0;
	
	// Если есть запрошенное действие или состояние не соответствует требуемому
	if (GBT_LockState.action_requested != 255 || (lock_is_open != lock_should_be_open)) {
		// Если действие еще не запрошено, запрашиваем его
		if (GBT_LockState.action_requested == 255) {
			GBT_LockState.action_requested = lock_should_be_open ? 0 : 1;
			GBT_LockState.retry_count = 0;
		}
		
		// Управление мотором через машину состояний
		switch (GBT_LockState.motor_state) {
			case 0: // idle - мотор выключен
				// Определяем, какой пин нужно включить
				if (LOCK_MOTOR_POLARITY) {
					if (GBT_LockState.action_requested == 1) { // LOCK
						HAL_GPIO_WritePin(LOCK_B_GPIO_Port, LOCK_B_Pin, 1);
					} else { // UNLOCK
						HAL_GPIO_WritePin(LOCK_A_GPIO_Port, LOCK_A_Pin, 1);
					}
				} else {
					if (GBT_LockState.action_requested == 1) { // LOCK
						HAL_GPIO_WritePin(LOCK_A_GPIO_Port, LOCK_A_Pin, 1);
					} else { // UNLOCK
						HAL_GPIO_WritePin(LOCK_B_GPIO_Port, LOCK_B_Pin, 1);
					}
				}
				GBT_LockState.motor_state = 1; // motor_on
				GBT_LockState.last_action_time = current_tick;
				break;
				
			case 1: // motor_on - мотор включен, ждем LOCK_DELAY
				if (current_tick - GBT_LockState.last_action_time >= LOCK_DELAY) {
					// Выключаем оба пина
					HAL_GPIO_WritePin(LOCK_A_GPIO_Port, LOCK_A_Pin, 0);
					HAL_GPIO_WritePin(LOCK_B_GPIO_Port, LOCK_B_Pin, 0);
					GBT_LockState.motor_state = 2; // waiting_off
					GBT_LockState.last_action_time = current_tick;
				}
				break;
				
			case 2: // waiting_off - ждем немного перед проверкой состояния
				// Небольшая задержка перед проверкой состояния (например, 50мс)
				if (current_tick - GBT_LockState.last_action_time >= 50) {
					// Проверяем, достигнуто ли требуемое состояние
					lock_is_open = GBT_LockGetState() == 0;
					bool action_success = (lock_is_open == (GBT_LockState.action_requested == 0));
					
					if (action_success) {
						// Действие выполнено успешно
						GBT_LockState.action_requested = 255; // сбрасываем флаг
						GBT_LockState.motor_state = 0; // idle
						GBT_LockState.retry_count = 0;
					} else {
						// Действие не выполнено, повторяем попытку
						GBT_LockState.retry_count++;
						if (GBT_LockState.retry_count >= MAX_RETRIES) {
							// Превышено количество попыток
							GBT_LockState.error = 1;
							GBT_LockState.error_tick = current_tick; // сохраняем время установки ошибки
							GBT_LockState.action_requested = 0; // пытаемся разблокировать
							GBT_LockState.motor_state = 0;
							GBT_LockState.retry_count = 0;
							EDCAN_printf(LOG_ERR, "Lock error\n");
						} else {
							// Повторяем попытку
							GBT_LockState.motor_state = 0; // возвращаемся к началу
						}
					}
				}
				break;
		}
	} else {
		// Состояние соответствует требуемому, сбрасываем флаги
		if (GBT_LockState.motor_state != 0) {
			HAL_GPIO_WritePin(LOCK_A_GPIO_Port, LOCK_A_Pin, 0);
			HAL_GPIO_WritePin(LOCK_B_GPIO_Port, LOCK_B_Pin, 0);
			GBT_LockState.motor_state = 0;
		}
		GBT_LockState.action_requested = 255;
		GBT_LockState.retry_count = 0;
	}
}

void GBT_ResetErrorTimeout(){
	static const uint32_t ERROR_TIMEOUT_MS = 300000; // 5 минут
	
	if (GBT_LockState.error && GBT_LockState.error_tick != 0) {
		
		if ((HAL_GetTick()-GBT_LockState.error_tick) >= ERROR_TIMEOUT_MS) {
			// Прошло 5 минут, сбрасываем ошибку
			GBT_LockState.error = 0;
			GBT_LockState.error_tick = 0;
			EDCAN_printf(LOG_WARN, "Lock error timeout reset\n");
		}
	}
}