#include "hal_data.h"
#include "MPU.h"
#include <stdio.h>
FSP_CPP_HEADER
void R_BSP_WarmStart(bsp_warm_start_event_t event);
FSP_CPP_FOOTER
int16_t ax, ay, az;
/* Callback function */
static volatile uint32_t g_ms_ticks = 0U;

/*----------------------------------------------------------------------------
   SysTick interrupt service routine.
   This runs every 1 ms once we configure SysTick correctly.
 ----------------------------------------------------------------------------*/
void SysTick_Handler(void)
{
    g_ms_ticks++;
}
void ticks_init(void)
{
    /*
       SysTick_Config is a CMSIS helper that does:
         - SysTick->LOAD = (SystemCoreClock/1000) - 1;
         - SysTick->VAL  = 0;
         - SysTick->CTRL = (CLKSOURCE | TICKINT | ENABLE);
       It automatically sets the clock source to the core clock.
    */
    if (SysTick_Config(SystemCoreClock / 1000U) != 0U)
    {
        /* If SysTick_Config returns non-zero, the reload value was invalid.
           That would only happen if SystemCoreClock/1000 > 0xFFFFFF.
           For most RA6M4 setups (e.g. 120 MHz → 120 000 ticks), this is fine. */
        while (1)
        {
            /* Trap here if you need to debug. */
        }
    }
}
uint32_t get_ticks(void)
{
    return g_ms_ticks;
}

/**
 * @brief  Busy‐wait for the specified number of milliseconds.
 */
void delay_ms(uint32_t ms)
{
    uint32_t start = get_ticks();
    while ((get_ticks() - start) < ms)
    {
        /* spin until elapsed */
    }
}

//extern void initialise_monitor_handles(void);
void hal_entry(void)
{
    //initialise_monitor_handles();

    ticks_init();
    /* Open the I²C channel and check for success */
    fsp_err_t status = R_SCI_I2C_Open(&g_i2c0_ctrl, &g_i2c0_cfg);
    R_SCI_UART_Open(&g_uart0_ctrl, &g_uart0_cfg);

    if (status != FSP_SUCCESS)
    {
        /* Handle error: maybe blink an LED or loop forever */
        __BKPT(0);
    }
    MPU_ConfigTypeDef myConfig;

//    myConfig.Accel_Full_Scale = AFS_SEL_2g;
//    myConfig.ClockSource = Internal_8MHz;
//    myConfig.CONFIG_DLPF = DLPF_260A_256G_Hz;
//    myConfig.Sleep_Mode_Bit = 0;
//    myConfig.Gyro_Full_Scale = FS_SEL_250;

    myConfig.Accel_Full_Scale = AFS_SEL_2g;        // ±2 g
    myConfig.Gyro_Full_Scale  = FS_SEL_250;        // ±250 °/s
    myConfig.CONFIG_DLPF      = DLPF_44A_42G_Hz;   // = 0x03 in the CONFIG register
    myConfig.Sleep_Mode_Bit   = 0;
    myConfig.ClockSource      = Internal_8MHz;     // or PLL_X_GYRO (0x01) if you want a cleaner clock

    MPU6050_Config(&myConfig);

    MPU6050_Config(&myConfig);
    ScaledData_Def meAccel;
    ScaledData_Def meGyro;    // <-- for gyro readings
    /* 3) Example “print accel every rate ms” loop: */
    uint32_t previous = get_ticks();
    const uint32_t rate = 1000;  /* 1000 ms = 1 s */
    uint8_t txbuf[128];
    while (1)
       {
           if ((get_ticks() - previous) >= rate)
           {
               previous = get_ticks();
               MPU6050_Get_Accel_Scale(&meAccel);
               MPU6050_Get_Gyro_Scale(&meGyro);
               snprintf((char*)txbuf, sizeof(txbuf),
                               "Accel: X=%.2f g   Y=%.2f g   Z=%.2f g    "
                               "Gyro: X=%.2f °/s   Y=%.2f °/s   Z=%.2f °/s\r\n",
                               meAccel.x, meAccel.y, meAccel.z,
                               meGyro.x,  meGyro.y,  meGyro.z);
               R_SCI_UART_Write(&g_uart0_ctrl, txbuf, strlen((char*)txbuf));
           }

        /* … you can also do delay_ms(10) here if you want to throttle CPU … */
    }


#if BSP_TZ_SECURE_BUILD
    /* Enter non-secure code */
    R_BSP_NonSecureEnter();
#endif
}

/*******************************************************************************************************************//**
 * This function is called at various points during the startup process.  This implementation uses the event that is
 * called right before main() to set up the pins.
 *
 * @param[in]  event    Where at in the start up process the code is currently at
 **********************************************************************************************************************/
void R_BSP_WarmStart(bsp_warm_start_event_t event)
{
    if (BSP_WARM_START_RESET == event)
    {
#if BSP_FEATURE_FLASH_LP_VERSION != 0

        /* Enable reading from data flash. */
        R_FACI_LP->DFLCTL = 1U;

        /* Would normally have to wait tDSTOP(6us) for data flash recovery. Placing the enable here, before clock and
         * C runtime initialization, should negate the need for a delay since the initialization will typically take more than 6us. */
#endif
    }

    if (BSP_WARM_START_POST_C == event)
    {
        /* C runtime environment and system clocks are setup. */

        /* Configure pins. */
        R_IOPORT_Open (&IOPORT_CFG_CTRL, &IOPORT_CFG_NAME);

#if BSP_CFG_SDRAM_ENABLED

        /* Setup SDRAM and initialize it. Must configure pins first. */
        R_BSP_SdramInit(true);
#endif
    }
}

#if BSP_TZ_SECURE_BUILD

FSP_CPP_HEADER
BSP_CMSE_NONSECURE_ENTRY void template_nonsecure_callable ();

/* Trustzone Secure Projects require at least one nonsecure callable function in order to build (Remove this if it is not required to build). */
BSP_CMSE_NONSECURE_ENTRY void template_nonsecure_callable ()
{

}
FSP_CPP_FOOTER

#endif