#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdint.h>

#include "stm32f4xx.h"      /* CMSIS device header - ensure correct device */

/* FreeRTOS includes */
#include "FreeRTOS.h"
#include "task.h"
#include "timers.h"
#include "semphr.h"
#include "queue.h"

/* ------------------- Configuration ------------------- */
#define CONTROL_TASK_PERIOD_MS 5      /* 200 Hz */
#define SENSOR_DATA_SIZE       10
#define QUEUE_LENGTH           5

/* Run-time counter frequency: use SystemCoreClock for DWT cycles */
#define RUN_TIME_COUNTER_HZ    ((uint32_t) SystemCoreClock)

/* ------------------- Types ------------------- */
typedef struct {
    int param1;
    int param2;
} Control_Config_t;

/* ------------------- Globals ------------------- */
static TaskHandle_t xControlTaskHandle = NULL;
static TaskHandle_t xSensorTaskHandle  = NULL;
static TaskHandle_t xCommsTaskHandle   = NULL;
static TaskHandle_t xWatchdogTaskHandle= NULL;

static SemaphoreHandle_t xBinarySemaphoreComms = NULL;
static SemaphoreHandle_t xMutexConfig = NULL;
static QueueHandle_t xSensorQueue = NULL;

static Control_Config_t gControlConfig = {0};

/* Instrumentation */
static volatile uint32_t ulControlMaxRunTimeCounter = 0;
static volatile uint32_t ulControlLastRunTimeCounter = 0;

/* Mock data */
typedef struct {
    uint8_t data[SENSOR_DATA_SIZE];
} SensorData_t;

typedef struct {
    char cmd[16];
} Command_t;

/* Forward decls */
void vControlTask(void *params);
void vSensorTask(void *params);
void vCommsTask(void *params);
void vWatchdogTask(void *params);
void vRTMonitorTask(void *params);
void vControlTimerCallback(TimerHandle_t xTimer);
void Mock_SendSensorData(void);
void Mock_SendCommand(void);

/* Run-time counter (DWT) */
void vConfigureTimerForRunTimeStats(void);
unsigned long ulGetRunTimeCounterValue(void);

#ifndef portCONFIGURE_TIMER_FOR_RUN_TIME_STATS
    #define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() vConfigureTimerForRunTimeStats()
#endif
#ifndef portGET_RUN_TIME_COUNTER_VALUE
    #define portGET_RUN_TIME_COUNTER_VALUE() ulGetRunTimeCounterValue()
#endif

/* Software timer */
static TimerHandle_t xControlTimer = NULL;

/* ------------------- main ------------------- */
int main(void)
{
    /* Optional: HAL and system init if using HAL */
    /* HAL_Init(); SystemClock_Config(); */

    /* Create primitives */
    xMutexConfig = xSemaphoreCreateMutex();
    configASSERT(xMutexConfig != NULL);

    xBinarySemaphoreComms = xSemaphoreCreateBinary();
    configASSERT(xBinarySemaphoreComms != NULL);

    xSensorQueue = xQueueCreate(QUEUE_LENGTH, sizeof(SensorData_t));
    configASSERT(xSensorQueue != NULL);

    /* configure run-time counter (DWT) */
    portCONFIGURE_TIMER_FOR_RUN_TIME_STATS();

    /* Create tasks (stack sizes chosen conservatively) */
    xTaskCreate(vControlTask,  "vControlTask",  configMINIMAL_STACK_SIZE * 4, NULL, 3, &xControlTaskHandle);
    xTaskCreate(vSensorTask,   "vSensorTask",   configMINIMAL_STACK_SIZE * 3, NULL, 2, &xSensorTaskHandle);
    xTaskCreate(vCommsTask,    "vCommsTask",    configMINIMAL_STACK_SIZE * 3, NULL, 1, &xCommsTaskHandle);
    xTaskCreate(vWatchdogTask, "vWatchdogTask", configMINIMAL_STACK_SIZE * 2, NULL, 0, &xWatchdogTaskHandle);

    /* Monitor task - prints verification info */
    xTaskCreate(vRTMonitorTask, "vRTMonitor", configMINIMAL_STACK_SIZE * 5, NULL, 1, NULL);

    /* Create 5ms software timer for the control loop */
    xControlTimer = xTimerCreate("ControlTimer",
                                 pdMS_TO_TICKS(CONTROL_TASK_PERIOD_MS),
                                 pdTRUE,
                                 NULL,
                                 vControlTimerCallback);
    configASSERT(xControlTimer != NULL);
    xTimerStart(xControlTimer, 0);

    /* Start scheduler */
    vTaskStartScheduler();

    /* Should never get here */
    for(;;);
}

/* ------------------- Tasks ------------------- */

void vControlTask(void *params)
{
    for(;;)
    {
        /* Wait for notification from timer */
        ulTaskNotifyTake(pdTRUE, portMAX_DELAY);

        /* Timestamp start */
        uint32_t start = portGET_RUN_TIME_COUNTER_VALUE();

        /* Simulated control computations (stub) */
        volatile int dummy = 0;
        for (int i = 0; i < 1000; ++i) dummy += i;

        /* Access shared config briefly (trylock) */
        if (xSemaphoreTake(xMutexConfig, pdMS_TO_TICKS(1)) == pdTRUE) {
            /* read/modify gControlConfig if required */
            xSemaphoreGive(xMutexConfig);
        }

        /* Timestamp end */
        uint32_t end = portGET_RUN_TIME_COUNTER_VALUE();
        uint32_t elapsed;
        if (end >= start) elapsed = end - start;
        else elapsed = (0xFFFFFFFFUL - start) + 1UL + end;

        taskENTER_CRITICAL();
        if (elapsed > ulControlMaxRunTimeCounter) ulControlMaxRunTimeCounter = elapsed;
        ulControlLastRunTimeCounter = elapsed;
        taskEXIT_CRITICAL();

        printf("[ControlTask] tick=%lu, elapsed_ticks=%lu\n",
               (unsigned long)xTaskGetTickCount(),
               (unsigned long)elapsed);
    }
}

void vSensorTask(void *params)
{
    SensorData_t sensorData;
    for (;;)
    {
        if (xQueueReceive(xSensorQueue, &sensorData, portMAX_DELAY) == pdTRUE)
        {
            int sum = 0;
            for (int i = 0; i < SENSOR_DATA_SIZE; ++i) sum += sensorData.data[i];

            if (xSemaphoreTake(xMutexConfig, pdMS_TO_TICKS(2)) == pdTRUE)
            {
                gControlConfig.param1 = sum;
                xSemaphoreGive(xMutexConfig);
            }

            printf("[SensorTask] sum=%d\n", sum);
        }
    }
}

void vCommsTask(void *params)
{
    Command_t cmd;
    for (;;)
    {
        if (xSemaphoreTake(xBinarySemaphoreComms, portMAX_DELAY) == pdTRUE)
        {
            strcpy(cmd.cmd, "MOVE_FORWARD");

            if (xSemaphoreTake(xMutexConfig, pdMS_TO_TICKS(2)) == pdTRUE)
            {
                gControlConfig.param2 += 1;
                xSemaphoreGive(xMutexConfig);
            }

            printf("[CommsTask] cmd=%s\n", cmd.cmd);
        }
    }
}

void vWatchdogTask(void *params)
{
    for (;;)
    {
        printf("[WatchdogTask] fed tick=%lu\n", (unsigned long)xTaskGetTickCount());
        vTaskDelay(pdMS_TO_TICKS(100));
    }
}

/* ------------------- Monitor Task (fixed-point printing, no %f) ------------------- */
void vRTMonitorTask(void *params)
{
    const TickType_t xDelay = pdMS_TO_TICKS(500); /* print twice/sec */

    for (;;)
    {
        vTaskDelay(xDelay);

        /* Get tasks count and allocate array */
        UBaseType_t uxArraySize = uxTaskGetNumberOfTasks();
        TaskStatus_t *pxTaskStatusArray = pvPortMalloc(uxArraySize * sizeof(TaskStatus_t));
        if (pxTaskStatusArray == NULL) {
            printf("[Monitor] Failed to alloc TaskStatus_t array\n");
            continue;
        }

        uint32_t ulTotalRunTime = 0;
        UBaseType_t uxTasks = uxTaskGetSystemState(pxTaskStatusArray, uxArraySize, &ulTotalRunTime);

        printf("\n=== Task Runtime / Stack (fixed-point) ===\n");
        printf("Name          RunTime    %%Time   StackHigh(bytes)\n");
        for (UBaseType_t i = 0; i < uxTasks; ++i)
        {
            const char *name = pxTaskStatusArray[i].pcTaskName;
            uint32_t runTime = pxTaskStatusArray[i].ulRunTimeCounter;
            /* usStackHighWaterMark is in words (StackType_t units) */
            UBaseType_t stackHighWords = pxTaskStatusArray[i].usStackHighWaterMark;
            unsigned int stackHighBytes = (unsigned int)(stackHighWords * sizeof(StackType_t));

            uint32_t percent_x100 = 0; /* percent * 100 (two decimals) */
            if (ulTotalRunTime > 0) {
                percent_x100 = (uint32_t)(( (uint64_t)runTime * 10000ULL ) / (uint64_t)ulTotalRunTime);
            }
            uint32_t intPart = percent_x100 / 100;
            uint32_t fracPart = percent_x100 % 100;

            printf("%-13s %10lu   %3lu.%02lu   %10u\n",
                   name,
                   (unsigned long)runTime,
                   (unsigned long)intPart,
                   (unsigned long)fracPart,
                   (unsigned int)stackHighBytes);
        }
        printf("Total run-time counter = %lu\n", (unsigned long)ulTotalRunTime);

        /* Print Idle percent if present (optional) */
        for (UBaseType_t i = 0; i < uxTasks; ++i) {
            if (strcmp(pxTaskStatusArray[i].pcTaskName, "IDLE") == 0 ||
                strcmp(pxTaskStatusArray[i].pcTaskName, "Idle") == 0 ||
                strcmp(pxTaskStatusArray[i].pcTaskName, "IDLE_TASK") == 0) {
                uint32_t idleRun = pxTaskStatusArray[i].ulRunTimeCounter;
                uint32_t idle_x100 = (ulTotalRunTime>0) ? (uint32_t)(((uint64_t)idleRun * 10000ULL) / ulTotalRunTime) : 0;
                printf("Idle percent = %lu.%02lu %%\n", (unsigned long)(idle_x100/100), (unsigned long)(idle_x100%100));
                break;
            }
        }

        /* Heap status */
        extern size_t xPortGetFreeHeapSize(void);
        extern size_t xPortGetMinimumEverFreeHeapSize(void);
        printf("Heap free: %lu bytes\n", (unsigned long)xPortGetFreeHeapSize());
        printf("Heap min ever free: %lu bytes\n", (unsigned long)xPortGetMinimumEverFreeHeapSize());

        /* vControlTask timing (convert ticks -> microseconds -> ms with 3 decimals) */
        uint32_t maxCnt, lastCnt;
        taskENTER_CRITICAL();
        maxCnt = ulControlMaxRunTimeCounter;
        lastCnt = ulControlLastRunTimeCounter;
        taskEXIT_CRITICAL();

        /* avoid division by zero */
        uint64_t max_us = 0, last_us = 0;
        if (RUN_TIME_COUNTER_HZ > 0) {
            max_us  = ((uint64_t)maxCnt  * 1000000ULL) / (uint64_t)RUN_TIME_COUNTER_HZ;
            last_us = ((uint64_t)lastCnt * 1000000ULL) / (uint64_t)RUN_TIME_COUNTER_HZ;
        }

        uint32_t max_ms_int  = (uint32_t)(max_us / 1000ULL);
        uint32_t max_ms_frac = (uint32_t)(max_us % 1000ULL);
        uint32_t last_ms_int  = (uint32_t)(last_us / 1000ULL);
        uint32_t last_ms_frac = (uint32_t)(last_us % 1000ULL);

        printf("vControlTask last exec = %lu.%03lu ms, max exec = %lu.%03lu ms\n",
               (unsigned long)last_ms_int, (unsigned long)last_ms_frac,
               (unsigned long)max_ms_int,  (unsigned long)max_ms_frac);

        /* Deadline check */
        if ( (max_ms_int < 5) || (max_ms_int == 5 && max_ms_frac == 0) ) {
            printf("DEADLINE CHECK: PASS (max <= 5.000 ms)\n");
        } else {
            printf("DEADLINE CHECK: FAIL (max > 5.000 ms)\n");
        }

        vPortFree(pxTaskStatusArray);
    }
}

/* ------------------- Software Timer callback + mocks ------------------- */
void vControlTimerCallback(TimerHandle_t xTimer)
{
    BaseType_t xHigherPriorityTaskWoken = pdFALSE;

    /* notify control task */
    vTaskNotifyGiveFromISR(xControlTaskHandle, &xHigherPriorityTaskWoken);
    portYIELD_FROM_ISR(xHigherPriorityTaskWoken);

    /* produce mocks */
    Mock_SendSensorData();
    Mock_SendCommand();
}

void Mock_SendSensorData(void)
{
    SensorData_t sensorData;
    for (int i = 0; i < SENSOR_DATA_SIZE; ++i) sensorData.data[i] = (uint8_t)(rand() & 0xFF);

    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
    xQueueSendFromISR(xSensorQueue, &sensorData, &xHigherPriorityTaskWoken);
    portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}

void Mock_SendCommand(void)
{
    static int counter = 0;
    counter++;
    /* every 200 ms (40 * 5ms) */
    if ((counter % 40) == 0)
    {
        BaseType_t xHigherPriorityTaskWoken = pdFALSE;
        xSemaphoreGiveFromISR(xBinarySemaphoreComms, &xHigherPriorityTaskWoken);
        portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
    }
}

/* ------------------- DWT runtime counter ------------------- */
void vConfigureTimerForRunTimeStats(void)
{
#if defined(DWT) && defined(CoreDebug)
    /* Enable trace and cycle counter */
    CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
    DWT->CYCCNT = 0;
    DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
#else
    /* Provide alternative timer if DWT not available on your simulator */
#endif
}

unsigned long ulGetRunTimeCounterValue(void)
{
#if defined(DWT)
    return (unsigned long)DWT->CYCCNT;
#else
    /* Low-res fallback using ticks */
    uint32_t ticks = (uint32_t)xTaskGetTickCount();
    return (unsigned long)( ( (uint64_t)ticks * (uint64_t)RUN_TIME_COUNTER_HZ ) / (uint64_t)configTICK_RATE_HZ );
#endif
}

/* ------------------- FreeRTOS Hook functions ------------------- */
void vApplicationIdleHook(void)
{
    /* Optionally toggle a pin here to verify tickless idle on an oscilloscope */
}

void vApplicationMallocFailedHook(void)
{
    printf("Malloc failed!\n");
    taskDISABLE_INTERRUPTS();
    for(;;);
}

void vApplicationStackOverflowHook(TaskHandle_t xTask, char *pcTaskName)
{
    printf("Stack overflow in %s\n", pcTaskName ? pcTaskName : "UNKNOWN");
    taskDISABLE_INTERRUPTS();
    for(;;);
}

void vApplicationTickHook(void) { }