/*
 **	Copyright 2023 Tjitte van der Ploeg, tjitte@tpee.nl
 **
 **	This file is part of the OpenBoost firmware.
 **	The Open-SEC firmware is free software: you can redistribute
 **	it and/or modify it under the terms of the GNU General Public License
 **	as published by the Free Software Foundation, either version 3 of the
 **	License, or (at your option) any later version. The Open-SEC firmware is
 **	distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
 **	without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
 **	PARTICULAR PURPOSE. See the GNU General Public License for more details.
 **
 **	You should have received a copy of the GNU General Public License along
 **	with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
 
#include "stm32g4xx_hal.h"
#include "delay.h"
#include "buffer.h"
#include "flash.h"
#include "crc.h"

#define LED0_Pin 			GPIO_PIN_15
#define LED0_GPIO_Port 		GPIOC
#define LED1_Pin 			GPIO_PIN_14
#define LED1_GPIO_Port 		GPIOC
#define LED2_Pin 			GPIO_PIN_13
#define LED2_GPIO_Port 		GPIOC
#define LED3_Pin 			GPIO_PIN_9
#define LED3_GPIO_Port 		GPIOB


#define LED_Pin LED0_Pin
#define LED_GPIO_Port LED0_GPIO_Port


void SystemClock_Config(void);
void Error_Handler(void);
static void MX_GPIO_Init(void);
void jumpToMainApplication(void);

#define NEW_APP_MAX_SIZE (APP_BASE - MAIN_BASE)

uint8_t   bootloaderStateCurrent;
uint8_t   bootloaderStateNext;
uint32_t  bootloaderDelayLastTick;
uint32_t  bootloaderDelayTime;
uint8_t*  newAppAdress;
uint32_t  newAppSize;
uint32_t  newAppCRC;
int32_t   indexPointer;
uint8_t   yAxisOffset;

typedef enum {
	BOOT_INIT = 0,
	BOOT_DELAY,
	BOOT_SIZE_CHECK,
	BOOT_SIZE_ZERO,
	BOOT_SIZE_WRONG,
	BOOT_SIZE_OK,
	BOOT_CRC_CHECK,
	BOOT_CRC_OK,
	BOOT_CRC_ERROR,
	BOOT_ERASE,
	BOOT_ERASE_ERROR,
	BOOT_ERASE_SUCCES,
	BOOT_COPYAPP,
	BOOT_COPYAPP_ERROR,
	BOOT_COPYAPP_SUCCES,
	BOOT_DONE,
	BOOT_REBOOT,
	BOOT_ERROR
} bootLoaderState;

int main(void) {
	HAL_Init();
	SystemClock_Config();
	MX_GPIO_Init();

	modDelayInit();

	//driverHWPowerStateInit();
	//driverHWPowerStateSetOutput(P_STAT_POWER_ENABLE,P_STAT_SET);

	newAppAdress           = (uint8_t*)APP_BASE;
	indexPointer           = 0;
	newAppSize             = buffer_get_uint32(newAppAdress, &indexPointer);
	newAppCRC              = buffer_get_uint16(newAppAdress, &indexPointer);

	indexPointer += 2;	//2 dummy butes to alaign memory with 8 bytes , dword

	bootloaderStateCurrent = BOOT_INIT;
	bootloaderStateNext    = BOOT_INIT;
	bootloaderDelayTime    = 100;

	//emptyData.Empty = 0.0f;

	bootloaderDelayLastTick = HAL_GetTick();

  while (1) {

		switch(bootloaderStateCurrent) {
			case BOOT_INIT:
			  bootloaderDelayTime    = 100;
			  bootloaderStateCurrent = BOOT_DELAY;
			  bootloaderStateNext    = BOOT_SIZE_CHECK;
			  break;

			case BOOT_DELAY:
				if(modDelayTick1ms(&bootloaderDelayLastTick,bootloaderDelayTime))
					bootloaderStateCurrent = bootloaderStateNext;
				break;

			case BOOT_SIZE_CHECK:
				if(newAppSize == 0)
					bootloaderStateNext = BOOT_SIZE_ZERO;
				else if(newAppSize > NEW_APP_MAX_SIZE)
					bootloaderStateNext = BOOT_SIZE_WRONG;
				else
					bootloaderStateNext = BOOT_SIZE_OK;

			  bootloaderDelayTime    = 100;
			  bootloaderStateCurrent = BOOT_DELAY;
			  break;

			case BOOT_SIZE_ZERO:
				bootloaderStateNext    = BOOT_ERROR;
				bootloaderDelayTime    = 100;
				bootloaderStateCurrent = BOOT_DELAY;
				break;

			case BOOT_SIZE_WRONG:
				bootloaderStateNext    = BOOT_ERROR;
				bootloaderDelayTime    = 100;
				bootloaderStateCurrent = BOOT_DELAY;
				break;

			case BOOT_SIZE_OK:
				bootloaderStateNext    = BOOT_CRC_CHECK;
				bootloaderDelayTime    = 100;
				bootloaderStateCurrent = BOOT_DELAY;
				break;

			case BOOT_CRC_CHECK:
				if(newAppCRC != libCRCCalcCRC16(newAppAdress + indexPointer, newAppSize))
					bootloaderStateNext = BOOT_CRC_ERROR;
				else
					bootloaderStateNext = BOOT_CRC_OK;

				bootloaderDelayTime    = 100;
				bootloaderStateCurrent = BOOT_DELAY;
				break;

			case BOOT_CRC_OK:
				bootloaderStateNext    = BOOT_ERASE;
				bootloaderDelayTime    = 100;
				bootloaderStateCurrent = BOOT_DELAY;
				break;
			case BOOT_CRC_ERROR:
				bootloaderStateNext    = BOOT_ERROR;
				bootloaderDelayTime    = 100;
				bootloaderStateCurrent = BOOT_DELAY;
				break;
			case BOOT_ERASE:
				// Code to erasing main flash
				if(modFlashEraseMainAppData(newAppSize) == HAL_OK)
					bootloaderStateNext    = BOOT_ERASE_SUCCES;
				else
					bootloaderStateNext    = BOOT_ERASE_ERROR;

				bootloaderDelayTime    = 100;
				bootloaderStateCurrent = BOOT_DELAY;
				break;

			case BOOT_ERASE_ERROR:
				bootloaderStateNext    = BOOT_ERROR;
				bootloaderDelayTime    = 100;
				bootloaderStateCurrent = BOOT_DELAY;
				break;

			case BOOT_ERASE_SUCCES:
				bootloaderStateNext    = BOOT_COPYAPP;
				bootloaderDelayTime    = 100;
				bootloaderStateCurrent = BOOT_DELAY;
				break;

			case BOOT_COPYAPP:
				// Code to copy new app to main flash
				if(modFlashCopyNewAppToMainApp((uint64_t*)(newAppAdress + indexPointer), newAppSize) == HAL_OK)
					bootloaderStateNext    = BOOT_COPYAPP_SUCCES;
				else
					bootloaderStateNext    = BOOT_COPYAPP_ERROR;

				bootloaderDelayTime    = 100;
			  bootloaderStateCurrent = BOOT_DELAY;
				break;
			case BOOT_COPYAPP_ERROR:
				bootloaderStateNext    = BOOT_ERROR;
				bootloaderDelayTime    = 100;
				bootloaderStateCurrent = BOOT_DELAY;
				break;
			case BOOT_COPYAPP_SUCCES:
				bootloaderStateNext    = BOOT_DONE;
				bootloaderDelayTime    = 100;
				bootloaderStateCurrent = BOOT_DELAY;
				break;
			case BOOT_DONE:
				bootloaderStateNext    = BOOT_REBOOT;
				bootloaderDelayTime    = 2000;
				bootloaderStateCurrent = BOOT_DELAY;
				break;
			case BOOT_REBOOT:
				jumpToMainApplication();
				break;
			case BOOT_ERROR:
				HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, 1);

				// Do nothing but keep showing error.
				bootloaderStateNext    = BOOT_REBOOT;
				bootloaderDelayTime    = 5000;
				bootloaderStateCurrent = BOOT_DELAY;
				break;
			default:
				break;
		}
	};
}

void jumpToMainApplication(void) {
	NVIC_SystemReset();
}

void SystemClock_Config(void) {
	RCC_OscInitTypeDef RCC_OscInitStruct = { 0 };
	RCC_ClkInitTypeDef RCC_ClkInitStruct = { 0 };
	RCC_PeriphCLKInitTypeDef PeriphClkInit = { 0 };

	// configure the main internal regulator output voltage
	HAL_PWREx_ControlVoltageScaling(PWR_REGULATOR_VOLTAGE_SCALE1_BOOST);
	/** Initializes the RCC Oscillators according to the specified parameters
	 * in the RCC_OscInitTypeDef structure.
	 */
	RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
	RCC_OscInitStruct.HSEState = RCC_HSE_ON;
	RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
	RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
	RCC_OscInitStruct.PLL.PLLM = RCC_PLLM_DIV4;
	RCC_OscInitStruct.PLL.PLLN = 85;
	RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
	RCC_OscInitStruct.PLL.PLLQ = RCC_PLLQ_DIV2;
	RCC_OscInitStruct.PLL.PLLR = RCC_PLLR_DIV2;
	if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) {
		Error_Handler();
	}
	/** Initializes the CPU, AHB and APB buses clocks
	 */
	RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK
			| RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2;
	RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
	RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
	RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1;
	RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

	if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_4) != HAL_OK) {
		Error_Handler();
	}
	/** Initializes the peripherals clocks
	 */
	PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_USART3
			| RCC_PERIPHCLK_ADC12 | RCC_PERIPHCLK_ADC345 | RCC_PERIPHCLK_FDCAN;
	PeriphClkInit.Usart3ClockSelection = RCC_USART3CLKSOURCE_PCLK1;
	PeriphClkInit.FdcanClockSelection = RCC_FDCANCLKSOURCE_PCLK1;
	PeriphClkInit.Adc12ClockSelection = RCC_ADC12CLKSOURCE_SYSCLK;
	PeriphClkInit.Adc345ClockSelection = RCC_ADC345CLKSOURCE_SYSCLK;
	if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK) {
		Error_Handler();
	}
}

static void MX_GPIO_Init(void) {
	__HAL_RCC_GPIOC_CLK_ENABLE();
	__HAL_RCC_GPIOF_CLK_ENABLE();
	__HAL_RCC_GPIOG_CLK_ENABLE();
	__HAL_RCC_GPIOA_CLK_ENABLE();
	__HAL_RCC_GPIOB_CLK_ENABLE();

	GPIO_InitTypeDef GPIO_InitStruct;
	GPIO_InitStruct.Pin = LED_Pin;
	GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
	GPIO_InitStruct.Pull = GPIO_NOPULL;
	HAL_GPIO_Init(LED_GPIO_Port, &GPIO_InitStruct);
}

void Error_Handler(void) {
  while(1){};
}

void NMI_Handler(void) {

}

void HardFault_Handler(void) {
	while (1) {

	}
}

void MemManage_Handler(void) {

	while (1) {

	}
}

void BusFault_Handler(void) {

	while (1) {

	}
}

void UsageFault_Handler(void) {

	while (1) {

	}
}

void SVC_Handler(void) {

}

void DebugMon_Handler(void) {

}

void PendSV_Handler(void) {

}

void SysTick_Handler(void) {
	HAL_IncTick();
}