/*
 * safety.c
 *
 *  Created on: 12 May 2021
 *      Author: Dmitrijs
 */

#include "main.h"
#include "dbw.h"
#include "ts_comms.h"
#include "safety.h"

const char msg1[42] = { "PPS1 Safety margin >4095 or less then 0\r\n" };
const char msg2[42] = { "PPS2 Safety margin >4095 or less then 0\r\n" };
const char msg3[42] = { "TPS1 Safety margin >4095 or less then 0\r\n" };
const char msg4[42] = { "TPS2 Safety margin >4095 or less then 0\r\n" };

void Flash_Write_Protect(void) {

	FLASH_OBProgramInitTypeDef p;
	p.OptionType = OPTIONBYTE_WRP;
	p.WRPState = OB_WRPSTATE_ENABLE;

	HAL_FLASH_Unlock();
	HAL_FLASH_OB_Unlock();
	HAL_FLASHEx_OBErase();

	p.WRPPage = 0x00003FFF;

	HAL_FLASHEx_OBProgram(&p);
	FLASH_WaitForLastOperation((uint32_t) FLASH_TIMEOUT_VALUE);

	HAL_FLASH_OB_Lock();
	HAL_FLASH_Lock();
	HAL_FLASH_OB_Launch();

}

void Flash_Write_Unprotect(void) {

	FLASH_OBProgramInitTypeDef p;
	p.OptionType = OPTIONBYTE_WRP;
	p.WRPState = OB_WRPSTATE_DISABLE;

	HAL_FLASH_Unlock();
	HAL_FLASH_OB_Unlock();
	HAL_FLASHEx_OBErase();

	p.WRPPage = 0x00003FFF;

	HAL_FLASHEx_OBProgram(&p);
	FLASH_WaitForLastOperation((uint32_t) FLASH_TIMEOUT_VALUE);

	HAL_FLASH_OB_Lock();
	HAL_FLASH_Lock();
	HAL_FLASH_OB_Launch();

}

void Watch_Dog_Update(void) {
	IWDG->KR = 0x0000AAAA;		// update wd timer
}

void Watch_Dog_Init(void) {
	IWDG->KR = 0x0000CCCC;		// enable watchdog
	IWDG->KR = 0x00005555;		// enable register access
	while (IWDG->SR)
		;
	IWDG->PR = 0x00000000;		// prescaler LSI / 4
	while (IWDG->SR)
		;
	IWDG->RLR = 1000; 			// set timeout to 100mS
	while (IWDG->SR)
		;
	IWDG->KR = 0x0000AAAA;		// enable register access
	while (IWDG->SR)
		;

}

void Check_Safety_Limits(void) {

//PPS1

	if (config->pps1_min < config->pps1_max) {
		if ((config->pps1_margin > config->pps1_min)
				|| ((config->pps1_max + config->pps1_margin) > 4095)) {
			//config errror
			var.status0 |= DBW_STATUS0_CONF_ERROR_F;
			DBW_Stop();
			TX_Schedule((unsigned char*) msg1, 42);
		}
	} else {
		//config->pps1_min > config->pps1_max
		if ((config->pps1_margin > config->pps1_max)
				|| ((config->pps1_min + config->pps1_margin) > 4095)) {
			//config errror
			var.status0 |= DBW_STATUS0_CONF_ERROR_F;
			DBW_Stop();
			TX_Schedule((unsigned char*) msg1, 42);
		}
	}
//PPS2

	if (config->pps2_min < config->pps2_max) {
		if ((config->pps2_margin > config->pps2_min)
				|| ((config->pps2_max + config->pps2_margin) > 4095)) {
			//config errror
			var.status0 |= DBW_STATUS0_CONF_ERROR_F;
			DBW_Stop();
			TX_Schedule((unsigned char*) msg2, 42);
		}
	} else {
		//config->pps2_min > config->pps1_max
		if ((config->pps2_margin > config->pps2_max)
				|| ((config->pps2_min + config->pps2_margin) > 4095)) {
			//config errror
			var.status0 |= DBW_STATUS0_CONF_ERROR_F;
			DBW_Stop();
			TX_Schedule((unsigned char*) msg2, 42);
		}
	}

//TPS1

	if (config->tps1_min < config->tps1_max) {
		if ((config->tps1_margin > config->tps1_min)
				|| ((config->tps1_max + config->tps1_margin) > 4095)) {
			//config errror
			var.status0 |= DBW_STATUS0_CONF_ERROR_F;
			DBW_Stop();
			TX_Schedule((unsigned char*) msg3, 42);
		}
	} else {
		//config->tps1_min > config->tps1_max
		if ((config->tps1_margin > config->tps1_max)
				|| ((config->tps1_min + config->tps1_margin) > 4095)) {
			//config errror
			var.status0 |= DBW_STATUS0_CONF_ERROR_F;
			DBW_Stop();
			TX_Schedule((unsigned char*) msg3, 42);
		}
	}
//TPS2

	if (config->tps2_min < config->tps2_max) {
		if ((config->tps2_margin > config->tps2_min)
				|| ((config->tps2_max + config->tps2_margin) > 4095)) {
			//config errror
			var.status0 |= DBW_STATUS0_CONF_ERROR_F;
			DBW_Stop();
			TX_Schedule((unsigned char*) msg4, 42);
		}
	} else {
		//config->tps2_min > config->tps1_max
		if ((config->tps2_margin > config->tps2_max)
				|| ((config->tps2_min + config->tps2_margin) > 4095)) {
			//config errror
			var.status0 |= DBW_STATUS0_CONF_ERROR_F;
			DBW_Stop();
			TX_Schedule((unsigned char*) msg4, 42);
		}
	}

}

void Check_Adc_Range(void) {

//PPS1
	if (config->pps1_min < config->pps1_max) {

		if (var.pps1_adc < (config->pps1_min - config->pps1_margin)) {
			//Fault condition PPS1 min
			var.status0 |= DBW_STATUS0_SENSOR_FAULT_F;
			DBW_Stop();
			var.status3 = STATUS3_PPS1_SS_GND;
		}

		if (var.pps1_adc > (config->pps1_max + config->pps1_margin)) {
			//Fault condition PPS1 MAX
			var.status0 |= DBW_STATUS0_SENSOR_FAULT_F;
			DBW_Stop();
			var.status3 = STATUS3_PPS1_SS_VREF;
		}
	} else {
		//config->pps1_min < config->pps1_max
		if (var.pps1_adc < (config->pps1_max - config->pps1_margin)) {
			//Fault condition PPS1 min
			var.status0 |= DBW_STATUS0_SENSOR_FAULT_F;
			DBW_Stop();
			var.status3 = STATUS3_PPS1_SS_GND;
		}

		if (var.pps1_adc > (config->pps1_min + config->pps1_margin)) {
			//Fault condition PPS1 MAX
			var.status0 |= DBW_STATUS0_SENSOR_FAULT_F;
			DBW_Stop();
			var.status3 = STATUS3_PPS1_SS_VREF;
		}
	}

//PPS2
	if (config->pps2_min < config->pps2_max) {
		if (var.pps2_adc < (config->pps2_min - config->pps2_margin)) {
			//Fault condition PPS1 min
			var.status0 |= DBW_STATUS0_SENSOR_FAULT_F;
			DBW_Stop();
			var.status3 = STATUS3_PPS2_SS_GND;
		}

		if (var.pps2_adc > (config->pps2_max + config->pps2_margin)) {
			//Fault condition PPS1 MAX
			var.status0 |= DBW_STATUS0_SENSOR_FAULT_F;
			DBW_Stop();
			var.status3 = STATUS3_PPS1_SS_VREF;
		}
	} else {

		if (var.pps2_adc < (config->pps2_max - config->pps2_margin)) {
			//Fault condition PPS1 min
			var.status0 |= DBW_STATUS0_SENSOR_FAULT_F;
			DBW_Stop();
			var.status3 = STATUS3_PPS2_SS_GND;
		}

		if (var.pps2_adc > (config->pps2_min + config->pps2_margin)) {
			//Fault condition PPS1 MAX
			var.status0 |= DBW_STATUS0_SENSOR_FAULT_F;
			DBW_Stop();
			var.status3 = STATUS3_PPS1_SS_VREF;
		}
	}

//PPS Delta
	if ((var.pps_delta > config->pps_delta_margin)
			|| (var.pps_delta < ((int16_t) -config->pps_delta_margin))) {
		//Fault condition PPS DELTA
		var.status0 |= DBW_STATUS0_SENSOR_FAULT_F;
		DBW_Stop();
		var.status3 = STATUS3_PPS_DELTA;
	}

//##########################################
	//TPS1
	if (config->tps1_min < config->tps1_max) {
		if (var.tps1_adc < (config->tps1_min - config->tps1_margin)) {
			//Fault condition PPS1 min
			var.status0 |= DBW_STATUS0_SENSOR_FAULT_F;
			DBW_Stop();
			var.status3 = STATUS3_TPS1_SS_GND;
		}

		if (var.tps1_adc > (config->tps1_max + config->tps1_margin)) {
			//Fault condition TPS1 MAX
			var.status0 |= DBW_STATUS0_SENSOR_FAULT_F;
			DBW_Stop();
			var.status3 = STATUS3_TPS1_SS_VREF;
		}
	} else {

		if (var.tps1_adc < (config->tps1_max - config->tps1_margin)) {
			//Fault condition PPS1 min
			var.status0 |= DBW_STATUS0_SENSOR_FAULT_F;
			DBW_Stop();
			var.status3 = STATUS3_TPS1_SS_GND;
		}

		if (var.tps1_adc > (config->tps1_min + config->tps1_margin)) {
			//Fault condition TPS1 MAX
			var.status0 |= DBW_STATUS0_SENSOR_FAULT_F;
			DBW_Stop();
			var.status3 = STATUS3_TPS1_SS_VREF;
		}
	}

	//TPS2
	if (config->tps2_min < config->tps2_max) {
		if (var.tps2_adc < (config->tps2_min - config->tps2_margin)) {
			//Fault condition PPS1 min
			var.status0 |= DBW_STATUS0_SENSOR_FAULT_F;
			DBW_Stop();
			var.status3 = STATUS3_TPS2_SS_GND;
		}

		if (var.tps2_adc > (config->tps2_max + config->tps2_margin)) {
			//Fault condition TPS2 MAX
			var.status0 |= DBW_STATUS0_SENSOR_FAULT_F;
			DBW_Stop();
			var.status3 = STATUS3_TPS2_SS_VREF;
		}
	} else {
		if (var.tps2_adc < (config->tps2_max - config->tps2_margin)) {
			//Fault condition PPS1 min
			var.status0 |= DBW_STATUS0_SENSOR_FAULT_F;
			DBW_Stop();
			var.status3 = STATUS3_TPS2_SS_GND;
		}

		if (var.tps2_adc > (config->tps2_min + config->tps2_margin)) {
			//Fault condition TPS2 MAX
			var.status0 |= DBW_STATUS0_SENSOR_FAULT_F;
			DBW_Stop();
			var.status3 = STATUS3_TPS2_SS_VREF;
		}
	}

	//TPS Delta
	if ((var.tps_delta > config->tps_delta_margin)
			|| (var.tps_delta < ((int16_t) -config->tps_delta_margin))) {
		//Fault condition PPS DELTA
		var.status0 |= DBW_STATUS0_SENSOR_FAULT_F;
		DBW_Stop();
		var.status3 = STATUS3_TPS_DELTA;
	}

}

volatile int dbw_target_tmr1, dbw_target_tmr2;

void Safety_TPS_Safety_Timer(void) {
	if (dbw_target_tmr1 > 0)
		dbw_target_tmr1--;
	if (dbw_target_tmr2 > 0)
		dbw_target_tmr2--;
}

void Safety_TPS_Safety_Timer_Start(void) {
//enable timers. add 1S time to start, because safety system starts in 1S after startup
	dbw_target_tmr1 = config->tps_error_time1 + 1000;
	dbw_target_tmr2 = config->tps_error_time2 + 1000;
}

void Check_TPS_Target() {
	if (var.tps_error > 0) {
		//check if tps is within specified target
		if (var.tps_error < config->tps_error_margin1)
			dbw_target_tmr1 = config->tps_error_time1;
		else {
			// if not in range in specified time
			if (dbw_target_tmr1 == 0) {
				//fault
				var.status0 |= DBW_STATUS0_FAULT_F;
				DBW_Stop();
				var.status2 = STATUS3_TARGET1_FAULT;
			}
		}

		if (var.tps_error < config->tps_error_margin2)
			dbw_target_tmr2 = config->tps_error_time2;
		else {
			// if not in range in specified time
			if (dbw_target_tmr2 == 0) {
				//fault
				var.status0 |= DBW_STATUS0_FAULT_F;
				DBW_Stop();
				var.status2 = STATUS3_TARGET2_FAULT;
			}
		}
	}

	else {
		//tps_error<0

		//check if tps is within specified target
		if (var.tps_error > (-config->tps_error_margin1))
			dbw_target_tmr1 = config->tps_error_time1;
		else {
			// if not in range in specified time
			if (dbw_target_tmr1 == 0) {
				//fault
				var.status0 |= DBW_STATUS0_FAULT_F;
				DBW_Stop();
				var.status2 = STATUS3_TARGET1_FAULT;
			}
		}

		if (var.tps_error > (-config->tps_error_margin2))
			dbw_target_tmr2 = config->tps_error_time2;
		else {
			// if not in range in specified time
			if (dbw_target_tmr2 == 0) {
				//fault
				var.status0 |= DBW_STATUS0_FAULT_F;
				DBW_Stop();
				var.status2 = STATUS3_TARGET2_FAULT;
			}
		}
	}

}