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

uint8_t LOCK_POLARITY = 1;
uint8_t LOCK_MOTOR_POLARITY = 1;
uint8_t LOCK_DELAY = 50;

GBT_LockState_t GBT_LockState;


void GBT_ForceLock(uint8_t state){
	if(LOCK_MOTOR_POLARITY){
		if(state){//LOCK
			  HAL_GPIO_WritePin(LOCK_B_GPIO_Port, LOCK_B_Pin, 1);
			  HAL_Delay(LOCK_DELAY);
			  HAL_GPIO_WritePin(LOCK_B_GPIO_Port, LOCK_B_Pin, 0);
		}else{ //UNLOCK
			  HAL_GPIO_WritePin(LOCK_A_GPIO_Port, LOCK_A_Pin, 1);
			  HAL_Delay(LOCK_DELAY);
			  HAL_GPIO_WritePin(LOCK_A_GPIO_Port, LOCK_A_Pin, 0);
		}
	}else{
		if(state){//LOCK
			  HAL_GPIO_WritePin(LOCK_A_GPIO_Port, LOCK_A_Pin, 1);
			  HAL_Delay(LOCK_DELAY);
			  HAL_GPIO_WritePin(LOCK_A_GPIO_Port, LOCK_A_Pin, 0);
		}else{ //UNLOCK
			  HAL_GPIO_WritePin(LOCK_B_GPIO_Port, LOCK_B_Pin, 1);
			  HAL_Delay(LOCK_DELAY);
			  HAL_GPIO_WritePin(LOCK_B_GPIO_Port, LOCK_B_Pin, 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_ManageLock(){
	uint8_t MAX_RETRIES = 5;
    if (GBT_LockState.error) {
        return;
    }

	bool lock_is_open = GBT_LockGetState() == 0;
	bool lock_should_be_open = GBT_LockState.demand == 0;
	uint8_t retry_count = 0;

	if (lock_is_open != lock_should_be_open) {
		while (retry_count < MAX_RETRIES) {
			if (lock_should_be_open) {
				GBT_ForceLock(0);
			} else {
				GBT_ForceLock(1);
			}

			lock_is_open = GBT_LockGetState() == 0;

			if (lock_is_open == lock_should_be_open) {
				break;
			}

			retry_count++;
		}

		if (retry_count >= MAX_RETRIES) {
			GBT_LockState.error = 1;
			GBT_ForceLock(0);
			printf ("Lock error\n");
		}
	}
}