/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2025 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "adc.h"
#include "crc.h"
#include "dma.h"
#include "i2c.h"
#include "iwdg.h"
#include "rtc.h"
#include "spi.h"
#include "tim.h"
#include "ucpd.h"
#include "usart.h"
#include "usbpd.h"
#include "usb_device.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include <stdio.h>
#include "sysdata.h"
#include "wh_counter.h"
#include "ah_counter.h"
#include "eeprom.h"
#include "modbus.h"
#include "chip_temperature.h"
#include "battery_voltage.h"
#include "ads1260.h"
#include "shunt_voltage.h"
#include "fast_current.h"
#include "int_bat_voltage.h"
#include "chip_temperature.h"
#include "shunt_temperature.h"
#include "esr.h"
#include "ads1260.h"
#include "outputs.h"
#include "crc.h"
#include "efficiency.h"
#include "self_discharge.h"
#include "ltc_current.h"
#include "usbd_cdc_if.h"
#include "ring.h"

/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

#define MAX_OVERCURRENT_RETRYS	3
#ifdef DEBUG
#define MX_IWDG_Init DoNothing
#endif
//#define USBPD_DPM_Run DoNothing

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */

uint32_t bootLoaderMark __attribute__((section(".no_init")));

modbus_t modbusData;


__IO uint32_t adc1Data[SAMPLE_ARRAY_SIZE];
__IO uint32_t adc2Data[SAMPLE_ARRAY_SIZE];
__IO uint32_t adc3Data[3];
__IO uint32_t adc4Data[SAMPLE_ARRAY_SIZE];
__IO uint32_t adc5Data[4];
int silentmode =0;
static volatile uint32_t newADC124Data = 0;
static volatile uint32_t newADC3Data = 0;
static volatile uint32_t newADC5Data = 0;

ring_buffer_t usb_in_buff __attribute__((section(".CCM_RAM1")));
ring_buffer_t usb_out_buff __attribute__((section(".CCM_RAM1")));

/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */

bool SetFlashReadProtection(bool state);
uint8_t printprotectionstate(void);
bool SetBootFromFlashAndReadOutProtection(void);
void LoadBackupRegister(void);
void SaveBackupRegister(void);
void JumpToBootloader(void);
void CheckRAMHWParity(void);
void DoNothing(void){};
void processUSB(ring_buffer_t* const ring_in_buff, ring_buffer_t* const ring_out_buff);

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */

/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{

  /* USER CODE BEGIN 1 */

  CheckRAMHWParity();

  if (bootLoaderMark == GOTO_SYSTEM_BOOTLOADER_MARK)
  {
	  bootLoaderMark = 0;
	  JumpToBootloader();
  }

  uint8_t firstStartCatcher;
  int mode_button_disable_time=0;
  uint32_t oldTime = 0;
  uint32_t newTime = 0;
  uint32_t timeDiff;
  uint8_t sekunde = 0;
  uint8_t sekundeOld = 0;
  uint32_t dummyDate;

  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */
  printf("System core clock: %uMHz\n", SystemCoreClock / 1000000U);
  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_DMA_Init();
  MX_ADC1_Init();
  MX_ADC2_Init();
  MX_ADC3_Init();
  MX_ADC4_Init();
  MX_ADC5_Init();
  MX_I2C3_Init();
  MX_I2C4_Init();
  MX_SPI3_Init();
  MX_USART1_UART_Init();
  MX_USART2_UART_Init();
  MX_CRC_Init();
  MX_RTC_Init();
  MX_IWDG_Init();
  MX_TIM3_Init();
  MX_USB_Device_Init();
  MX_UCPD1_Init();
  /* USER CODE BEGIN 2 */
  printf("Test debug io\r\n");
  SYS_DATA_Init();
  WH_COUNTER_Init();
  AH_COUNTER_Init();

  startType_t startType = EEPROM_isFirstStart();

  switch(startType)
  {
	  case FIRST_START_AFTER_ERASE:					EEPROM_fullRestore(&sys_data);		  break;
	  case FIRST_START_AFTER_COMPARTIBLE_UPDATE:	EEPROM_readConfig(&sys_data);		  break;
	  case FIRST_START_AFTER_INCOMPARTIBLE_UPDATE:	EEPROM_factoryRestore(&sys_data, 0);  break;  // Preserving calibration and settings
	  case FIRST_START_ERROR:						EEPROM_fullRestore(&sys_data);		  break;
  }
  HAL_IWDG_Refresh(&hiwdg);

  if(HAL_GPIO_ReadPin(GPIO_INPUT_BTN_MODE_GPIO_Port, GPIO_INPUT_BTN_MODE_Pin) == GPIO_PIN_RESET)
  {
    HAL_Delay(50);
    if(HAL_GPIO_ReadPin(GPIO_INPUT_BTN_MODE_GPIO_Port, GPIO_INPUT_BTN_MODE_Pin) == GPIO_PIN_RESET)
    {
      printf("factory restore...\n");
      EEPROM_factoryRestore(&sys_data, 1);
    }
  }
  HAL_IWDG_Refresh(&hiwdg);
  LoadBackupRegister();

  // Modbus Initialisierung
  mbInit(&modbusData, sys_data.s.parameter.baudrate, sys_data.s.parameter.parityMode, sys_data.s.parameter.stopBit, &huart2);
  HAL_IWDG_Refresh(&hiwdg);
    // STM32G0 Chiptemperatur Kalibrierung
  CHIP_TEMPERATURE_Calibration();

  HAL_ADCEx_Calibration_Start(&hadc1, ADC_DIFFERENTIAL_ENDED);
  HAL_ADCEx_Calibration_Start(&hadc2, ADC_DIFFERENTIAL_ENDED);
  HAL_ADCEx_Calibration_Start(&hadc3, ADC_SINGLE_ENDED);
  HAL_ADCEx_Calibration_Start(&hadc4, ADC_DIFFERENTIAL_ENDED);
  HAL_ADCEx_Calibration_Start(&hadc5, ADC_SINGLE_ENDED);
  HAL_IWDG_Refresh(&hiwdg);
  
  //SET_BIT(hadc2.Instance->CFGR, ADC_CFGR_DMAEN); //Enable DMA transfer for ADC slave (ADC12_CCR.MDMA = 0b00 -> MDMA mode disabled)
  //HAL_DMA_Start(hadc2.DMA_Handle,(uint32_t)&hadc2.Instance->DR, (uint32_t)adc2Data,1); //Start ADC slave DMA 
  //SET_BIT(hadc1.Instance->CFGR, ADC_CFGR_DMAEN); //Enable DMA transfer for ADC master (ADC12_CCR.MDMA = 0b00 -> MDMA mode disabled)
  if (HAL_ADC_Start_DMA(&hadc1, (uint32_t*)adc1Data,SAMPLE_ARRAY_SIZE))		Error_Handler();	  // Start ADC interleaved mode
  if (HAL_ADC_Start_DMA(&hadc2, (uint32_t*)adc2Data,SAMPLE_ARRAY_SIZE))		Error_Handler();	  // Start ADC interleaved mode
  if (HAL_ADC_Start_DMA(&hadc3, (uint32_t*)adc3Data, 3))					Error_Handler();
  if (HAL_ADC_Start_DMA(&hadc4, (uint32_t*)adc4Data, SAMPLE_ARRAY_SIZE))	Error_Handler();
  if (HAL_ADC_Start_DMA(&hadc5, (uint32_t*)adc5Data, 4))					Error_Handler();

  // ADS1260 Initialierung
  HAL_IWDG_Refresh(&hiwdg);
  ADS1260_init();
  HAL_IWDG_Refresh(&hiwdg);
  printf("ADS1260 Init\n");
  OUTPUTS_Init();

  //Display Adress with Error LED blinks
  for (int n = 0; n < sys_data.s.parameter.slave_address; n++)
  {
    HAL_GPIO_WritePin(LED_ERROR_GPIO_Port, LED_ERROR_Pin, GPIO_PIN_SET);
	HAL_GPIO_WritePin(BUZZER_GPIO_Port, BUZZER_Pin, GPIO_PIN_SET);
    HAL_Delay(350);
	HAL_IWDG_Refresh(&hiwdg);
    HAL_GPIO_WritePin(LED_ERROR_GPIO_Port, LED_ERROR_Pin, GPIO_PIN_RESET);
	HAL_GPIO_WritePin(BUZZER_GPIO_Port, BUZZER_Pin, GPIO_PIN_RESET);
    HAL_Delay(350);
	HAL_IWDG_Refresh(&hiwdg);
  }

  /* USER CODE END 2 */

  /* USBPD initialisation ---------------------------------*/
  MX_USBPD_Init();

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
    /* USER CODE END WHILE */
    USBPD_DPM_Run();

    /* USER CODE BEGIN 3 */
	HAL_IWDG_Refresh(&hiwdg);
	if (newADC124Data == 1)
    {
	  SHUNT_VOLTAGE_Exec( adc4Data[0]);
	  //Mit ADC_DIV2,Sample time 12,5Cycklen, ADC Clock 50Mhz, Oversampling 256
	  //Tconv = 6400 Takte = 0,128ms Pro Konvertierung. Also für 100 messwerte 12,8mS
	  BATTERY_VOLTAGE_Exec( adc2Data[SAMPLE_ARRAY_SIZE-1]);
	  FAST_CURRENT_Exec(adc1Data[SAMPLE_ARRAY_SIZE-1]); 
	  ESR_FAST_Exec();
	  sys_data.s.values.batteryPower = ((int64_t) sys_data.s.values.batteryVoltage * (int64_t)sys_data.s.values.batteryCurrent) / 1000LL;
	  sys_data.s.values.power_Fast = ((int64_t) sys_data.s.values.batteryVoltage * (int64_t)sys_data.s.values.fast_current) / 1000LL;
	  newADC124Data = 0;

	  if (HAL_ADC_Start_DMA(&hadc1,(uint32_t*)adc1Data,SAMPLE_ARRAY_SIZE)) Error_Handler(); //Start ADC interleaved mode
	  if (HAL_ADC_Start_DMA(&hadc2,(uint32_t*)adc2Data,SAMPLE_ARRAY_SIZE)) Error_Handler(); //Start ADC interleaved mode
	}

	if (newADC3Data == 1)
    {
	  SHUNT_TEMPERATURE_Exec(adc3Data[0]);
	  LTC_CURRENT_Exec(adc3Data[1], adc3Data[2]);
	}

	if (newADC5Data == 1)
    {
	  CHIP_TEMPERATURE_Exec(adc5Data[0]);
	  INT_BAT_VOLTAGE_Exec( adc5Data[1]);
	  sys_data.s.values.ovp_sense =  (adc5Data[2] * VREF * 21 ) / 65536.0;
      sys_data.s.values.lvp_sense =  (adc5Data[3] * VREF * 21 ) / 65536.0;
	}

	if (newCurrentValue == 1)
    {
      ADS1260_ConversionFinished(); 
      //sys_data.s.values.esrCalcStatus = ESR_Exec();

      newCurrentValue = 0;
    }

		
	// Zeitbasis ms Systick;
    newTime = HAL_GetTick();
    
	if (newTime != oldTime)
	{
	  oldTime = newTime;
      
	  if (mode_button_disable_time > 0) 
      {
        mode_button_disable_time--;
      }

	}
   
	if (sys_data.s.values.overcurrentRetryCounter > MAX_OVERCURRENT_RETRYS)
	{
	  printf("SHORT_CIRCUIT_PROTECTION\n");
	  sys_data.s.values.lvpState = OUTPUTS_LVP_SHORT_PROTECTION;
	  OUTPUTS_OverwriteDischargeCtrl_DischargeDisabled();

	  sys_data.s.values.lvpState = OUTPUTS_OVP_SHORT_PROTECTION;
	  OUTPUTS_OverwriteChargeCtrl_ChargeDisabled();
	}

    // Zeitbasis Sekunde über RTC
    // muss immer sowohl das Zeit- als auch das Datumsregister lesen
    // da beide Shadowregister sind sonst wird die Zeit nicht mehr geupdatet
    sekunde = hrtc.Instance->TR & 0x0000000F;
    dummyDate = hrtc.Instance->DR;



    if (sekundeOld != sekunde)
    {
      sekundeOld = sekunde;

      sys_data.s.values.onTime++;
	  sys_data.s.values.lastTimeVbatEmpty++;
	  sys_data.s.values.lastTimeVbatFull++;
      // Funktions Led blinken
      if (silentmode == 0)
      {
        HAL_GPIO_TogglePin(LED_FUNCTION_GPIO_Port, LED_FUNCTION_Pin);
		//HAL_GPIO_TogglePin(AUX_EN_GPIO_Port, AUX_EN_Pin);
      }

      // Amperestundenzhler
      AH_COUNTER_Exec();

      WH_COUNTER_Exec();

      // LVP
      OUTPUTS_CheckLVP();

      // OVP
      OUTPUTS_CheckOVP();

	  // AUX
	  OUTPUTS_CheckAUX();

      SaveBackupRegister();

      sys_data.s.values.selfDischargeStatus = SELF_DISCHARGE_Exec();

      sys_data.s.values.efficiency = EFFICIENCY_Exec();

#ifdef DEBUG
	  /*if (sys_data.s.values.onTime % 5U == 0U)
	  {
		  static char* Buf = "Hello everybody!\r\n";
		  const unsigned BufLen = strlen(Buf);
		  CDC_Transmit_FS((uint8_t*)Buf, BufLen - 1);
	  }*/
#endif
    }



	if(sys_data.s.parameter.command != 0)
    {
      if  (modbusData.current_query ==  MB_QUERY_NOTHING)
      {
        //printf("CMD = %d\n", sys_data.s.parameter.command);
        switch (sys_data.s.parameter.command )
        {
          case COMMAND_STORE_CONFIG:                          EEPROM_storeConfig(&sys_data,0);											  break;
          case COMMAND_FULL_RESTORE:                          EEPROM_fullRestore(&sys_data);											  break;
          case COMMAND_FACTORY_RESTORE:                       EEPROM_factoryRestore(&sys_data, 1);										  break;
          case COMMAND_RESTORE_LAST_SAVED_VALUES:             EEPROM_readConfig(&sys_data);												  break;
          case COMMAND_STORE_WITH_SERIAL_NUMBER:              EEPROM_storeConfig(&sys_data,1);											  break;	// Seriennummer schreiben
          case COMMAND_RESTART:                               while(1);																	  break;
          case COMMAND_BATTERY_CURRENT_OFFSET_CAL:            
				ADS_1260_BatteryCurrentOffsetCalibrationStart(&sys_data); 
				CurrentOffsetCal(adc1Data[SAMPLE_ARRAY_SIZE - 1]);  
				break;
          case COMMAND_BATTERY_CURRENT_OFFSET_COMMONMODE_CAL: ADS_1260_BatteryCurrentOffsetCommonModeErrorComepensationStart(&sys_data);  break;
          case COMMAND_BATTERY_CURRENT_OFFSET_TEMP_CAL:       ADS_1260_BatteryCurrentOffsetTemperatureErrorComepensationStart();		  break;
          case COMMAND_BATTERY_CURRENT_GAIN_CAL:              
				ADS_1260_BatteryCurrentGainCalibrationStart(&sys_data); 
				CurrentGainCal(adc1Data[SAMPLE_ARRAY_SIZE - 1]);	  
				break;
          case COMMAND_BATTERY_CURRENT_GAIN_TEMP_SHUNT_CAL:   ADS_1260_BatteryCurrentGainTemperatureCalibrationShuntStart();			  break;
//        case COMMAND_BATTERY_CURRENT_GAIN_TEMP_CHIP_CAL:    ADS_1260_BatteryCurrentGainTemperatureCalibrationChipStart(); break;
          case COMMAND_SET_RDP_LEVEL0:                        SetFlashReadProtection(false);											  break;
          case COMMAND_SET_RDP_LEVEL1:                        SetFlashReadProtection(true);												  break;
          case COMMAND_SET_RDP_LEVEL1_AND_BOOTSEL:            SetBootFromFlashAndReadOutProtection();									  break;
		  case COMMAND_GOTO_SYS_BOOTLOADER:					  bootLoaderMark = GOTO_SYSTEM_BOOTLOADER_MARK;
															  HAL_NVIC_SystemReset();
          default:                                            printf("UNKNOWN COMMAND\n");
        }
        sys_data.s.parameter.command = 0;
      }
      else
      {
        //printf("wait with execution till modbus communnikation finished\n");
      }
    }

    if((HAL_GPIO_ReadPin(GPIO_INPUT_BTN_MODE_GPIO_Port, GPIO_INPUT_BTN_MODE_Pin) == GPIO_PIN_RESET) && (mode_button_disable_time == 0))
    {
      HAL_Delay(10);
      //Taste weiterhin gedrckt?
      if(HAL_GPIO_ReadPin(GPIO_INPUT_BTN_MODE_GPIO_Port, GPIO_INPUT_BTN_MODE_Pin) == GPIO_PIN_RESET)
      {
        //Ja, dann Silent Mode umschalten
        mode_button_disable_time=500;
        if (silentmode == 0)
        {
          silentmode = 1;
          HAL_GPIO_WritePin(LED_FUNCTION_GPIO_Port, LED_FUNCTION_Pin,GPIO_PIN_SET);
        }
        else
        {
          silentmode = 0;
        }
       }
    }

	// Modbus Kommunikation

//	printf("data12d1=%d,data12d2=%d,data5=%d\r\n", adc12Data[0], adc12Data[1] , adc5Data[2]);
    if (mbGetFrameComplete(&modbusData) == true)
    {
      if (mbSlaveCheckModbusRtuQuery(&modbusData) == RESPOND_TO_QUERY)
      {
          if (silentmode == 0)
          {
            mbSlaveProcessRtuQuery(&modbusData);
          }
      }
      else
      {
        huart1.RxState = HAL_UART_STATE_BUSY_RX;
      }
    }

	if (rb_available(&usb_in_buff) > 8)											// 8 minimum packet size in Modbus RTU RHR/WHR
	{	// Theres something in our USB buffer. We need to process it
		processUSB(&usb_in_buff, &usb_out_buff);
	}

	if (rb_available(&usb_out_buff) > 0)
	{
		static uint32_t lastTime_ms;

		uint16_t data_num = rb_available(&usb_out_buff);

		static uint8_t out_buff[1U + 1U + 1U + 250U + 2U];

		for (int i = 0; i < data_num; ++i)
		{
			uint8_t data;
			rb_pop(&usb_out_buff, &data);
			out_buff[i] = data;
		}

		CDC_Transmit_FS(out_buff, data_num);

		lastTime_ms = HAL_GetTick();
	}
  
  }
  /* USER CODE END 3 */
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  /** Configure the main internal regulator output voltage
  */
  HAL_PWREx_ControlVoltageScaling(PWR_REGULATOR_VOLTAGE_SCALE1);

  /** Configure LSE Drive Capability
  */
  HAL_PWR_EnableBkUpAccess();
  __HAL_RCC_LSEDRIVE_CONFIG(RCC_LSEDRIVE_LOW);

  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI|RCC_OSCILLATORTYPE_HSI48
                              |RCC_OSCILLATORTYPE_LSI|RCC_OSCILLATORTYPE_HSE
                              |RCC_OSCILLATORTYPE_LSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.LSEState = RCC_LSE_ON;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
  RCC_OscInitStruct.LSIState = RCC_LSI_ON;
  RCC_OscInitStruct.HSI48State = RCC_HSI48_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLM = RCC_PLLM_DIV2;
  RCC_OscInitStruct.PLL.PLLN = 16;
  RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV8;
  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_3) != HAL_OK)
  {
    Error_Handler();
  }
}

/* USER CODE BEGIN 4 */

void processUSB(ring_buffer_t* const ring_in_buff, ring_buffer_t* const ring_out_buff)
{
	const uint32_t MAX_DELAY_ms = 1000U;
	static uint32_t lastTime;
	static int stage;
	uint8_t adr, data;

	switch (stage)
	{
		case 0:	// Initial stage. We are waiting for our slave address in input buffer
			while (rb_peek(ring_in_buff, &adr) && adr != sys_data.s.parameter.slave_address) // Removing elements from ring buffer, till we get a byte with our salve address
			{
				rb_pop(ring_in_buff, &data);
#ifdef DEBUG
				printf("USB: Removed 0x%02X from input buffer\n", data);
#endif
			}
	
			uint8_t offset = 0U;
			uint8_t fc;
			bool r = rb_peek_at(ring_in_buff, ++offset, &fc);					// Testing function code, if present
			if (r == false)
			{
				lastTime = HAL_GetTick();										// Saving time of arrival of last byte
				++stage;
				return;
			}

			switch (fc)
			{
				case FC_READ_HOLDING_REGISTERS: {
					uint8_t start_addr_hi;
					r = rb_peek_at(ring_in_buff, ++offset, &start_addr_hi);
					if (r == false)
					{
						lastTime = HAL_GetTick();								// Saving time of arrival of last byte
						++stage;
						return;
					}

					uint8_t start_addr_lo;
					r = rb_peek_at(ring_in_buff, ++offset, &start_addr_lo);
					if (r == false)
					{
						lastTime = HAL_GetTick();								// Saving time of arrival of last byte
						++stage;
						return;
					}

					uint16_t start_addr = (((uint16_t)start_addr_hi) << 8U) | ((uint16_t)start_addr_lo);  // Modbus register start address
#ifdef DEBUG
					printf("USB: Start address is 0x%04X\n", start_addr);
#endif
					uint8_t reg_num_hi;
					r = rb_peek_at(ring_in_buff, ++offset, &reg_num_hi);
					if (r == false)
					{
						lastTime = HAL_GetTick();								// Saving time of arrival of last byte
						++stage;
						return;
					}

					uint8_t reg_num_lo;
					r = rb_peek_at(ring_in_buff, ++offset, &reg_num_lo);
					if (r == false)
					{
						lastTime = HAL_GetTick();								// Saving time of arrival of last byte
						++stage;
						return;
					}

					uint16_t reg_num = (((uint16_t)reg_num_hi) << 8U) | ((uint16_t)reg_num_lo);  // Modbus Quantity of Registers
#ifdef DEBUG
					printf("USB: Register number is %u(0x%04X)\n", reg_num, reg_num);
#endif
					uint8_t crc_hi;
					r = rb_peek_at(ring_in_buff, ++offset, &crc_hi);
					if (r == false)
					{
						lastTime = HAL_GetTick();								// Saving time of arrival of last byte
						++stage;
						return;
					}

					uint8_t crc_lo;
					r = rb_peek_at(ring_in_buff, ++offset, &crc_lo);
					if (r == false)
					{
						lastTime = HAL_GetTick();								// Saving time of arrival of last byte
						++stage;
						return;
					}

					uint16_t crc = (((uint16_t)crc_hi) << 8U) | ((uint16_t)crc_lo);  // Modbus CRC
#ifdef DEBUG
					printf("USB: CRC from data packet is %u(0x%04X)\n", crc, crc);
#endif
					const uint8_t in_buff[6U] = { adr, fc, start_addr_hi, start_addr_lo, reg_num_hi, reg_num_lo };
					uint16_t calc_crc = mbCrc16(in_buff, 6U);
					calc_crc = __REV16(calc_crc);
#ifdef DEBUG
					printf("USB: Calculated CRC is %u(0x%04X)\n", calc_crc, calc_crc);
#endif
					if (calc_crc != crc)										// Transmission error or bad packet
					{
						stage = 0;												// Starting all over again
						return;
					}

					for (int i = 0; i < 8; ++i) rb_pop(ring_in_buff, &data);	// If CRC Ok, then removing recognized bytes from ring buffer to free some place as soon as possible

					// Now we need to send a respond
					static uint8_t out_buff[1U + 1U + 1U + 250U + 2U];
					out_buff[0U] = adr;
					out_buff[1U] = fc;
					out_buff[2U] = reg_num * 2U;
					for (uint16_t i = 3U, j = start_addr; j < reg_num; i += 2U, ++j)
					{
						//printf("i = %u, j = %u\n", i , j);
						out_buff[i] = sys_data.mb[j].b[1];
						out_buff[i + 1U] = sys_data.mb[j].b[0];
					}
					uint16_t resp_crc = mbCrc16(out_buff, 1U + 1U + 1U + reg_num * 2U);
					resp_crc = __REV16(resp_crc);
					out_buff[1U + 1U + 1U + reg_num * 2U] = resp_crc >> 8U;
					out_buff[1U + 1U + 1U + reg_num * 2U + 1U] = (resp_crc & 0x00FF);

					/*for (int i = 0; i < 1 + 1 + 1 + reg_num * 2 + 2; ++i)
						printf("0x%02X ", out_buff[i]);
					printf("\n");*/

					// Putting prepared packet into output buffer
					for (int i = 0; i < 1 + 1 + 1 + reg_num * 2 + 2; ++i)
						rb_push(ring_out_buff, out_buff[i]);
					}
					break;

				case FC_WRITE_SINGLE_REGISTER:
					break;
	
				case FC_WRITE_MULTIPLE_REGISTER:
					break;
	
				case FC_READ_COILS:
				default:
					break;
			}
			break;

		case 1:
			break;

		case 2:
			break;
	}
}

//------------------------------------------------------------------------------

void LoadBackupRegister(void)
{
    if (HAL_RTCEx_BKUPRead(&hrtc, RTC_BKP_DR1) == 0xBEBE)
    {
	  printf("BackupRegisterData available\r\n");
	  sys_data.s.values.dischargeTotalAh = HAL_RTCEx_BKUPRead(&hrtc, RTC_BKP_DR2);

	  sys_data.s.values.chargeTotalAh = HAL_RTCEx_BKUPRead(&hrtc, RTC_BKP_DR3);
	  sys_data.s.values.dischargeTotalWh = HAL_RTCEx_BKUPRead(&hrtc, RTC_BKP_DR4);
	  sys_data.s.values.chargeTotalWh = HAL_RTCEx_BKUPRead(&hrtc, RTC_BKP_DR5);

	  sys_data.s.values.onTime = HAL_RTCEx_BKUPRead(&hrtc, RTC_BKP_DR6);


	  sys_data.s.values.detectedCapacity = HAL_RTCEx_BKUPRead(&hrtc, RTC_BKP_DR7);
	  sys_data.s.values.detectedEnergy = HAL_RTCEx_BKUPRead(&hrtc, RTC_BKP_DR8);


	  sys_data.s.values.mAh_AutoMode = (int32_t) HAL_RTCEx_BKUPRead(&hrtc, RTC_BKP_DR9);
	  sys_data.s.values.mWh_AutoMode = (int32_t) HAL_RTCEx_BKUPRead(&hrtc, RTC_BKP_DR10);
	  sys_data.s.values.mAhCounter =  (int32_t) HAL_RTCEx_BKUPRead(&hrtc, RTC_BKP_DR11);
	  sys_data.s.values.mWhCounter =  (int32_t) HAL_RTCEx_BKUPRead(&hrtc, RTC_BKP_DR12);

	  sys_data.s.values.mAh_AutoModeU = (int32_t) HAL_RTCEx_BKUPRead(&hrtc, RTC_BKP_DR13);


	  sys_data.s.values.mAs_AutoMode = sys_data.s.values.mAh_AutoMode * 3600LL;
	  sys_data.s.values.mAs_AutoModeU = sys_data.s.values.mAh_AutoModeU * 3600LL;
	  
	  sys_data.s.values.mWs_AutoMode = sys_data.s.values.mWh_AutoMode * 3600LL;
	  sys_data.s.values.mAsCounter = sys_data.s.values.mAhCounter  * 3600LL; 



	}


}

//------------------------------------------------------------------------------

void SaveBackupRegister(void)
{
	static uint32_t lastTime;

	
	uint32_t time = sys_data.s.values.onTime;

	// Alle 24 Stunden führen wir ein Speicherbefehl durch um die Counter zu speichern
	if (time != lastTime)
	{
		lastTime = time;
		HAL_PWR_EnableBkUpAccess();
		HAL_RTCEx_BKUPWrite(&hrtc, RTC_BKP_DR1, 0xBEBE);

		// Total Energie und Ah Umsatz der Batterie (seit inbetriebnahme)
	    HAL_RTCEx_BKUPWrite(&hrtc, RTC_BKP_DR2, sys_data.s.values.dischargeTotalAh);
		HAL_RTCEx_BKUPWrite(&hrtc, RTC_BKP_DR3, sys_data.s.values.chargeTotalAh);
		HAL_RTCEx_BKUPWrite(&hrtc, RTC_BKP_DR4, sys_data.s.values.dischargeTotalWh);
		HAL_RTCEx_BKUPWrite(&hrtc, RTC_BKP_DR5, sys_data.s.values.chargeTotalWh);

		// Betriebszeit
		HAL_RTCEx_BKUPWrite(&hrtc, RTC_BKP_DR6, sys_data.s.values.onTime);

		// Erkannte Kapazitäten
		HAL_RTCEx_BKUPWrite(&hrtc, RTC_BKP_DR7, sys_data.s.values.detectedCapacity);
		HAL_RTCEx_BKUPWrite(&hrtc, RTC_BKP_DR8, sys_data.s.values.detectedEnergy);


		HAL_RTCEx_BKUPWrite(&hrtc, RTC_BKP_DR9, sys_data.s.values.mAh_AutoMode);
		HAL_RTCEx_BKUPWrite(&hrtc, RTC_BKP_DR10, sys_data.s.values.mWh_AutoMode);
		HAL_RTCEx_BKUPWrite(&hrtc, RTC_BKP_DR11, sys_data.s.values.mAhCounter);
		HAL_RTCEx_BKUPWrite(&hrtc, RTC_BKP_DR12, sys_data.s.values.mWhCounter);
		
		HAL_RTCEx_BKUPWrite(&hrtc, RTC_BKP_DR13, sys_data.s.values.mAh_AutoModeU);
	}
}

//------------------------------------------------------------------------------

 void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef* hadc)
 {
    if (hadc->Instance==ADC1)
	{
	  newADC124Data=1;
	}

	if (hadc->Instance==ADC3)
	{
	  newADC3Data=1;
	}

	if (hadc->Instance==ADC5)
	{
	  newADC5Data=1;
	}
 }

//------------------------------------------------------------------------------
 
/**

  * @brief  Set flash read protection.

  * @param  [in] state: Flash read protection state, true: enable protection, false: disable protection.

  * @retval true:  Successful operation.

  * @retval false: Operation failed.

  */

bool SetFlashReadProtection(bool state)
{

  FLASH_OBProgramInitTypeDef OptionsBytesStruct = {0};
  HAL_FLASHEx_OBGetConfig(&OptionsBytesStruct);

  if(state == true)
  {
    printf("Start enable readout protection\n");
    if(OptionsBytesStruct.RDPLevel == OB_RDP_LEVEL_0)
    {
      OptionsBytesStruct.OptionType = OPTIONBYTE_RDP;
      OptionsBytesStruct.RDPLevel   = OB_RDP_LEVEL_1;
      if (HAL_FLASH_Unlock() != HAL_OK)
      {
        printf("Flash unlock error\n");
      }
      if (HAL_FLASH_OB_Unlock() != HAL_OK)
      {
        printf("Flash ob unlock error\n");
      }

      printf("...Flash unlock\n");
      if(HAL_FLASHEx_OBProgram(&OptionsBytesStruct) != HAL_OK)
      {
        printf("...Enable lock error\n");
        HAL_FLASH_OB_Lock();
        return false;
      }
      HAL_FLASH_OB_Lock();
      printf("Flash Optionbyte locked\n");
      HAL_FLASH_Lock();
      printf("Flash  locked\n");
      printf("...Enable lock process finished\n");
    }
    else
    {
      printf("...Flash lock already active\n");
    }
  }
  else
  {
    if(OptionsBytesStruct.RDPLevel == OB_RDP_LEVEL_1)
    {
      OptionsBytesStruct.OptionType = OPTIONBYTE_RDP;
      OptionsBytesStruct.RDPLevel   = OB_RDP_LEVEL_0;

      if (HAL_FLASH_Unlock() != HAL_OK)
      {
        printf("Flash unlock error\n");
        return false;
      }
      printf("...Flash unlocked\n");

      if (HAL_FLASH_OB_Unlock() != HAL_OK)
      {
        printf("Flash ob unlock error\n");
        return false;
      }
      printf("...Flash ob unlocked\n");

      if(HAL_FLASHEx_OBProgram(&OptionsBytesStruct) != HAL_OK)
      {
        HAL_FLASH_OB_Lock();
        printf("Flash Optionbyte programm failed\n");
        return false;
      }

      printf("Flash Optionbyte programmed\n");
      HAL_FLASH_OB_Lock();
      printf("Flash Optionbyte locked\n");
      HAL_FLASH_Lock();
      printf("Flash  locked\n");
      printf("...Disable lock process finished\n");
    }
  }
  return true;
}

//------------------------------------------------------------------------------

void CheckRAMHWParity(void)
{
	if (FLASH->OPTR & FLASH_OPTR_SRAM_PE)
	{	// RAM HW Parity is not activated
		if (HAL_OK != HAL_FLASH_Unlock())
		{
#ifdef DEBUG
			printf("Cannot unlock FLASH!\n");
#endif
			return;
		}

		__HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_OPTVERR);								// Clear OPTVERR bit set on virgin samples

		if (HAL_OK != HAL_FLASH_OB_Unlock())
		{
#ifdef DEBUG
			printf("Cannot unlock Option Bytes!\n");
#endif
			return;
		}

		// Activating SRAM1(first 32Kb) and CCM SRAM parity check (see RM0440, page 139)
		FLASH->OPTR &= ~FLASH_OPTR_SRAM_PE;
        while (FLASH->SR & FLASH_SR_BSY) {};
        FLASH->CR |= FLASH_CR_OPTSTRT;
        while (FLASH->SR & FLASH_SR_BSY) {};
        // Updating option bytes by resetting of the device
        FLASH->CR |= FLASH_CR_OBL_LAUNCH;
		while (1)
#ifdef DEBUG
		  printf("Option Bytes write ERROR!\n")
#endif
		;
	}
	else
	{
#ifdef DEBUG
		printf("RAM Parity check was activated\n");
#endif
	}
}

//------------------------------------------------------------------------------

bool SetBootFromFlashAndReadOutProtection(void)
{

  FLASH_OBProgramInitTypeDef OptionsBytesStruct = {0};
  HAL_FLASHEx_OBGetConfig(&OptionsBytesStruct);

  //Set Readout Protection Level 1
  OptionsBytesStruct.OptionType = OPTIONBYTE_USER | OPTIONBYTE_RDP;
  OptionsBytesStruct.RDPLevel   = OB_RDP_LEVEL_1;

  //Selecting Boot from Main Flash Memory
  OptionsBytesStruct.USERType =    OB_USER_nBOOT0 | OB_USER_nSWBOOT0 | OB_USER_nBOOT1 ;
  OptionsBytesStruct.USERConfig  = OB_USER_nBOOT0 | OB_USER_nSWBOOT0;

  if (HAL_FLASH_Unlock() != HAL_OK) printf("Flash unlock error\n");
  if (HAL_FLASH_OB_Unlock() != HAL_OK) printf("Flash ob unlock error\n");

  printf("...Flash unlock\n");
  if(HAL_FLASHEx_OBProgram(&OptionsBytesStruct) != HAL_OK)
  {
    printf("...Enable lock error\n");
    HAL_FLASH_OB_Lock();
    return false;
  }
  HAL_FLASH_OB_Lock();
  printf("Flash Optionbyte locked\n");
  HAL_FLASH_Lock();
  printf("Flash  locked\n");
  printf("...Enable lock process finished\n");

  return true;
}

//------------------------------------------------------------------------------

uint8_t printprotectionstate(void)
{
  FLASH_OBProgramInitTypeDef OptionsBytesStruct = {0};
  HAL_FLASHEx_OBGetConfig(&OptionsBytesStruct);
  uint8_t result = 0;

  if(OptionsBytesStruct.RDPLevel == OB_RDP_LEVEL_0)
  {
    //OptionsBytesStruct.OptionType = OPTIONBYTE_RDP;
    //OptionsBytesStruct.RDPLevel   = OB_RDP_LEVEL_1;
    printf("PROTECTION: OB_RDP_LEVEL_0\n");
    result = 0;
  }
  else if(OptionsBytesStruct.RDPLevel == OB_RDP_LEVEL_1)
  {
    printf("PROTECTION: OB_RDP_LEVEL_1\n");
    result = 1;
  }
  else if(OptionsBytesStruct.RDPLevel == OB_RDP_LEVEL_2)
  {
    printf("PROTECTION: OB_RDP_LEVEL_2\n");
    result = 2;
  }
  return  result;
}

//------------------------------------------------------------------------------

void JumpToBootloader (void)
{
	typedef void (SystemBootloader)(void);
#ifdef DEBUG
	printf("Cannot jump in DEBUG mode!\n");
	return;
#endif
	// Disable all interrupts
	__disable_irq();

#ifdef DEINIT_RCC
	// Set the clock to the default state
	HAL_RCC_DeInit();
#endif
#ifdef DEINIT_SYSTICK
	// Disable Systick timer
	SysTick->CTRL = SysTick->LOAD = SysTick->VAL = 0;
#endif
	// Clear Interrupt Enable Register & Interrupt Pending Register
	for (int i = 0; i < sizeof(NVIC->ICER) / sizeof(NVIC->ICER[0]); i++)
    {
      NVIC->ICER[i] = 0xFFFFFFFF;
      NVIC->ICPR[i] = 0xFFFFFFFF;
    }

	// Set up the jump to boot loader address + 4
	SystemBootloader* sb = (SystemBootloader*) (*((uint32_t *) ((SYSTEM_BOOTLOADER_START_ADDR + 4))));

	// Set the main stack pointer to the bootloader stack
	__set_MSP(*(uint32_t *)SYSTEM_BOOTLOADER_START_ADDR);

	__HAL_RCC_SYSCFG_CLK_ENABLE();
	__HAL_SYSCFG_REMAPMEMORY_SYSTEMFLASH();
	//__HAL_RCC_SYSCFG_CLK_DISABLE();

	// Re-enable all interrupts
	__enable_irq();

	// Call the function to jump to boot loader location
	sb();

	// Jump is done successfully
	while (1)
	{
		// Code should never reach this loop
		printf("Jump has failed!\n");
	}
}

/* USER CODE END 4 */

/**
  * @brief  This function is executed in case of error occurrence.
  * @retval None
  */
void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */
  __disable_irq();
  printf("error handler\r\n"); //Eventuell zeile entfernen, um besser den callstack zu sehen

  while (1)
  {
  }
  /* USER CODE END Error_Handler_Debug */
}
#ifdef USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t *file, uint32_t line)
{
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
	 printf("Wrong parameters value: file %s on line %d\r\n", file, line);
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */
