/* 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 "usb_device.h"
#include "USBHandler.h"
#include "usbd_cdc_if.h"
#include "adar1000.h"
#include "ADAR1000_Manager.h"
extern "C" {
#include "ad9523.h"
}
#include "no_os_delay.h"
#include "no_os_alloc.h"
#include "no_os_print_log.h"
#include "no_os_error.h"
#include "no_os_units.h"
#include "no_os_dma.h"
#include "no_os_spi.h"
#include "no_os_uart.h"
#include "no_os_util.h"
#include <stdint.h>
#include <errno.h>
#include <math.h>
#include <stdio.h>
#include <string.h>
#include <inttypes.h>
#include <iostream>
#include <vector>
#include "stm32_spi.h"
#include "stm32_delay.h"
#include "TinyGPSPlus.h"
extern "C" {
#include "GY_85_HAL.h"
}
#include "BMP180.h"
extern "C" {
#include "platform_noos_stm32.h"
}
extern "C" {

#include "adf4382a_manager.h"
#include "adf4382.h"
#include "no_os_delay.h"
#include "no_os_error.h"
}
#include <cmath>
#include "DAC5578.h"
#include "ADS7830.h"
#include "gps_handler.h"

#include "stm32f7xx_hal.h"




/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */

/* USER CODE END Includes */

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


/* USER CODE END PTD */

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


/* -------- GPIO index mapping for no-OS interface -------- */
#define GPIO_IDX_TX_CS       0
#define GPIO_IDX_RX_CS       1
#define GPIO_IDX_TX_DELSTR   2
#define GPIO_IDX_TX_DELADJ   3
#define GPIO_IDX_RX_DELSTR   4
#define GPIO_IDX_RX_DELADJ   5
#define GPIO_IDX_TX_CE       6
#define GPIO_IDX_RX_CE       7
#define GPIO_IDX_SW_SYNC     8

/* USER CODE END PD */

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

/* USER CODE END PM */

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

I2C_HandleTypeDef hi2c1;
I2C_HandleTypeDef hi2c2;
I2C_HandleTypeDef hi2c3;

SPI_HandleTypeDef hspi1;
SPI_HandleTypeDef hspi4;

TIM_HandleTypeDef htim1;

UART_HandleTypeDef huart5;
UART_HandleTypeDef huart3;

/* USER CODE BEGIN PV */
// The TinyGPSPlus object
TinyGPSPlus gps;

// Global data structures
GPS_Data_t current_gps_data = {0};

// Global USB handler
USBHandler usbHandler;
extern USBD_HandleTypeDef hUsbDeviceFS;

// Buffer for USB reception
uint8_t usb_rx_buffer[64];

//IMU variable
GY85_t imu;

float Pitch_Sensor, Yaw_Sensor, Roll_Sensor;

float abias[3] = {-0.108, -0.038, -0.006}, gbias[3] = {-10, 6, -12}, mbias[3]={0.0,0.0,0.0};
float M11=1.0,  M12=-0.0,  M13=0.0,
      M21=0.0,  M22=1.0,   M23=0.0,
      M31=0.0,  M32=-0.0,  M33=1.0;
float ax, ay, az, gx, gy, gz, mx, my, mz,mxc,myc,mzc; // variables to hold latest sensor data values
float q[4] = {1.0f, 0.0f, 0.0f, 0.0f};    // vector to hold quaternion
float temperature;
float Mag_Declination = -0.61; //0°

float RxAcc,RyAcc,RzAcc;
float Rate_Ayz_1,Rate_Axz_1,Rate_Axy_1;
float Axz_1,Ayz_1,Axy_1, Axz_0,Ayz_0,Axy_0;
float RxEst_0,RyEst_0,RzEst_0, RxEst_1,RyEst_1,RzEst_1,Azr_Est;
float RxGyro,RyGyro,RzGyro, R_Est;

unsigned long now_timeperiod;
unsigned long lasttime_timeperiod = 0;
int Time_Period = 0;

//Barometer BMP180

/*resolution:
BMP180_ULTRALOWPOWER - pressure oversampled 1 time  & power consumption 3μA
BMP180_STANDARD      - pressure oversampled 2 times & power consumption 5μA
BMP180_HIGHRES       - pressure oversampled 4 times & power consumption 7μA
BMP180_ULTRAHIGHRES  - pressure oversampled 8 times & power consumption 12μA, library default
*/
BMP180 myBMP(BMP180_ULTRAHIGHRES);
float RADAR_Altitude;

//GPS
double RADAR_Longitude = 0;
double RADAR_Latitude = 0;

extern uint8_t GUI_start_flag_received;


//RADAR
// Radar parameters
const int m_max = 32;  // Number of chirps per beam position
const int n_max = 31;   // Number of beam positions
const float T1 = 30.0f; // Chirp duration in microseconds
const float PRI1 = 167.0f; // Pulse repetition interval in microseconds
const float T2 = 0.5f;  // Short chirp duration in microseconds
const float PRI2 = 175.0f; // Short PRI in microseconds
const float Guard = 175.4f; // Guard time in microseconds

uint8_t	m = 1; // m = N° of chirp/ position = 16 (made of T1 and PRF1)+ Guard = 175µs +16 (made of T2 and PRF2)
uint8_t	n = 1; // n =[1-31] N° of elevations/azimuth
uint8_t	y = 1;// y = N° of azimuths/revolution = 50 controlled by a microcontroller connected to a stepper motor
uint8_t y_max = 50;// y = N° of azimuths/revolution
uint64_t IF_freq = 120000000ULL;//120MHz

#define PowerAmplifier 1

//Stepper Motor
float Stepper_steps = 200.0f;//step per revolution

// DAC5578 handles (RF Power Amplifier DAC controlling Vg)
DAC5578_HandleTypeDef hdac1, hdac2;
/* Phase accumulators */
float phase_dac1[8] = {0};
float phase_dac2[8] = {0};
const uint32_t sampleRate = 370;    // Sample rate in Hz
const uint32_t period = 2700;       // Period in microseconds
uint8_t DAC_val = 126;

// ADC handles (RF Power Amplifier ADC measuring Idq)
ADS7830_HandleTypeDef hadc1, hadc2;
uint8_t adc1_readings[8] = {0};
uint8_t adc2_readings[8] = {0};
float Idq_reading[16]={0.0f};


// Global manager instance ADF4382A
ADF4382A_Manager lo_manager;
extern SPI_HandleTypeDef hspi4;

//ADAR1000

ADAR1000Manager adarManager;
static uint8_t matrix1[15][16];
static uint8_t matrix2[15][16];
static uint8_t vector_0[16] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};

/* Radar parameters
const int m_max = 32;  // Number of chirps per beam position
const int n_max = 31;   // Number of beam positions
const float T1 = 30.0f; // Chirp duration in microseconds
const float PRI1 = 167.0f; // Pulse repetition interval in microseconds
const float T2 = 0.5f;  // Short chirp duration in microseconds
const float T2 = 175.0f; // Short PRI in microseconds
const float Guard = 175.40f; // Guard time in microseconds*/

//Temperature Sensors
ADS7830_HandleTypeDef hadc3;
float Temperature_1 = 0.0f, Temperature_2 = 0.0f, Temperature_3 = 0.0f, Temperature_4 = 0.0f;
float Temperature_5 = 0.0f, Temperature_6 = 0.0f, Temperature_7 = 0.0f, Temperature_8 = 0.0f;

// Phase differences for 31 beam positions
const float phase_differences[31] = {
    160.0f, 80.0f, 53.333f, 40.0f, 32.0f, 26.667f, 22.857f, 20.0f, 17.778f, 16.0f,
    14.545f, 13.333f, 12.308f, 11.429f, 10.667f, 0.0f,
    -10.667f, -11.429f, -12.308f, -13.333f, -14.545f, -16.0f, -17.778f, -20.0f,
    -22.857f, -26.667f, -32.0f, -40.0f, -53.333f, -80.0f, -160.0f
};

// Convenience HAL wrappers for AD9523 control pins (GPIOF as you said)
static inline void AD9523_PD(bool v)        { HAL_GPIO_WritePin(GPIOF, AD9523_PD_Pin, v ? GPIO_PIN_SET : GPIO_PIN_RESET); }
static inline void AD9523_REF_SEL(bool v)   { HAL_GPIO_WritePin(GPIOF, AD9523_REF_SEL_Pin, v ? GPIO_PIN_SET : GPIO_PIN_RESET); }
static inline void AD9523_SYNC_PULSE(void)  { HAL_GPIO_WritePin(GPIOF, AD9523_SYNC_Pin, GPIO_PIN_SET); HAL_Delay(1); HAL_GPIO_WritePin(GPIOF, AD9523_SYNC_Pin, GPIO_PIN_RESET); }
static inline void AD9523_RESET_ASSERT()    { HAL_GPIO_WritePin(GPIOF, AD9523_RESET_Pin, GPIO_PIN_RESET); } // check polarity
static inline void AD9523_RESET_RELEASE()   { HAL_GPIO_WritePin(GPIOF, AD9523_RESET_Pin, GPIO_PIN_SET); }
static inline void AD9523_CS(bool v)        { HAL_GPIO_WritePin(GPIOF, AD9523_CS_Pin, v ? GPIO_PIN_SET : GPIO_PIN_RESET); }
static inline void AD9523_EEPROM_SEL(bool v){ HAL_GPIO_WritePin(GPIOF, AD9523_EEPROM_SEL_Pin, v ? GPIO_PIN_SET : GPIO_PIN_RESET); }

/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
void PeriphCommonClock_Config(void);
static void MPU_Config(void);
static void MX_GPIO_Init(void);
static void MX_TIM1_Init(void);
static void MX_I2C1_Init(void);
static void MX_I2C2_Init(void);
static void MX_I2C3_Init(void);
static void MX_SPI1_Init(void);
static void MX_SPI4_Init(void);
static void MX_UART5_Init(void);
static void MX_USART3_UART_Init(void);
/* USER CODE BEGIN PFP */

// Function prototypes
void systemPowerUpSequence();
void systemPowerDownSequence();
void initializeBeamMatrices();
void runRadarPulseSequence();
void executeChirpSequence(int num_chirps, uint32_t T1, uint32_t PRI1, uint32_t T2, uint32_t PRI2);
void printSystemStatus();

//////////////////////////////////////////////
////////////////////micros()//////////////////
//////////////////////////////////////////////
unsigned long micros(void){
unsigned long Micros = __HAL_TIM_GET_COUNTER(&htim1);
return Micros; //Clock TIM1 -> AHB/APB1 is set to 72MHz/presc+1   presc = 71
}

//////////////////////////////////////////////
//////////////////Delay_us()//////////////////
//////////////////////////////////////////////

void delay_us(volatile uint32_t us){
__HAL_TIM_SET_COUNTER(&htim1,0);  // set the counter value a
while (__HAL_TIM_GET_COUNTER(&htim1) < us);  // //Clock TIMx -> AHB/APB1 is set to 72MHz/presc+1   presc = 71
}

//////////////////////////////////////////////
//////////////////delay_ns()//////////////////
//////////////////////////////////////////////
void delay_ns(uint32_t nanoseconds)
{
    // Get the starting cycle count
    uint32_t start_cycles = DWT->CYCCNT;

    // Convert nanoseconds to the number of CPU clock cycles
    // Use `SystemCoreClock` to get the current CPU frequency.
    // Use 64-bit math to prevent overflow for large nanosecond values.
    uint32_t cycles = (uint32_t)(((uint64_t)nanoseconds * SystemCoreClock) / 1000000000);

    // Busy-wait until the required number of cycles has passed
    while ((DWT->CYCCNT - start_cycles) < cycles);
}

//////////////////////////////////////////////
//////////////////////RADAR///////////////////
//////////////////////////////////////////////
uint8_t degreesTo7BitPhase(float degrees) {
    // Normalize to 0-360 range
    while(degrees < 0) degrees += 360.0f;
    while(degrees >= 360.0f) degrees -= 360.0f;

    // Convert to 7-bit (0-127)
    uint8_t phase_7bit = (uint8_t)((degrees / 360.0f) * 128.0f);

    return phase_7bit % 128;
}

extern "C" {

    // USB CDC receive callback (called by STM32 HAL)
    void CDC_Receive_FS(uint8_t* Buf, uint32_t *Len) {
        // Process received USB data
        usbHandler.processUSBData(Buf, *Len);

        // Prepare for next reception
        USBD_CDC_SetRxBuffer(&hUsbDeviceFS, &usb_rx_buffer[0]);
        USBD_CDC_ReceivePacket(&hUsbDeviceFS);
    }
}

void systemPowerUpSequence() {
    uint8_t msg[] = "Starting Power Up Sequence...\r\n";
    HAL_UART_Transmit(&huart3, msg, sizeof(msg)-1, 1000);

    // Step 1: Initialize ADTR1107 power sequence
    if (!adarManager.initializeADTR1107Sequence()) {
        uint8_t err[] = "ERROR: ADTR1107 power sequence failed!\r\n";
        HAL_UART_Transmit(&huart3, err, sizeof(err)-1, 1000);
        Error_Handler();
    }

    // Step 2: Initialize all ADAR1000 devices
    if (!adarManager.initializeAllDevices()) {
        uint8_t err[] = "ERROR: ADAR1000 initialization failed!\r\n";
        HAL_UART_Transmit(&huart3, err, sizeof(err)-1, 1000);
        Error_Handler();
    }

    // Step 3: Perform system calibration
    if (!adarManager.performSystemCalibration()) {
        uint8_t warn[] = "WARNING: System calibration issues\r\n";
        HAL_UART_Transmit(&huart3, warn, sizeof(warn)-1, 1000);
    }

    // Step 4: Set to safe TX mode
    adarManager.setAllDevicesTXMode();

    uint8_t success[] = "Power Up Sequence Completed Successfully\r\n";
    HAL_UART_Transmit(&huart3, success, sizeof(success)-1, 1000);
}

void systemPowerDownSequence() {
    uint8_t msg[] = "Starting Power Down Sequence...\r\n";
    HAL_UART_Transmit(&huart3, msg, sizeof(msg)-1, 1000);

    // Step 1: Set all devices to RX mode (safest state)
    adarManager.setAllDevicesRXMode();
    HAL_Delay(10);

    // Step 2: Disable PA power supplies
    HAL_GPIO_WritePin(EN_P_5V0_PA1_GPIO_Port, EN_P_5V0_PA1_Pin | EN_P_5V0_PA2_Pin | EN_P_5V0_PA3_Pin, GPIO_PIN_RESET);
    HAL_Delay(10);

    // Step 3: Set PA biases to safe values
    for (uint8_t dev = 0; dev < 4; dev++) {
        adarManager.adarWrite(dev, REG_PA_CH1_BIAS_ON, 0x20, BROADCAST_OFF);
        adarManager.adarWrite(dev, REG_PA_CH2_BIAS_ON, 0x20, BROADCAST_OFF);
        adarManager.adarWrite(dev, REG_PA_CH3_BIAS_ON, 0x20, BROADCAST_OFF);
        adarManager.adarWrite(dev, REG_PA_CH4_BIAS_ON, 0x20, BROADCAST_OFF);
    }
    HAL_Delay(10);

    // Step 4: Disable LNA power supply
    HAL_GPIO_WritePin(EN_P_3V3_ADTR_GPIO_Port, EN_P_3V3_ADTR_Pin, GPIO_PIN_RESET);
    HAL_Delay(10);

    // Step 5: Set LNA bias to safe value
    for (uint8_t dev = 0; dev < 4; dev++) {
        adarManager.adarWrite(dev, REG_LNA_BIAS_ON, 0x00, BROADCAST_OFF);
    }
    HAL_Delay(10);

    // Step 6: Disable switch power supplies
    HAL_GPIO_WritePin(EN_P_3V3_VDD_SW_GPIO_Port, EN_P_3V3_VDD_SW_Pin, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(EN_P_3V3_SW_GPIO_Port, EN_P_3V3_SW_Pin, GPIO_PIN_RESET);
    HAL_Delay(10);

    uint8_t success[] = "Power Down Sequence Completed\r\n";
    HAL_UART_Transmit(&huart3, success, sizeof(success)-1, 1000);
}

void initializeBeamMatrices() {
    uint8_t msg[] = "Initializing Beam Matrices...\r\n";
    HAL_UART_Transmit(&huart3, msg, sizeof(msg)-1, 1000);

    // Matrix1: Positions 1-15 (positive phase differences)
    for(int beam_pos = 0; beam_pos < 15; beam_pos++) {
        float phase_diff_degrees = phase_differences[beam_pos];

        for(int element = 0; element < 16; element++) {
            float cumulative_phase_degrees = element * phase_diff_degrees;
            matrix1[beam_pos][element] = degreesTo7BitPhase(cumulative_phase_degrees);
        }
    }

    // Matrix2: Positions 17-31 (negative phase differences)
    for(int beam_pos = 0; beam_pos < 15; beam_pos++) {
        float phase_diff_degrees = phase_differences[beam_pos + 16];

        for(int element = 0; element < 16; element++) {
            float cumulative_phase_degrees = element * phase_diff_degrees;
            matrix2[beam_pos][element] = degreesTo7BitPhase(cumulative_phase_degrees);
        }
    }

    // Vector_0: Position 16 (zero phase - broadside)
    for(int element = 0; element < 16; element++) {
        vector_0[element] = 0; // 0 in 7-bit = 0° phase
    }

    uint8_t done[] = "Beam Matrices Initialized\r\n";
    HAL_UART_Transmit(&huart3, done, sizeof(done)-1, 1000);
}

void executeChirpSequence(int num_chirps, float T1, float PRI1, float T2, float PRI2) {
    // First chirp sequence (microsecond timing)
    for(int i = 0; i < num_chirps; i++) {
        HAL_GPIO_TogglePin(GPIOD, GPIO_PIN_8); // New chirp signal to FPGA
        adarManager.pulseTXMode();
        delay_us((uint32_t)T1);
        adarManager.pulseRXMode();
        delay_us((uint32_t)(PRI1 - T1));
    }

    delay_us((uint32_t)Guard);

    // Second chirp sequence (nanosecond timing)
    for(int i = 0; i < num_chirps; i++) {
    	HAL_GPIO_TogglePin(GPIOD, GPIO_PIN_8); // New chirp signal to FPGA
        adarManager.pulseTXMode();
        delay_ns((uint32_t)(T2 * 1000));
        adarManager.pulseRXMode();
        delay_ns((uint32_t)((PRI2 - T2) * 1000));

    }
}

void runRadarPulseSequence() {
    static int sequence_count = 0;
    char msg[50];

    snprintf(msg, sizeof(msg), "Starting RADAR Sequence #%d\r\n", ++sequence_count);
    HAL_UART_Transmit(&huart3, (uint8_t*)msg, strlen(msg), 1000);

    // Configure for fast switching
    adarManager.setFastSwitchMode(true);

    int m = 1; // Chirp counter
    int n = 1; // Beam Elevation position counter
    int y = 1; // Beam Azimuth counter

    // Main beam steering sequence
    for(int beam_pos = 0; beam_pos < 15; beam_pos++) {
    	HAL_GPIO_TogglePin(GPIOD, GPIO_PIN_9);// Notify FPGA of elevation change
        // Pattern 1: matrix1 (positive steering angles)
        adarManager.setCustomBeamPattern16(matrix1[beam_pos], ADAR1000Manager::BeamDirection::TX);
        adarManager.setCustomBeamPattern16(matrix1[beam_pos], ADAR1000Manager::BeamDirection::RX);

        executeChirpSequence(m_max/2, T1, PRI1, T2, PRI2);
        m += m_max/2;

        // Pattern 2: vector_0 (broadside)
        adarManager.setCustomBeamPattern16(vector_0, ADAR1000Manager::BeamDirection::TX);
        adarManager.setCustomBeamPattern16(vector_0, ADAR1000Manager::BeamDirection::RX);

        executeChirpSequence(m_max/2, T1, PRI1, T2, PRI2);
        m += m_max/2;

        // Pattern 3: matrix2 (negative steering angles)
        adarManager.setCustomBeamPattern16(matrix2[beam_pos], ADAR1000Manager::BeamDirection::TX);
        adarManager.setCustomBeamPattern16(matrix2[beam_pos], ADAR1000Manager::BeamDirection::RX);

        executeChirpSequence(m_max/2, T1, PRI1, T2, PRI2);
        m += m_max/2;

        // Reset chirp counter if needed
        if(m > m_max) m = 1;

        n++;
        if(n > n_max) n = 1;

    }

    HAL_GPIO_TogglePin(GPIOD,GPIO_PIN_10);//Tell FPGA that there is a new azimuth

    y++; if(y>y_max)y=1;
	  //Rotate stepper to next y position
	  for(int k= 0;k<(int)(Stepper_steps/y_max);k++){
		  HAL_GPIO_WritePin(STEPPER_CLK_P_GPIO_Port, STEPPER_CLK_P_Pin, GPIO_PIN_SET);
		  delay_us(500);
		  HAL_GPIO_WritePin(STEPPER_CLK_P_GPIO_Port, STEPPER_CLK_P_Pin, GPIO_PIN_RESET);
		  delay_us(500);
	  }



    snprintf(msg, sizeof(msg), "RADAR Sequence #%d Completed\r\n", sequence_count);
    HAL_UART_Transmit(&huart3, (uint8_t*)msg, strlen(msg), 1000);
}




void printSystemStatus() {
    char status_msg[100];

    // Print device status
    for(int i = 0; i < 4; i++) {
        if(adarManager.verifyDeviceCommunication(i)) {
            float temp = adarManager.readTemperature(i);
            snprintf(status_msg, sizeof(status_msg),
                    "ADAR1000 #%d: OK, Temperature: %.1fC\r\n", i+1, temp);
            HAL_UART_Transmit(&huart3, (uint8_t*)status_msg, strlen(status_msg), 1000);
        } else {
            snprintf(status_msg, sizeof(status_msg),
                    "ADAR1000 #%d: COMMUNICATION ERROR\r\n", i+1);
            HAL_UART_Transmit(&huart3, (uint8_t*)status_msg, strlen(status_msg), 1000);
        }
    }

    // Print matrix info
    snprintf(status_msg, sizeof(status_msg),
            "Beam Matrices: 31 positions, %d chirps/position\r\n", m_max);
    HAL_UART_Transmit(&huart3, (uint8_t*)status_msg, strlen(status_msg), 1000);
}

/***************************************************************/
/**********************SYSTEM ERROR HANDLER*********************/
/***************************************************************/

typedef enum {
    ERROR_NONE = 0,
    ERROR_AD9523_CLOCK,
    ERROR_ADF4382_TX_UNLOCK,
    ERROR_ADF4382_RX_UNLOCK,
    ERROR_ADAR1000_COMM,
    ERROR_ADAR1000_TEMP,
    ERROR_IMU_COMM,
    ERROR_BMP180_COMM,
    ERROR_GPS_COMM,
    ERROR_RF_PA_OVERCURRENT,
    ERROR_RF_PA_BIAS,
    ERROR_STEPPER_MOTOR,
    ERROR_FPGA_COMM,
    ERROR_POWER_SUPPLY,
    ERROR_TEMPERATURE_HIGH,
    ERROR_MEMORY_ALLOC,
    ERROR_WATCHDOG_TIMEOUT
} SystemError_t;

static SystemError_t last_error = ERROR_NONE;
static uint32_t error_count = 0;
static bool system_emergency_state = false;

// Error handler function
SystemError_t checkSystemHealth(void) {
    SystemError_t current_error = ERROR_NONE;

    // 1. Check AD9523 Clock Generator
    static uint32_t last_clock_check = 0;
    if (HAL_GetTick() - last_clock_check > 5000) {
        if (HAL_GPIO_ReadPin(AD9523_STATUS0_GPIO_Port, AD9523_STATUS0_Pin) == GPIO_PIN_RESET ||
            HAL_GPIO_ReadPin(AD9523_STATUS1_GPIO_Port, AD9523_STATUS1_Pin) == GPIO_PIN_RESET) {
            current_error = ERROR_AD9523_CLOCK;
        }
        last_clock_check = HAL_GetTick();
    }

    // 2. Check ADF4382 Lock Status
    bool tx_locked, rx_locked;
    if (ADF4382A_CheckLockStatus(&lo_manager, &tx_locked, &rx_locked) == ADF4382A_MANAGER_OK) {
        if (!tx_locked) current_error = ERROR_ADF4382_TX_UNLOCK;
        if (!rx_locked) current_error = ERROR_ADF4382_RX_UNLOCK;
    }

    // 3. Check ADAR1000 Communication and Temperature
    for (int i = 0; i < 4; i++) {
        if (!adarManager.verifyDeviceCommunication(i)) {
            current_error = ERROR_ADAR1000_COMM;
            break;
        }

        float temp = adarManager.readTemperature(i);
        if (temp > 85.0f) {
            current_error = ERROR_ADAR1000_TEMP;
            break;
        }
    }

    // 4. Check IMU Communication
    static uint32_t last_imu_check = 0;
    if (HAL_GetTick() - last_imu_check > 10000) {
        GY85_Update(&imu);
        if (isnan(imu.ax) || isnan(imu.ay) || isnan(imu.az)) {
            current_error = ERROR_IMU_COMM;
        }
        last_imu_check = HAL_GetTick();
    }

    // 5. Check BMP180 Communication
    static uint32_t last_bmp_check = 0;
    if (HAL_GetTick() - last_bmp_check > 15000) {
        double pressure = myBMP.getPressure();
        if (pressure < 30000.0 || pressure > 110000.0 || isnan(pressure)) {
            current_error = ERROR_BMP180_COMM;
        }
        last_bmp_check = HAL_GetTick();
    }

    // 6. Check GPS Communication
    static uint32_t last_gps_fix = 0;
    if (gps.location.isUpdated()) {
        last_gps_fix = HAL_GetTick();
    }
    if (HAL_GetTick() - last_gps_fix > 30000) {
        current_error = ERROR_GPS_COMM;
    }

    // 7. Check RF Power Amplifier Current
    if (PowerAmplifier) {
        for (int i = 0; i < 16; i++) {
            if (Idq_reading[i] > 2.5f) {
                current_error = ERROR_RF_PA_OVERCURRENT;
                break;
            }
            if (Idq_reading[i] < 0.1f) {
                current_error = ERROR_RF_PA_BIAS;
                break;
            }
        }
    }

    // 8. Check System Temperature
    if (temperature > 75.0f) {
        current_error = ERROR_TEMPERATURE_HIGH;
    }

    // 9. Simple watchdog check
    static uint32_t last_health_check = 0;
    if (HAL_GetTick() - last_health_check > 60000) {
        current_error = ERROR_WATCHDOG_TIMEOUT;
    }
    last_health_check = HAL_GetTick();

    return current_error;
}

// Error recovery function
void attemptErrorRecovery(SystemError_t error) {
    char recovery_msg[80];
    snprintf(recovery_msg, sizeof(recovery_msg),
             "Attempting recovery from error: %d\r\n", error);
    HAL_UART_Transmit(&huart3, (uint8_t*)recovery_msg, strlen(recovery_msg), 1000);

    switch (error) {
        case ERROR_ADF4382_TX_UNLOCK:
        case ERROR_ADF4382_RX_UNLOCK:
            // Re-initialize LO
            ADF4382A_Manager_Init(&lo_manager, SYNC_METHOD_TIMED);
            HAL_Delay(100);
            break;

        case ERROR_ADAR1000_COMM:
            // Reset ADAR1000 communication
            adarManager.initializeAllDevices();
            HAL_Delay(50);
            break;

        case ERROR_IMU_COMM:
            // Re-initialize IMU
            GY85_Init();
            HAL_Delay(100);
            break;

        case ERROR_GPS_COMM:
            // GPS will auto-recover when signal returns
            break;

        default:
            // For other errors, just log and continue
            break;
    }

    snprintf(recovery_msg, sizeof(recovery_msg),
             "Recovery attempt completed.\r\n");
    HAL_UART_Transmit(&huart3, (uint8_t*)recovery_msg, strlen(recovery_msg), 1000);
}

////////////////////////////////////////////////////////////////////////////////
//:::::RF POWER AMPLIFIER DAC5578 Emergency stop function using CLR pin/////////
////////////////////////////////////////////////////////////////////////////////
void Emergency_Stop(void) {
    /* Immediately clear all DAC outputs to zero using hardware CLR */
    DAC5578_ActivateClearPin(&hdac1);
    DAC5578_ActivateClearPin(&hdac2);

    /* Keep outputs cleared until reset */
    while (1) {
        HAL_Delay(100);
    }
}

// Error response function
void handleSystemError(SystemError_t error) {
    if (error == ERROR_NONE) return;

    error_count++;
    last_error = error;

    char error_msg[100];
    const char* error_strings[] = {
        "No error",
        "AD9523 Clock failure",
        "ADF4382 TX LO unlocked",
        "ADF4382 RX LO unlocked",
        "ADAR1000 communication error",
        "ADAR1000 temperature high",
        "IMU communication error",
        "BMP180 communication error",
        "GPS communication lost",
        "RF PA overcurrent detected",
        "RF PA bias fault",
        "Stepper motor fault",
        "FPGA communication error",
        "Power supply fault",
        "System temperature high",
        "Memory allocation failed",
        "Watchdog timeout"
    };

    snprintf(error_msg, sizeof(error_msg),
             "ERROR #%d: %s (Count: %lu)\r\n",
             error, error_strings[error], error_count);
    HAL_UART_Transmit(&huart3, (uint8_t*)error_msg, strlen(error_msg), 1000);

    // Blink LED pattern based on error code
    for (int i = 0; i < (error % 5) + 1; i++) {
        HAL_GPIO_TogglePin(LED_3_GPIO_Port, LED_3_Pin);
        HAL_Delay(200);
    }

    // Critical errors trigger emergency shutdown
    if (error >= ERROR_RF_PA_OVERCURRENT && error <= ERROR_POWER_SUPPLY) {
        snprintf(error_msg, sizeof(error_msg),
                 "CRITICAL ERROR! Initiating emergency shutdown.\r\n");
        HAL_UART_Transmit(&huart3, (uint8_t*)error_msg, strlen(error_msg), 1000);

        Emergency_Stop();
        system_emergency_state = true;
    }

    // For non-critical errors, attempt recovery
    if (!system_emergency_state) {
        attemptErrorRecovery(error);
    }
}


// System health monitoring function
bool checkSystemHealthStatus(void) {
    SystemError_t error = checkSystemHealth();

    if (error != ERROR_NONE) {
        handleSystemError(error);

        // If we're in emergency state or too many errors, shutdown
        if (system_emergency_state || error_count > 10) {
            return false;
        }
    }

    return true;
}

// Get system status for GUI
// Get system status for GUI with 8 temperature variables
void getSystemStatusForGUI(char* status_buffer, size_t buffer_size) {
    char temp_buffer[200];
    char final_status[500] = "System Status: ";

    // Basic status
    if (system_emergency_state) {
        strcat(final_status, "EMERGENCY_STOP|");
    } else {
        strcat(final_status, "NORMAL|");
    }

    // Error information
    snprintf(temp_buffer, sizeof(temp_buffer), "LastError:%d|ErrorCount:%lu|",
             last_error, error_count);
    strcat(final_status, temp_buffer);

    // Sensor status
    snprintf(temp_buffer, sizeof(temp_buffer), "IMU:%.1f,%.1f,%.1f|GPS:%.6f,%.6f|ALT:%.1f|",
             Pitch_Sensor, Roll_Sensor, Yaw_Sensor,
             RADAR_Latitude, RADAR_Longitude, RADAR_Altitude);
    strcat(final_status, temp_buffer);

    // LO Status
    bool tx_locked, rx_locked;
    ADF4382A_CheckLockStatus(&lo_manager, &tx_locked, &rx_locked);
    snprintf(temp_buffer, sizeof(temp_buffer), "LO_TX:%s|LO_RX:%s|",
             tx_locked ? "LOCKED" : "UNLOCKED",
             rx_locked ? "LOCKED" : "UNLOCKED");
    strcat(final_status, temp_buffer);

    // Temperature readings (8 variables)
    // You'll need to populate these temperature values from your sensors
    // For now, I'll show how to format them - replace with actual temperature readings
    Temperature_1 = ADS7830_Measure_SingleEnded(&hadc3, 0);
    Temperature_2 = ADS7830_Measure_SingleEnded(&hadc3, 1);
    Temperature_3 = ADS7830_Measure_SingleEnded(&hadc3, 2);
    Temperature_4 = ADS7830_Measure_SingleEnded(&hadc3, 3);
    Temperature_5 = ADS7830_Measure_SingleEnded(&hadc3, 4);
    Temperature_6 = ADS7830_Measure_SingleEnded(&hadc3, 5);
    Temperature_7 = ADS7830_Measure_SingleEnded(&hadc3, 6);
    Temperature_8 = ADS7830_Measure_SingleEnded(&hadc3, 7);

    // Format all 8 temperature variables
    snprintf(temp_buffer, sizeof(temp_buffer),
             "T1:%.1f|T2:%.1f|T3:%.1f|T4:%.1f|T5:%.1f|T6:%.1f|T7:%.1f|T8:%.1f|",
             Temperature_1, Temperature_2, Temperature_3, Temperature_4,
             Temperature_5, Temperature_6, Temperature_7, Temperature_8);
    strcat(final_status, temp_buffer);

    // RF Power Amplifier status (if enabled)
    if (PowerAmplifier) {
        float avg_current = 0.0f;
        for (int i = 0; i < 16; i++) {
            avg_current += Idq_reading[i];
        }
        avg_current /= 16.0f;

        snprintf(temp_buffer, sizeof(temp_buffer), "PA_AvgCurrent:%.2f|PA_Enabled:%d|",
                 avg_current, PowerAmplifier);
        strcat(final_status, temp_buffer);
    }

    // Radar operation status
    snprintf(temp_buffer, sizeof(temp_buffer), "BeamPos:%d|Azimuth:%d|ChirpCount:%d|",
             n, y, m);
    strcat(final_status, temp_buffer);

    // Copy to output buffer
    strncpy(status_buffer, final_status, buffer_size - 1);
    status_buffer[buffer_size - 1] = '\0';
}

/* ---------- UART printing helpers ---------- */
/*
static void uart_print(const char *msg)
{
    HAL_UART_Transmit(&huart3, (uint8_t*)msg, strlen(msg), HAL_MAX_DELAY);
}

static void uart_println(const char *msg)
{
    HAL_UART_Transmit(&huart3, (uint8_t*)msg, strlen(msg), HAL_MAX_DELAY);
    const char crlf[] = "\r\n";
    HAL_UART_Transmit(&huart3, (uint8_t*)crlf, 2, HAL_MAX_DELAY);
}
*/

/* ---------- Helper delay wrappers ---------- */
static inline void delay_ms(uint32_t ms) { HAL_Delay(ms); }



// This custom version of delay() ensures that the gps object
// is being "fed".
static void smartDelay(unsigned long ms)
{
    uint32_t start = HAL_GetTick();
    uint8_t ch;

    do {
        // While there is new data available in UART (non-blocking)
        if (HAL_UART_Receive(&huart5, &ch, 1, 0) == HAL_OK) {
            gps.encode(ch);   // Pass received byte to TinyGPS++ equivalent parser
        }
    } while (HAL_GetTick() - start < ms);
}

// Small helper to enable DWT cycle counter for microdelay
static void DWT_Init(void)
{
    // Enable DWT CYCCNT
    CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
    DWT->CYCCNT = 0;
    DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
}

// Configure and program AD9523 via no-os driver
static int configure_ad9523(void)
{
    int32_t ret;
    struct ad9523_dev *dev = NULL;

    static struct ad9523_platform_data pdata;
    memset(&pdata, 0, sizeof(pdata));

    // VCXO + refs
    pdata.vcxo_freq = 100000000; // 100 MHz VCXO on OSC_IN
    pdata.refa_diff_rcv_en = 0;  // REFA 10 MHz single-ended
    pdata.refb_diff_rcv_en = 0;  // REFB 100 MHz single-ended
    pdata.osc_in_diff_en = 0;

    // PLL1: keep bypass disabled so VCXO can be used as reference cleanup if desired
    pdata.pll1_bypass_en = 0;
    pdata.refa_r_div = 1;
    pdata.refb_r_div = 1;

    // PLL2: 100 MHz PFD (R2=1), N = 36 to get VCO = 3.6 GHz
    pdata.pll2_ndiv_a_cnt = 0;
    pdata.pll2_ndiv_b_cnt = 9; // 4*9 + 0 = 36
    pdata.pll2_r2_div = 0;     // R2=1
    pdata.pll2_charge_pump_current_nA = 3500; // example

    // Loop filters: reasonable starting values from examples
    pdata.rpole2 = RPOLE2_900_OHM;
    pdata.rzero = RZERO_2000_OHM;
    pdata.cpole1 = CPOLE1_24_PF;
    pdata.rzero_bypass_en = 0;

    // Channels array (must allocate AD9523_NUM_CHAN entries)
    static struct ad9523_channel_spec channels[AD9523_NUM_CHAN];
    pdata.channels = channels;
    pdata.num_channels = AD9523_NUM_CHAN;

    // Initialize channels to disabled defaults
    for (int i=0; i<AD9523_NUM_CHAN; ++i) {
        channels[i].channel_num = i;
        channels[i].driver_mode = TRISTATE;
        channels[i].channel_divider = 0;
        channels[i].divider_phase = 0;
        channels[i].use_alt_clock_src = 0;
        channels[i].output_dis = 1;
    }

    // Map your required outputs (3.6 GHz PLL2)
    // OUT0 = ADF4382A_TX= 300 MHz LVDS ( /12 )
    channels[0].channel_divider = 12;
    channels[0].driver_mode = LVDS_7mA;
    channels[0].divider_phase = 0;
    channels[0].output_dis = 0;

    // OUT1 = ADF4382A_RX = 300 MHz LVDS (phase aligned with OUT0)
    channels[1].channel_divider = 12;
    channels[1].driver_mode = LVDS_7mA;
    channels[1].divider_phase = 0;
    channels[1].output_dis = 0;

    // OUT4 = ADC = 400 MHz LVDS ( /9 )
    channels[4].channel_divider = 9;
    channels[4].driver_mode = LVDS_7mA;
    channels[4].divider_phase = 0;
    channels[4].output_dis = 0;

    // OUT5 = FPGA_ADC = 400 MHz LVDS (phase aligned with OUT4)
    channels[5].channel_divider = 9;
    channels[5].driver_mode = LVDS_7mA;
    channels[5].divider_phase = 0;
    channels[5].output_dis = 0;

    // OUT6 = FPGA_SYSTEM_CLOCK = 100 MHz LVCMOS ( /36 )
    channels[6].channel_divider = 36;
    channels[6].driver_mode = CMOS_CONF1;
    channels[6].divider_phase = 0;
    channels[6].output_dis = 0;

    // OUT7 = FPGA_TEST_CLOCK = 20 MHz LVCMOS ( /180 )
    channels[7].channel_divider = 180;
    channels[7].driver_mode = CMOS_CONF1;
    channels[7].divider_phase = 0;
    channels[7].output_dis = 0;

    // OUT8 = SYNC_TX = 60 MHz LVDS ( /60 )
    channels[8].channel_divider = 60;
    channels[8].driver_mode = LVDS_4mA;
    channels[8].divider_phase = 0;
    channels[8].output_dis = 0;

    // OUT9 = SYNC_RX = 60 MHz LVDS (phase aligned with OUT8)
    channels[9].channel_divider = 60;
    channels[9].driver_mode = LVDS_4mA;
    channels[9].divider_phase = 0;
    channels[9].output_dis = 0;

    // OUT10 = DAC = 120 MHz LVCMOS ( /30 )
    channels[10].channel_divider = 30;
    channels[10].driver_mode = CMOS_CONF1;
    channels[10].divider_phase = 0;
    channels[10].output_dis = 0;

    // OUT11 = FPGA_DAC = 120 MHz LVCMOS (phase aligned with OUT10)
    channels[11].channel_divider = 30;
    channels[11].driver_mode = CMOS_CONF1;
    channels[11].divider_phase = 0;
    channels[11].output_dis = 0;

    // Fill ad9523 init param
    struct ad9523_init_param init_param;
    memset(&init_param, 0, sizeof(init_param));

    // SPI init (no_os type)
    init_param.spi_init.max_speed_hz = 10000000; // 10 MHz SPI
    init_param.spi_init.chip_select = 0;
    init_param.spi_init.mode = NO_OS_SPI_MODE_0;
    init_param.spi_init.platform_ops = &stm32_spi_ops;
    init_param.spi_init.extra = &hspi4; // pass HAL handle via extra


    init_param.pdata = &pdata;

    // init ad9523 defaults (fills any missing pdata defaults)
    ad9523_init(&init_param);
    ret = ad9523_setup(&dev, &init_param);


    // Bring AD9523 out of reset
    AD9523_RESET_RELEASE();
    HAL_Delay(5);

    // Select REFB (100MHz) using REF_SEL pin (true selects REFB here)
    AD9523_REF_SEL(true);

    // Call setup which uses no_os_spi to write registers, io_update, calibrate, sync
    ret = ad9523_setup(&dev, &init_param);
    if (ret != 0) {
        // handle error: lock missing or SPI error
        return -1;
    }

    // Final check
    ret = ad9523_status(dev);
    if (ret != 0) {
        // log/handle
        return -1;
    }

    // optionally manual sync
    ad9523_sync(dev);

    // keep device pointer globally if needed (dev)
    return 0;
}

////////////////////////////////////////////////////////////////////////////////
//////////////////////////////ADAR1000//////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
// Alternative approach using the beam sequence feature
void setupBeamSequences(ADAR1000Manager& manager) {
    std::vector<ADAR1000Manager::BeamConfig> tx_sequence;
    std::vector<ADAR1000Manager::BeamConfig> rx_sequence;

    // Create beam configurations for each pattern
    // Matrix1 sequences (14 steps)
    for(int i = 0; i < 14; i++) {
        ADAR1000Manager::BeamConfig tx_config;
        ADAR1000Manager::BeamConfig rx_config;

        // Calculate angles or use your matrix directly
        // For now, we'll use a placeholder angle and set phases manually later
        tx_config.angle_degrees = 0; // This would be calculated from your matrix
        rx_config.angle_degrees = 0;

        // Copy phase settings from matrix1
        for(int ch = 0; ch < 16; ch++) {
            if(ch < 4) {
                tx_config.phase_settings[ch] = matrix1[i][ch];
                rx_config.phase_settings[ch] = matrix1[i][ch];
            }
        }

        tx_sequence.push_back(tx_config);
        rx_sequence.push_back(rx_config);
    }

    // You would similarly add the zero phase and matrix2 sequences

    // Note: The beam sequence feature in the manager currently uses calculated phases
    // You might need to modify the manager to support pre-calculated phase arrays
}
void setCustomBeamPattern(ADAR1000Manager& manager, uint8_t phase_pattern[16]) {
    // Set TX phases for all 4 ADAR1000 devices
    for(int dev = 0; dev < 4; dev++) {
        for(int ch = 0; ch < 4; ch++) {
            uint8_t phase = phase_pattern[dev * 4 + ch];
            manager.adarSetTxPhase(dev, ch + 1, phase, BROADCAST_OFF);
        }
    }

    // Set RX phases for all 4 ADAR1000 devices
    for(int dev = 0; dev < 4; dev++) {
        for(int ch = 0; ch < 4; ch++) {
            uint8_t phase = phase_pattern[dev * 4 + ch];
            manager.adarSetRxPhase(dev, ch + 1, phase, BROADCAST_OFF);
        }
    }
}

void initializeBeamMatricesWithSteeringAngles() {
    const float wavelength = 0.02857f; // 10.5 GHz wavelength in meters
    const float element_spacing = wavelength / 2.0f;

    // Calculate steering angles from phase differences
    float steering_angles[31];
    for(int i = 0; i < 31; i++) {
        float phase_diff_rad = phase_differences[i] * M_PI / 180.0f;
        steering_angles[i] = asin((phase_diff_rad * wavelength) / (2 * M_PI * element_spacing)) * 180.0f / M_PI;
    }

    // Matrix1: Positive angles (positions 1-15)
    for(int beam_pos = 0; beam_pos < 15; beam_pos++) {
        float angle = steering_angles[beam_pos];

        for(int element = 0; element < 16; element++) {
            // Calculate phase shift for linear array
            float phase_shift_rad = (2 * M_PI * element_spacing * element * sin(angle * M_PI / 180.0f)) / wavelength;
            float phase_degrees = phase_shift_rad * 180.0f / M_PI;

            // Wrap to 0-360 degrees
            while(phase_degrees < 0) phase_degrees += 360;
            while(phase_degrees >= 360) phase_degrees -= 360;

            matrix1[beam_pos][element] = (uint8_t)((phase_degrees / 360.0f) * 128);
        }
    }

    // Matrix2: Negative angles (positions 17-31)
    for(int beam_pos = 0; beam_pos < 15; beam_pos++) {
        float angle = steering_angles[beam_pos + 16];

        for(int element = 0; element < 16; element++) {
            float phase_shift_rad = (2 * M_PI * element_spacing * element * sin(angle * M_PI / 180.0f)) / wavelength;
            float phase_degrees = phase_shift_rad * 180.0f / M_PI;

            while(phase_degrees < 0) phase_degrees += 360;
            while(phase_degrees >= 360) phase_degrees -= 360;

            matrix2[beam_pos][element] = (uint8_t)((phase_degrees / 360.0f) * 128);
        }
    }
}



/* 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 */

  /* USER CODE END 1 */

  /* MPU Configuration--------------------------------------------------------*/
  MPU_Config();

  /* 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();

  /* Configure the peripherals common clocks */
  PeriphCommonClock_Config();

  /* USER CODE BEGIN SysInit */


  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_TIM1_Init();
  MX_I2C1_Init();
  MX_I2C2_Init();
  MX_I2C3_Init();
  MX_SPI1_Init();
  MX_SPI4_Init();
  MX_UART5_Init();
  MX_USART3_UART_Init();
  MX_USB_DEVICE_Init();
  /* USER CODE BEGIN 2 */

  HAL_TIM_Base_Start(&htim1);

  /* --- Enable DWT cycle counter for accurate microsecond delays --- */
  DWT_Init();
  //Wait for OCXO 3mn
  HAL_Delay(180000);
  HAL_GPIO_WritePin(AD9523_RESET_GPIO_Port,AD9523_RESET_Pin,GPIO_PIN_RESET);

  //Power sequencing AD9523
  HAL_GPIO_WritePin(EN_P_1V8_CLOCK_GPIO_Port,EN_P_1V8_CLOCK_Pin,GPIO_PIN_SET);
  HAL_Delay(100);
  HAL_GPIO_WritePin(EN_P_3V3_CLOCK_GPIO_Port,EN_P_3V3_CLOCK_Pin,GPIO_PIN_SET);
  HAL_Delay(100);
  HAL_GPIO_WritePin(AD9523_RESET_GPIO_Port,AD9523_RESET_Pin,GPIO_PIN_SET);
  HAL_Delay(100);

  //Set planned Clocks on AD9523
  /*
  o Channel 0 = 300MHz LVDS (ADF4382 TX)
  o Channel 1 = 300MHz LVDS (ADF4382 RX) (Phase aligned with Channel 0)
  o Channel 4 = 400MHz LVDS (ADC)
  o Channel 5 = 400MHz LVDS (FPGA_ADC)(Phase aligned with Channel 4)
  o Channel 6 = 100MHz LVCMOS (FPGA system clock)
  o Channel 7 = 20MHz LVCMOS (FPGA test)
  o Channel 8 = 60MHz LVDS (ADF4382 TX sync)
  o Channel 9 = 60MHz LVDS (ADF4382 RX sync)(Phase aligned with Channel 8)
  o Channel 10 = 120MHz LVCMOS
  o Channel 11 = 120MHz LVCMOS (Phase aligned with Channel 10)
   */


  // Ensure stm32_spi.c and stm32_delay.c are compiled and linked

  if (configure_ad9523() != 0) {
      // configuration error - handle as needed
      while (1) { HAL_Delay(1000); }
  }

  //Power sequencing FPGA
  HAL_GPIO_WritePin(EN_P_1V0_FPGA_GPIO_Port,EN_P_1V0_FPGA_Pin,GPIO_PIN_SET);
  HAL_Delay(100);
  HAL_GPIO_WritePin(EN_P_1V8_FPGA_GPIO_Port,EN_P_1V8_FPGA_Pin,GPIO_PIN_SET);
  HAL_Delay(100);
  HAL_GPIO_WritePin(EN_P_3V3_FPGA_GPIO_Port,EN_P_3V3_FPGA_Pin,GPIO_PIN_SET);
  HAL_Delay(100);


// Initialize module IMU
  GY85_Init();
  for(int i=0; i<10;i++){
  GY85_Update(&imu);

  ax = imu.ax;
  ay = imu.ay;
  az = imu.az;
  gx = -imu.gx;
  gy = -imu.gy;
  gz = imu.gz;
  mx = imu.mx;
  my = imu.my;
  mz = imu.mz;

  ax *= 6.10351e-5;   //2.0 / 32768.0;
  ay *= 6.10351e-5;
  az *= 6.10351e-5;

  ax -= abias[0];   // Convert to g's, remove accelerometer biases
  ay -= abias[1];
  az -= abias[2];

  gx *= 0.0152588f;   //500.0 / 32768.0;
  gy *= 0.0152588f;
  gz *= 0.0152588f;

  gx -= gbias[0];   // Convert to degrees per seconds, remove gyro biases
  gy -= gbias[1];
  gz -= gbias[2];

  mx *= 6.10351e-5;   //2.0 / 32768.0;
  my *= 6.10351e-5;
  mz *= 6.10351e-5;

  mxc = M11*(mx-mbias[0]) + M12*(my-mbias[1]) +M13*(mz-mbias[2]);
  myc = M21*(mx-mbias[0]) + M22*(my-mbias[1]) +M23*(mz-mbias[2]);
  mzc = M31*(mx-mbias[0]) + M32*(my-mbias[1]) +M33*(mz-mbias[2]);

  mx = mxc;
  my = myc;
  mz = mzc;

  float norm = sqrt((mx*mx) + (my*my) + (mz*mz));
  mx /=norm;
  my /=norm;
  mz /=norm;

  /***************************************************************/
  /*********************IMU Complementary Filter******************/
  /***************************************************************/
    RxAcc = ax;
    RyAcc = ay;
    RzAcc = az;

    Rate_Ayz_1 = gx;
    Rate_Axz_1 = gy;
    Rate_Axy_1 = gz;

    Rate_Ayz_1 = Rate_Ayz_1*0.01745;//Convertion from °/s to Radians/s
    Rate_Axz_1 = Rate_Axz_1*0.01745;
    Rate_Axy_1 = Rate_Axy_1*0.01745;

    /*Combining Accelerometer & Gyroscope Data*/
    Axz_0 = atan2(RxEst_0,RzEst_0);
    Ayz_0 = atan2(RyEst_0,RzEst_0);
    Axy_0 = atan2(RxEst_0,RyEst_0);

    now_timeperiod = micros();

    Time_Period = now_timeperiod - lasttime_timeperiod;

    Axz_1= Axz_0 + Rate_Axz_1*Time_Period*0.000001;
    Ayz_1= Ayz_0 + Rate_Ayz_1*Time_Period*0.000001;
    Axy_1= Axy_0 + Rate_Axy_1*Time_Period*0.000001;

    lasttime_timeperiod = now_timeperiod;

    RxGyro = sin(Axz_1)/(sqrt(1.0 + cos(Axz_1)*cos(Axz_1) * tan(Ayz_1)*tan(Ayz_1)));
    RyGyro = sin(Ayz_1)/(sqrt(1.0 + cos(Ayz_1)*cos(Ayz_1) * tan(Axz_1)*tan(Axz_1)));
    if(RzEst_0 >= 0){
      RzGyro = 1.0*sqrt(1 - RxGyro*RxGyro - RyGyro*RyGyro);
    }
    else {
      RzGyro = -1*sqrt(1 - RxGyro*RxGyro - RyGyro*RyGyro);
    }

    RxEst_1 = RxAcc*0.5 + RxGyro*0.5 ;// Weight of Gyro. over Acc.
    RyEst_1 = RyAcc*0.5 + RyGyro*0.5 ;
    RzEst_1 = RzAcc*0.5 + RzGyro*0.5 ;

    R_Est = sqrt(RxEst_1*RxEst_1+RyEst_1*RyEst_1+RzEst_1*RzEst_1);

    Pitch_Sensor = atan2(RxEst_1,sqrt(RyEst_1*RyEst_1)+(RzEst_1*RzEst_1))*180/PI;
    Roll_Sensor = atan2(RyEst_1,sqrt(RxEst_1*RxEst_1)+(RzEst_1*RzEst_1))*180/PI;

    float magRawX = mx*cos(Pitch_Sensor*PI/180.0f)  - mz*sin(Pitch_Sensor*PI/180.0f);
	float magRawY = mx*sin(Roll_Sensor*PI/180.0f)*sin(Pitch_Sensor*PI/180.0f) + my*cos(Roll_Sensor*PI/180.0f)- mz*sin(Roll_Sensor*PI/180.0f)*cos(Pitch_Sensor*PI/180.0f);
    Yaw_Sensor = (180*atan2(magRawY,magRawX)/PI) - Mag_Declination;

    if(Yaw_Sensor<0)Yaw_Sensor+=360;
    RxEst_0 = RxEst_1;
    RyEst_0 = RyEst_1;
    RzEst_0 = RzEst_1;

    HAL_Delay(300);

  }

    ///////////////////////////////////////////////////////////////////////////////////
    ///////////////////////////////////BAROMETER BMP180////////////////////////////////
    ///////////////////////////////////////////////////////////////////////////////////
  	for(int i = 0; i<5 ; i++){
		float BMP_Perssure = myBMP.getPressure();
		RADAR_Altitude = 44330*(1-(pow((BMP_Perssure/101325),(1/5.255))));
		HAL_Delay(100);
  	}

    ///////////////////////////////////////////////////////////////////////////////////
    ///////////////////////////////////////ADF4382/////////////////////////////////////
    ///////////////////////////////////////////////////////////////////////////////////
    printf("Starting ADF4382A Radar LO System...\n");
    printf("Using SPI4 with TIMED SYNCHRONIZATION (60 MHz clocks)\n");

    // Initialize LO manager with TIMED synchronization
    int ret = ADF4382A_Manager_Init(&lo_manager, SYNC_METHOD_TIMED);
    // Set phase shift (e.g., 500 ps for TX, 500 ps for RX)
    ADF4382A_SetPhaseShift(&lo_manager, 500, 500);

    // Strobe to apply phase shifts
    ADF4382A_StrobePhaseShift(&lo_manager, 0); // TX device
    ADF4382A_StrobePhaseShift(&lo_manager, 1); // RX device
    if (ret != ADF4382A_MANAGER_OK) {
        printf("LO Manager initialization failed: %d\n", ret);
        Error_Handler();
    }

    // Check initial lock status
    bool tx_locked, rx_locked;
    ret = ADF4382A_CheckLockStatus(&lo_manager, &tx_locked, &rx_locked);
    if (ret == ADF4382A_MANAGER_OK) {
        printf("Initial Lock Status - TX: %s, RX: %s\n",
               tx_locked ? "LOCKED" : "UNLOCKED",
               rx_locked ? "LOCKED" : "UNLOCKED");
    }
    // Wait for both LOs to lock
        uint32_t lock_timeout = 0;
        while (!(tx_locked && rx_locked) && lock_timeout < 100) {
            HAL_Delay(100);
            ADF4382A_CheckLockStatus(&lo_manager, &tx_locked, &rx_locked);
            lock_timeout++;

            if (lock_timeout % 10 == 0) {
                printf("Waiting for LO lock... TX: %s, RX: %s\n",
                       tx_locked ? "LOCKED" : "UNLOCKED",
                       rx_locked ? "LOCKED" : "UNLOCKED");
            }
        }

        if (tx_locked && rx_locked) {
            printf("Both LOs locked successfully!\n");
            printf("Synchronization ready - 60 MHz clocks on SYNCP/SYNCN pins\n");
            // - 60 MHz phase-aligned clocks on SYNCP/SYNCN pins
            // - SYNC pin connected for triggering

            // When ready to synchronize:
            ADF4382A_TriggerTimedSync(&lo_manager);
            // At this point, the SYNC pin can be toggled to trigger synchronization
        } else {
            printf("LO lock timeout! TX: %s, RX: %s\n",
                   tx_locked ? "LOCKED" : "UNLOCKED",
                   rx_locked ? "LOCKED" : "UNLOCKED");
        }
  // check if there is a lock
  if(HAL_GPIO_ReadPin (ADF4382_TX_LKDET_GPIO_Port, ADF4382_TX_LKDET_Pin))
  {
      // Set The LED_1 ON!
      HAL_GPIO_WritePin(LED_1_GPIO_Port, LED_1_Pin, GPIO_PIN_SET);
  }
  else
  {
      // Else .. Turn LED_1 OFF!
      HAL_GPIO_WritePin(LED_1_GPIO_Port, LED_1_Pin, GPIO_PIN_RESET);
  }

  if(HAL_GPIO_ReadPin (ADF4382_RX_LKDET_GPIO_Port, ADF4382_RX_LKDET_Pin))
  {
      // Set The LED_2 ON!
      HAL_GPIO_WritePin(LED_2_GPIO_Port, LED_2_Pin, GPIO_PIN_SET);
  }
  else
  {
      // Else .. Turn LED_2 OFF!
      HAL_GPIO_WritePin(LED_2_GPIO_Port, LED_2_Pin, GPIO_PIN_RESET);
  }


  //////////////////////////////////////////////////////////////////////////////////////
  /////////////////////////////////////ADAR1000/////////////////////////////////////////
  //////////////////////////////////////////////////////////////////////////////////////

  //Power sequencing ADAR1000

  //Tell FPGA to turn off TX RF signal by disabling Mixers
  HAL_GPIO_WritePin(GPIOD, GPIO_PIN_11, GPIO_PIN_RESET);

  HAL_GPIO_WritePin(EN_P_3V3_ADAR12_GPIO_Port,EN_P_3V3_ADAR12_Pin,GPIO_PIN_SET);
  HAL_GPIO_WritePin(EN_P_3V3_ADAR34_GPIO_Port,EN_P_3V3_ADAR34_Pin,GPIO_PIN_SET);
  HAL_Delay(500);
  HAL_GPIO_WritePin(EN_P_5V0_ADAR_GPIO_Port,EN_P_5V0_ADAR_Pin,GPIO_PIN_SET);
  HAL_Delay(500);

  // System startup message
      uint8_t startup_msg[] = "Starting Phased Array RADAR System...\r\n";
      HAL_UART_Transmit(&huart3, startup_msg, sizeof(startup_msg)-1, 1000);

      // Power up sequence
      systemPowerUpSequence();

      // Initialize beam matrices
      initializeBeamMatrices();

      // Print system status
      printSystemStatus();

  	//////////////////////////////////////////////////////////////////////////////////////
    //////////////////////////////////////////GPS/////////////////////////////////////////
    //////////////////////////////////////////////////////////////////////////////////////
  for(int i=0; i<10;i++){
  smartDelay(1000);
  RADAR_Longitude = gps.location.lng();
  RADAR_Latitude = gps.location.lat();
  }

  //move Stepper to position 1 = 0°
  HAL_GPIO_WritePin(STEPPER_CW_P_GPIO_Port, STEPPER_CW_P_Pin, GPIO_PIN_RESET);//Set stepper motor spinning direction to CCW
  //Point Stepper to North
  for(int i= 0;i<(int)(Yaw_Sensor*Stepper_steps/360);i++){
	  HAL_GPIO_WritePin(STEPPER_CLK_P_GPIO_Port, STEPPER_CLK_P_Pin, GPIO_PIN_SET);
	  delay_us(500);
	  HAL_GPIO_WritePin(STEPPER_CLK_P_GPIO_Port, STEPPER_CLK_P_Pin, GPIO_PIN_RESET);
	  delay_us(500);
  }

  /***************************************************************/
  /**********wait for GUI start flag and Send Lat/Long/alt********/
  /***************************************************************/

  GPS_Data_t gps_data;
  // Binary packet structure:
  // [Header 4 bytes][Latitude 8 bytes][Longitude 8 bytes][Altitude 4 bytes][CRC 2 bytes]
  gps_data = {RADAR_Latitude, RADAR_Longitude, RADAR_Altitude, Pitch_Sensor, HAL_GetTick()};
  GPS_SendBinaryToGUI(&gps_data);

  // Check if start flag was received and settings are ready
  do{
	  if (usbHandler.isStartFlagReceived() &&
		  usbHandler.getState() == USBHandler::USBState::READY_FOR_DATA) {

		  const RadarSettings& settings = usbHandler.getSettings();

		  // Use the settings to configure your radar system
		  /*
			  settings.getSystemFrequency();
			  settings.getChirpDuration1();
			  settings.getChirpDuration2();
			  settings.getChirpsPerPosition();
			  settings.getFreqMin();
			  settings.getFreqMax();
			  settings.getPRF1();
			  settings.getPRF2();
			  settings.getMaxDistance();
			  */


              }
  }while(!usbHandler.isStartFlagReceived());

  /***************************************************************/
  /************RF Power Amplifier Powering up sequence************/
  /***************************************************************/
  if(PowerAmplifier){
	  /* Initialize DACs */
	  /* DAC1: Address 0b1001000 = 0x48 */
	  if (!DAC5578_Init(&hdac1, &hi2c1, 0x48, 8,
			  DAC_1_VG_LDAC_GPIO_Port, DAC_1_VG_LDAC_Pin,
			  DAC_1_VG_CLR_GPIO_Port, DAC_1_VG_CLR_Pin)) {
		  Error_Handler();
	  }

	  /* DAC2: Address 0b1001001 = 0x49 */
	  if (!DAC5578_Init(&hdac2, &hi2c1, 0x49, 8,
			  DAC_2_VG_LDAC_GPIO_Port, DAC_2_VG_LDAC_Pin,
			  DAC_2_VG_CLR_GPIO_Port, DAC_2_VG_CLR_Pin)) {
		  Error_Handler();
	  }
	  /* Configure clear code behavior */
	  DAC5578_SetClearCode(&hdac1, DAC5578_CLR_CODE_ZERO); // Clear to 0V on CLR pulse
	  DAC5578_SetClearCode(&hdac2, DAC5578_CLR_CODE_ZERO); // Clear to 0V on CLR pulse

	  /* Configure LDAC so all channels update simultaneously on hardware LDAC */
	  DAC5578_SetupLDAC(&hdac1, 0xFF); // All channels respond to LDAC
	  DAC5578_SetupLDAC(&hdac2, 0xFF); // All channels respond to LDAC

	  //set Vg [1-8] to -3.98V -> input opamp = 1.63058V->126(8bits)
	  for(int channel = 0; channel < 8; channel++){
		  DAC5578_WriteAndUpdateChannelValue(&hdac1, channel, DAC_val);
	  }

	  //set Vg [9-16] to -3.98V -> input opamp = 1.63058V->126(8bits)
	  for(int channel = 0; channel < 8; channel++){
		  DAC5578_WriteAndUpdateChannelValue(&hdac2, channel, DAC_val);
	  }

	  /* Optional: Use hardware LDAC for simultaneous update of all channels */
	  HAL_GPIO_WritePin(DAC_1_VG_LDAC_GPIO_Port, DAC_1_VG_LDAC_Pin, GPIO_PIN_RESET);
	  HAL_GPIO_WritePin(DAC_2_VG_LDAC_GPIO_Port, DAC_2_VG_LDAC_Pin, GPIO_PIN_RESET);
	  HAL_Delay(1);
	  HAL_GPIO_WritePin(DAC_1_VG_LDAC_GPIO_Port, DAC_1_VG_LDAC_Pin, GPIO_PIN_SET);
	  HAL_GPIO_WritePin(DAC_2_VG_LDAC_GPIO_Port, DAC_2_VG_LDAC_Pin, GPIO_PIN_SET);

	  //Enable RF Power Amplifier VDD = 22V
	  HAL_GPIO_WritePin(EN_DIS_RFPA_VDD_GPIO_Port, EN_DIS_RFPA_VDD_Pin, GPIO_PIN_SET);

	  /* Initialize ADCs with correct addresses */
	  /* ADC1: Address 0x48, Single-Ended mode, Internal Ref ON + ADC ON */
	  if (!ADS7830_Init(&hadc1, &hi2c2, 0x48,
						ADS7830_SDMODE_SINGLE, ADS7830_PDIRON_ADON)) {
		  Error_Handler();
	  }

	  /* ADC2: Address 0x4A, Single-Ended mode, Internal Ref ON + ADC ON */
	  if (!ADS7830_Init(&hadc2, &hi2c2, 0x4A,
						ADS7830_SDMODE_SINGLE, ADS7830_PDIRON_ADON)) {
		  Error_Handler();
	  }

	  /* Read all 8 channels from ADC1 and calculate Idq */
	  for (uint8_t channel = 0; channel < 8; channel++) {
		  adc1_readings[channel] = ADS7830_Measure_SingleEnded(&hadc1, channel);
		  Idq_reading[channel]= (3.3/255)*adc1_readings[channel]/(50*0.005);//Idq=Vadc/(GxRshunt)//G_INA241A3=50;Rshunt=5mOhms
	  }

	  /* Read all 8 channels from ADC2 and calculate Idq*/
	  for (uint8_t channel = 0; channel < 8; channel++) {
		  adc2_readings[channel] = ADS7830_Measure_SingleEnded(&hadc2, channel);
		  Idq_reading[channel+8]= (3.3/255)*adc2_readings[channel]/(50*0.005);//Idq=Vadc/(GxRshunt)//G_INA241A3=50;Rshunt=5mOhms
	  }

	  for (uint8_t channel = 0; channel < 8; channel++){
	      uint8_t safety_counter = 0;
	      DAC_val = 126; // Reset for each channel

	      do {
	          if (safety_counter++ > 50) { // Prevent infinite loop
	              break;
	          }
	          DAC_val = DAC_val - 4;
	          DAC5578_WriteAndUpdateChannelValue(&hdac1, channel, DAC_val);
	          adc1_readings[channel] = ADS7830_Measure_SingleEnded(&hadc1, channel);
	          Idq_reading[channel] = (3.3/255) * adc1_readings[channel] / (50 * 0.005);
	      } while (DAC_val > 38 && abs(Idq_reading[channel] - 1.680) < 0.2); // Fixed logic
	  }

	  for (uint8_t channel = 0; channel < 8; channel++){
	      uint8_t safety_counter = 0;
	      DAC_val = 126; // Reset for each channel

	      do {
	          if (safety_counter++ > 50) { // Prevent infinite loop
	              break;
	          }
	          DAC_val = DAC_val - 4;
	          DAC5578_WriteAndUpdateChannelValue(&hdac2, channel, DAC_val);
	          adc1_readings[channel] = ADS7830_Measure_SingleEnded(&hadc2, channel);
	          Idq_reading[channel+8] = (3.3/255) * adc2_readings[channel] / (50 * 0.005);
	      } while (DAC_val > 38 && abs(Idq_reading[channel+8] - 1.680) < 0.2); // Fixed logic
	  }
  }

  //RESET FPGA
  HAL_GPIO_WritePin(GPIOD, GPIO_PIN_12, GPIO_PIN_RESET);
  HAL_Delay(10);
  HAL_GPIO_WritePin(GPIOD, GPIO_PIN_12, GPIO_PIN_SET);



  //Tell FPGA to apply TX RF by enabling Mixers
  HAL_GPIO_WritePin(GPIOD, GPIO_PIN_11, GPIO_PIN_SET);

  /* T°C sensor TMP37 ADC3: Address 0x49, Single-Ended mode, Internal Ref ON + ADC ON */
  if (!ADS7830_Init(&hadc3, &hi2c2, 0x49,
					ADS7830_SDMODE_SINGLE, ADS7830_PDIRON_ADON)) {
	  Error_Handler();
  }

  /***************************************************************/
  /************************ERRORS HANDLERS************************/
  /***************************************************************/

  // Initialize error handler system
  error_count = 0;
  last_error = ERROR_NONE;
  system_emergency_state = false;

  /***************************************************************/
  /*********************SEND TO GUI STATUS************************/
  /***************************************************************/

  // Send initial status to GUI
  char initial_status[500];
  getSystemStatusForGUI(initial_status, sizeof(initial_status));
  // Send via USB to GUI
  CDC_Transmit_FS((uint8_t*)initial_status, strlen(initial_status));






  // main loop

  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
	  //////////////////////////////////////////////////////////////////////////////////////
	  //////////////////////// Check system health at the start of each loop////////////////
	  //////////////////////////////////////////////////////////////////////////////////////

	    if (!checkSystemHealthStatus()) {
	        // System is in bad state, enter safe mode
	    	//Tell FPGA to turn off TX RF signal by disabling Mixers
	    	HAL_GPIO_WritePin(GPIOD, GPIO_PIN_11, GPIO_PIN_RESET);
	        systemPowerDownSequence();

	        char emergency_msg[] = "SYSTEM IN SAFE MODE - Manual intervention required\r\n";
	        HAL_UART_Transmit(&huart3, (uint8_t*)emergency_msg, strlen(emergency_msg), 1000);

	        // Blink all LEDs to indicate safe mode
	        while (system_emergency_state) {
	            HAL_GPIO_TogglePin(LED_1_GPIO_Port, LED_1_Pin);
	            HAL_GPIO_TogglePin(LED_2_GPIO_Port, LED_2_Pin);
	            HAL_GPIO_TogglePin(LED_3_GPIO_Port, LED_3_Pin);
	            HAL_GPIO_TogglePin(LED_4_GPIO_Port, LED_4_Pin);
	        }
	    }
	  //////////////////////////////////////////////////////////////////////////////////////
	  ////////////////////////// Monitor ADF4382A lock status periodically//////////////////
	  //////////////////////////////////////////////////////////////////////////////////////

      // Monitor lock status periodically
      static uint32_t last_check = 0;
      if (HAL_GetTick() - last_check > 5000) {
          ADF4382A_CheckLockStatus(&lo_manager, &tx_locked, &rx_locked);

          if (!tx_locked || !rx_locked) {
              printf("LO Lock Lost! TX: %s, RX: %s\n",
                     tx_locked ? "LOCKED" : "UNLOCKED",
                     rx_locked ? "LOCKED" : "UNLOCKED");
          }

          last_check = HAL_GetTick();
      }
	  //////////////////////////////////////////////////////////////////////////////////////
	  ////////////////////////// Monitor Temperature Sensors periodically//////////////////
	  //////////////////////////////////////////////////////////////////////////////////////
	  /* Read all 8 channels from ADC2 and calculate Idq*/
      // Monitor T°C periodically
      static uint32_t last_check1 = 0;
		  if (HAL_GetTick() - last_check1 > 5000) {
		  //TMP37 3.3V-->165°C & ADS7830 3.3V-->255 => Temperature_n °C = ADC_val_n * 165/255
		  Temperature_1 = ADS7830_Measure_SingleEnded(&hadc3, 0)*0.64705f;
		  Temperature_2 = ADS7830_Measure_SingleEnded(&hadc3, 1)*0.64705f;
		  Temperature_3 = ADS7830_Measure_SingleEnded(&hadc3, 2)*0.64705f;
		  Temperature_4 = ADS7830_Measure_SingleEnded(&hadc3, 3)*0.64705f;
		  Temperature_5 = ADS7830_Measure_SingleEnded(&hadc3, 4)*0.64705f;
		  Temperature_6 = ADS7830_Measure_SingleEnded(&hadc3, 5)*0.64705f;
		  Temperature_7 = ADS7830_Measure_SingleEnded(&hadc3, 6)*0.64705f;
		  Temperature_8 = ADS7830_Measure_SingleEnded(&hadc3, 7)*0.64705f;

		  //(20 mV/°C on TMP37) QPA2962 RF amplifier Operating Temp. Range, TBASE min−40 normal+25 max+85 °C
		  int Max_Temp = 25;
		  if((Temperature_1>Max_Temp)||(Temperature_2>Max_Temp)||(Temperature_3>Max_Temp)||(Temperature_4>Max_Temp)
				  ||(Temperature_5>Max_Temp)||(Temperature_6>Max_Temp)||(Temperature_7>Max_Temp)||(Temperature_8>Max_Temp))
			{
			  HAL_GPIO_WritePin(EN_DIS_COOLING_GPIO_Port, EN_DIS_COOLING_Pin, GPIO_PIN_SET);
			}
		  else{
			  HAL_GPIO_WritePin(EN_DIS_COOLING_GPIO_Port, EN_DIS_COOLING_Pin, GPIO_PIN_RESET);
		  }


		  last_check = HAL_GetTick();
		  }
	  //////////////////////////////////////////////////////////////////////////////////////
	  /////////////////////////////////////ADAR1000/////////////////////////////////////////
	  //////////////////////////////////////////////////////////////////////////////////////
	 //phase_step = 0 => phase = 0°
	 //phase_step = 127 => phase = 360°
	 //steering angle (rad)= arcsin(phase_dif/Pi)

      runRadarPulseSequence();

      // Optional: Add system monitoring here
      // Check temperatures, power levels, etc.


    /* USER CODE END WHILE */




    /* USER CODE BEGIN 3 */
  }
  /* 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 LSE Drive Capability
  */
  HAL_PWR_EnableBkUpAccess();

  /** Configure the main internal regulator output voltage
  */
  __HAL_RCC_PWR_CLK_ENABLE();
  __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE3);

  /** 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 = 25;
  RCC_OscInitStruct.PLL.PLLN = 144;
  RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
  RCC_OscInitStruct.PLL.PLLQ = 3;
  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_DIV2;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
  {
    Error_Handler();
  }
}

/**
  * @brief Peripherals Common Clock Configuration
  * @retval None
  */
void PeriphCommonClock_Config(void)
{
  RCC_PeriphCLKInitTypeDef PeriphClkInitStruct = {0};

  /** Initializes the peripherals clock
  */
  PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_TIM;
  PeriphClkInitStruct.TIMPresSelection = RCC_TIMPRES_ACTIVATED;

  if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK)
  {
    Error_Handler();
  }
}

/**
  * @brief I2C1 Initialization Function
  * @param None
  * @retval None
  */
static void MX_I2C1_Init(void)
{

  /* USER CODE BEGIN I2C1_Init 0 */

  /* USER CODE END I2C1_Init 0 */

  /* USER CODE BEGIN I2C1_Init 1 */

  /* USER CODE END I2C1_Init 1 */
  hi2c1.Instance = I2C1;
  hi2c1.Init.Timing = 0x00808CD2;
  hi2c1.Init.OwnAddress1 = 0;
  hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
  hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
  hi2c1.Init.OwnAddress2 = 0;
  hi2c1.Init.OwnAddress2Masks = I2C_OA2_NOMASK;
  hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
  hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
  if (HAL_I2C_Init(&hi2c1) != HAL_OK)
  {
    Error_Handler();
  }

  /** Configure Analogue filter
  */
  if (HAL_I2CEx_ConfigAnalogFilter(&hi2c1, I2C_ANALOGFILTER_ENABLE) != HAL_OK)
  {
    Error_Handler();
  }

  /** Configure Digital filter
  */
  if (HAL_I2CEx_ConfigDigitalFilter(&hi2c1, 0) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN I2C1_Init 2 */

  /* USER CODE END I2C1_Init 2 */

}

/**
  * @brief I2C2 Initialization Function
  * @param None
  * @retval None
  */
static void MX_I2C2_Init(void)
{

  /* USER CODE BEGIN I2C2_Init 0 */

  /* USER CODE END I2C2_Init 0 */

  /* USER CODE BEGIN I2C2_Init 1 */

  /* USER CODE END I2C2_Init 1 */
  hi2c2.Instance = I2C2;
  hi2c2.Init.Timing = 0x00808CD2;
  hi2c2.Init.OwnAddress1 = 0;
  hi2c2.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
  hi2c2.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
  hi2c2.Init.OwnAddress2 = 0;
  hi2c2.Init.OwnAddress2Masks = I2C_OA2_NOMASK;
  hi2c2.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
  hi2c2.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
  if (HAL_I2C_Init(&hi2c2) != HAL_OK)
  {
    Error_Handler();
  }

  /** Configure Analogue filter
  */
  if (HAL_I2CEx_ConfigAnalogFilter(&hi2c2, I2C_ANALOGFILTER_ENABLE) != HAL_OK)
  {
    Error_Handler();
  }

  /** Configure Digital filter
  */
  if (HAL_I2CEx_ConfigDigitalFilter(&hi2c2, 0) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN I2C2_Init 2 */

  /* USER CODE END I2C2_Init 2 */

}

/**
  * @brief I2C3 Initialization Function
  * @param None
  * @retval None
  */
static void MX_I2C3_Init(void)
{

  /* USER CODE BEGIN I2C3_Init 0 */

  /* USER CODE END I2C3_Init 0 */

  /* USER CODE BEGIN I2C3_Init 1 */

  /* USER CODE END I2C3_Init 1 */
  hi2c3.Instance = I2C3;
  hi2c3.Init.Timing = 0x00808CD2;
  hi2c3.Init.OwnAddress1 = 0;
  hi2c3.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
  hi2c3.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
  hi2c3.Init.OwnAddress2 = 0;
  hi2c3.Init.OwnAddress2Masks = I2C_OA2_NOMASK;
  hi2c3.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
  hi2c3.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
  if (HAL_I2C_Init(&hi2c3) != HAL_OK)
  {
    Error_Handler();
  }

  /** Configure Analogue filter
  */
  if (HAL_I2CEx_ConfigAnalogFilter(&hi2c3, I2C_ANALOGFILTER_ENABLE) != HAL_OK)
  {
    Error_Handler();
  }

  /** Configure Digital filter
  */
  if (HAL_I2CEx_ConfigDigitalFilter(&hi2c3, 0) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN I2C3_Init 2 */

  /* USER CODE END I2C3_Init 2 */

}

/**
  * @brief SPI1 Initialization Function
  * @param None
  * @retval None
  */
static void MX_SPI1_Init(void)
{

  /* USER CODE BEGIN SPI1_Init 0 */

  /* USER CODE END SPI1_Init 0 */

  /* USER CODE BEGIN SPI1_Init 1 */

  /* USER CODE END SPI1_Init 1 */
  /* SPI1 parameter configuration*/
  hspi1.Instance = SPI1;
  hspi1.Init.Mode = SPI_MODE_MASTER;
  hspi1.Init.Direction = SPI_DIRECTION_2LINES;
  hspi1.Init.DataSize = SPI_DATASIZE_8BIT;
  hspi1.Init.CLKPolarity = SPI_POLARITY_LOW;
  hspi1.Init.CLKPhase = SPI_PHASE_1EDGE;
  hspi1.Init.NSS = SPI_NSS_SOFT;
  hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_2;
  hspi1.Init.FirstBit = SPI_FIRSTBIT_MSB;
  hspi1.Init.TIMode = SPI_TIMODE_DISABLE;
  hspi1.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
  hspi1.Init.CRCPolynomial = 7;
  hspi1.Init.CRCLength = SPI_CRC_LENGTH_DATASIZE;
  hspi1.Init.NSSPMode = SPI_NSS_PULSE_ENABLE;
  if (HAL_SPI_Init(&hspi1) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN SPI1_Init 2 */

  /* USER CODE END SPI1_Init 2 */

}

/**
  * @brief SPI4 Initialization Function
  * @param None
  * @retval None
  */
static void MX_SPI4_Init(void)
{

  /* USER CODE BEGIN SPI4_Init 0 */

  /* USER CODE END SPI4_Init 0 */

  /* USER CODE BEGIN SPI4_Init 1 */

  /* USER CODE END SPI4_Init 1 */
  /* SPI4 parameter configuration*/
  hspi4.Instance = SPI4;
  hspi4.Init.Mode = SPI_MODE_MASTER;
  hspi4.Init.Direction = SPI_DIRECTION_2LINES;
  hspi4.Init.DataSize = SPI_DATASIZE_8BIT;
  hspi4.Init.CLKPolarity = SPI_POLARITY_LOW;
  hspi4.Init.CLKPhase = SPI_PHASE_1EDGE;
  hspi4.Init.NSS = SPI_NSS_SOFT;
  hspi4.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_2;
  hspi4.Init.FirstBit = SPI_FIRSTBIT_MSB;
  hspi4.Init.TIMode = SPI_TIMODE_DISABLE;
  hspi4.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
  hspi4.Init.CRCPolynomial = 7;
  hspi4.Init.CRCLength = SPI_CRC_LENGTH_DATASIZE;
  hspi4.Init.NSSPMode = SPI_NSS_PULSE_ENABLE;
  if (HAL_SPI_Init(&hspi4) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN SPI4_Init 2 */

  /* USER CODE END SPI4_Init 2 */

}

/**
  * @brief TIM1 Initialization Function
  * @param None
  * @retval None
  */
static void MX_TIM1_Init(void)
{

  /* USER CODE BEGIN TIM1_Init 0 */

  /* USER CODE END TIM1_Init 0 */

  TIM_ClockConfigTypeDef sClockSourceConfig = {0};
  TIM_MasterConfigTypeDef sMasterConfig = {0};

  /* USER CODE BEGIN TIM1_Init 1 */

  /* USER CODE END TIM1_Init 1 */
  htim1.Instance = TIM1;
  htim1.Init.Prescaler = 71;
  htim1.Init.CounterMode = TIM_COUNTERMODE_UP;
  htim1.Init.Period = 0xffff-1;
  htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
  htim1.Init.RepetitionCounter = 0;
  htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
  if (HAL_TIM_Base_Init(&htim1) != HAL_OK)
  {
    Error_Handler();
  }
  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
  if (HAL_TIM_ConfigClockSource(&htim1, &sClockSourceConfig) != HAL_OK)
  {
    Error_Handler();
  }
  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
  sMasterConfig.MasterOutputTrigger2 = TIM_TRGO2_RESET;
  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
  if (HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN TIM1_Init 2 */

  /* USER CODE END TIM1_Init 2 */

}

/**
  * @brief UART5 Initialization Function
  * @param None
  * @retval None
  */
static void MX_UART5_Init(void)
{

  /* USER CODE BEGIN UART5_Init 0 */

  /* USER CODE END UART5_Init 0 */

  /* USER CODE BEGIN UART5_Init 1 */

  /* USER CODE END UART5_Init 1 */
  huart5.Instance = UART5;
  huart5.Init.BaudRate = 9600;
  huart5.Init.WordLength = UART_WORDLENGTH_8B;
  huart5.Init.StopBits = UART_STOPBITS_1;
  huart5.Init.Parity = UART_PARITY_NONE;
  huart5.Init.Mode = UART_MODE_TX_RX;
  huart5.Init.HwFlowCtl = UART_HWCONTROL_NONE;
  huart5.Init.OverSampling = UART_OVERSAMPLING_16;
  huart5.Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE;
  huart5.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT;
  if (HAL_UART_Init(&huart5) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN UART5_Init 2 */

  /* USER CODE END UART5_Init 2 */

}

/**
  * @brief USART3 Initialization Function
  * @param None
  * @retval None
  */
static void MX_USART3_UART_Init(void)
{

  /* USER CODE BEGIN USART3_Init 0 */

  /* USER CODE END USART3_Init 0 */

  /* USER CODE BEGIN USART3_Init 1 */

  /* USER CODE END USART3_Init 1 */
  huart3.Instance = USART3;
  huart3.Init.BaudRate = 115200;
  huart3.Init.WordLength = UART_WORDLENGTH_8B;
  huart3.Init.StopBits = UART_STOPBITS_1;
  huart3.Init.Parity = UART_PARITY_NONE;
  huart3.Init.Mode = UART_MODE_TX_RX;
  huart3.Init.HwFlowCtl = UART_HWCONTROL_NONE;
  huart3.Init.OverSampling = UART_OVERSAMPLING_16;
  huart3.Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE;
  huart3.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT;
  if (HAL_UART_Init(&huart3) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN USART3_Init 2 */

  /* USER CODE END USART3_Init 2 */

}

/**
  * @brief GPIO Initialization Function
  * @param None
  * @retval None
  */
static void MX_GPIO_Init(void)
{
  GPIO_InitTypeDef GPIO_InitStruct = {0};
  /* USER CODE BEGIN MX_GPIO_Init_1 */

  /* USER CODE END MX_GPIO_Init_1 */

  /* GPIO Ports Clock Enable */
  __HAL_RCC_GPIOE_CLK_ENABLE();
  __HAL_RCC_GPIOC_CLK_ENABLE();
  __HAL_RCC_GPIOF_CLK_ENABLE();
  __HAL_RCC_GPIOH_CLK_ENABLE();
  __HAL_RCC_GPIOA_CLK_ENABLE();
  __HAL_RCC_GPIOG_CLK_ENABLE();
  __HAL_RCC_GPIOB_CLK_ENABLE();
  __HAL_RCC_GPIOD_CLK_ENABLE();

  /*Configure GPIO pin Output Level */
  HAL_GPIO_WritePin(GPIOF, AD9523_PD_Pin|AD9523_REF_SEL_Pin|AD9523_SYNC_Pin|AD9523_RESET_Pin
                          |AD9523_CS_Pin|AD9523_EEPROM_SEL_Pin|LED_1_Pin|LED_2_Pin
                          |LED_3_Pin|LED_4_Pin, GPIO_PIN_RESET);

  /*Configure GPIO pin Output Level */
  HAL_GPIO_WritePin(GPIOA, ADAR_1_CS_3V3_Pin|ADAR_2_CS_3V3_Pin|ADAR_3_CS_3V3_Pin|ADAR_4_CS_3V3_Pin, GPIO_PIN_RESET);

  /*Configure GPIO pin Output Level */
  HAL_GPIO_WritePin(GPIOG, EN_P_5V0_PA1_Pin|EN_P_5V0_PA2_Pin|EN_P_5V0_PA3_Pin|EN_P_5V5_PA_Pin
                          |EN_P_1V8_CLOCK_Pin|EN_P_3V3_CLOCK_Pin|ADF4382_RX_DELADJ_Pin|ADF4382_RX_DELSTR_Pin
                          |ADF4382_RX_CE_Pin|ADF4382_RX_CS_Pin|ADF4382_TX_DELSTR_Pin|ADF4382_TX_DELADJ_Pin
                          |ADF4382_TX_CS_Pin|ADF4382_TX_CE_Pin, GPIO_PIN_RESET);

  /*Configure GPIO pin Output Level */
  HAL_GPIO_WritePin(GPIOE, EN_P_1V0_FPGA_Pin|EN_P_1V8_FPGA_Pin|EN_P_3V3_FPGA_Pin|EN_P_5V0_ADAR_Pin
                          |EN_P_3V3_ADAR12_Pin|EN_P_3V3_ADAR34_Pin|EN_P_3V3_ADTR_Pin|EN_P_3V3_SW_Pin
                          |EN_P_3V3_VDD_SW_Pin, GPIO_PIN_RESET);

  /*Configure GPIO pin Output Level */
  HAL_GPIO_WritePin(GPIOD, GPIO_PIN_8|GPIO_PIN_9|GPIO_PIN_10|GPIO_PIN_11
                          |STEPPER_CW_P_Pin|STEPPER_CLK_P_Pin|EN_DIS_RFPA_VDD_Pin|EN_DIS_COOLING_Pin, GPIO_PIN_RESET);

  /*Configure GPIO pin Output Level */
  HAL_GPIO_WritePin(GPIOB, DAC_1_VG_CLR_Pin|DAC_1_VG_LDAC_Pin|DAC_2_VG_CLR_Pin|DAC_2_VG_LDAC_Pin, GPIO_PIN_RESET);

  /*Configure GPIO pins : AD9523_PD_Pin AD9523_REF_SEL_Pin AD9523_SYNC_Pin AD9523_RESET_Pin
                           AD9523_CS_Pin AD9523_EEPROM_SEL_Pin LED_1_Pin LED_2_Pin
                           LED_3_Pin LED_4_Pin */
  GPIO_InitStruct.Pin = AD9523_PD_Pin|AD9523_REF_SEL_Pin|AD9523_SYNC_Pin|AD9523_RESET_Pin
                          |AD9523_CS_Pin|AD9523_EEPROM_SEL_Pin|LED_1_Pin|LED_2_Pin
                          |LED_3_Pin|LED_4_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
  HAL_GPIO_Init(GPIOF, &GPIO_InitStruct);

  /*Configure GPIO pins : AD9523_STATUS0_Pin AD9523_STATUS1_Pin */
  GPIO_InitStruct.Pin = AD9523_STATUS0_Pin|AD9523_STATUS1_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  HAL_GPIO_Init(GPIOF, &GPIO_InitStruct);

  /*Configure GPIO pins : ADAR_1_CS_3V3_Pin ADAR_2_CS_3V3_Pin ADAR_3_CS_3V3_Pin ADAR_4_CS_3V3_Pin */
  GPIO_InitStruct.Pin = ADAR_1_CS_3V3_Pin|ADAR_2_CS_3V3_Pin|ADAR_3_CS_3V3_Pin|ADAR_4_CS_3V3_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
  HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);

  /*Configure GPIO pins : EN_P_5V0_PA1_Pin EN_P_5V0_PA2_Pin EN_P_5V0_PA3_Pin EN_P_5V5_PA_Pin
                           EN_P_1V8_CLOCK_Pin EN_P_3V3_CLOCK_Pin ADF4382_RX_DELADJ_Pin ADF4382_RX_DELSTR_Pin
                           ADF4382_RX_CE_Pin ADF4382_RX_CS_Pin ADF4382_TX_DELSTR_Pin ADF4382_TX_DELADJ_Pin
                           ADF4382_TX_CS_Pin ADF4382_TX_CE_Pin */
  GPIO_InitStruct.Pin = EN_P_5V0_PA1_Pin|EN_P_5V0_PA2_Pin|EN_P_5V0_PA3_Pin|EN_P_5V5_PA_Pin
                          |EN_P_1V8_CLOCK_Pin|EN_P_3V3_CLOCK_Pin|ADF4382_RX_DELADJ_Pin|ADF4382_RX_DELSTR_Pin
                          |ADF4382_RX_CE_Pin|ADF4382_RX_CS_Pin|ADF4382_TX_DELSTR_Pin|ADF4382_TX_DELADJ_Pin
                          |ADF4382_TX_CS_Pin|ADF4382_TX_CE_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
  HAL_GPIO_Init(GPIOG, &GPIO_InitStruct);

  /*Configure GPIO pins : EN_P_1V0_FPGA_Pin EN_P_1V8_FPGA_Pin EN_P_3V3_FPGA_Pin EN_P_5V0_ADAR_Pin
                           EN_P_3V3_ADAR12_Pin EN_P_3V3_ADAR34_Pin EN_P_3V3_ADTR_Pin EN_P_3V3_SW_Pin
                           EN_P_3V3_ADAR12EN_P_3V3_VDD_SW_Pin */
  GPIO_InitStruct.Pin = EN_P_1V0_FPGA_Pin|EN_P_1V8_FPGA_Pin|EN_P_3V3_FPGA_Pin|EN_P_5V0_ADAR_Pin
                          |EN_P_3V3_ADAR12_Pin|EN_P_3V3_ADAR34_Pin|EN_P_3V3_ADTR_Pin|EN_P_3V3_SW_Pin
                          |EN_P_3V3_VDD_SW_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
  HAL_GPIO_Init(GPIOE, &GPIO_InitStruct);

  /*Configure GPIO pins : PD8 PD9 PD10 PD11
                           STEPPER_CW_P_Pin STEPPER_CLK_P_Pin EN_DIS_RFPA_VDD_Pin EN_DIS_COOLING_Pin */
  GPIO_InitStruct.Pin = GPIO_PIN_8|GPIO_PIN_9|GPIO_PIN_10|GPIO_PIN_11|GPIO_PIN_12
                          |STEPPER_CW_P_Pin|STEPPER_CLK_P_Pin|EN_DIS_RFPA_VDD_Pin|EN_DIS_COOLING_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
  HAL_GPIO_Init(GPIOD, &GPIO_InitStruct);

  /*Configure GPIO pins : PD12 PD13 PD14 PD15 */
  GPIO_InitStruct.Pin = GPIO_PIN_13|GPIO_PIN_14|GPIO_PIN_15;
  GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  HAL_GPIO_Init(GPIOD, &GPIO_InitStruct);

  /*Configure GPIO pins : ADF4382_RX_LKDET_Pin ADF4382_TX_LKDET_Pin */
  GPIO_InitStruct.Pin = ADF4382_RX_LKDET_Pin|ADF4382_TX_LKDET_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  HAL_GPIO_Init(GPIOG, &GPIO_InitStruct);

  /*Configure GPIO pins : MAG_DRDY_Pin ACC_INT_Pin GYR_INT_Pin */
  GPIO_InitStruct.Pin = MAG_DRDY_Pin|ACC_INT_Pin|GYR_INT_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);

  /*Configure GPIO pins : DAC_1_VG_CLR_Pin DAC_1_VG_LDAC_Pin DAC_2_VG_CLR_Pin DAC_2_VG_LDAC_Pin */
  GPIO_InitStruct.Pin = DAC_1_VG_CLR_Pin|DAC_1_VG_LDAC_Pin|DAC_2_VG_CLR_Pin|DAC_2_VG_LDAC_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
  HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);

  /* USER CODE BEGIN MX_GPIO_Init_2 */

  /* USER CODE END MX_GPIO_Init_2 */
}

/* USER CODE BEGIN 4 */
/* ============================================================
 *                  DEVICE INITIALIZATION
 * ============================================================ */



/* USER CODE END 4 */

 /* MPU Configuration */

void MPU_Config(void)
{
  MPU_Region_InitTypeDef MPU_InitStruct = {0};

  /* Disables the MPU */
  HAL_MPU_Disable();

  /** Initializes and configures the Region and the memory to be protected
  */
  MPU_InitStruct.Enable = MPU_REGION_ENABLE;
  MPU_InitStruct.Number = MPU_REGION_NUMBER0;
  MPU_InitStruct.BaseAddress = 0x0;
  MPU_InitStruct.Size = MPU_REGION_SIZE_4GB;
  MPU_InitStruct.SubRegionDisable = 0x87;
  MPU_InitStruct.TypeExtField = MPU_TEX_LEVEL0;
  MPU_InitStruct.AccessPermission = MPU_REGION_NO_ACCESS;
  MPU_InitStruct.DisableExec = MPU_INSTRUCTION_ACCESS_DISABLE;
  MPU_InitStruct.IsShareable = MPU_ACCESS_SHAREABLE;
  MPU_InitStruct.IsCacheable = MPU_ACCESS_NOT_CACHEABLE;
  MPU_InitStruct.IsBufferable = MPU_ACCESS_NOT_BUFFERABLE;

  HAL_MPU_ConfigRegion(&MPU_InitStruct);
  /* Enables the MPU */
  HAL_MPU_Enable(MPU_PRIVILEGED_DEFAULT);

}

/**
  * @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();
  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) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */
