# Platform Support for ARM R5

#include <FpConfig.hpp>
#include <Fw/Types/BasicTypes.hpp>

namespace Os {

    void initPlatform() {
        // Initialize ARM R5 specific hardware
        SystemInit();  // System initialization
        EnableFPU();   // Enable Floating Point Unit
        ConfigureMPU();// Configure Memory Protection Unit
        
        // Initialize system tick timer for RTOS
        SysTick_Config(SystemCoreClock / configTICK_RATE_HZ);
        
        // Initialize interrupt vector table
        NVIC_SetPriorityGrouping(0);
    }

    void startBareMatrix() {
        while(1) {
            // Bare metal scheduling loop
            taskDispatch();  // Custom task dispatcher for bare metal
            
            // Check for watchdog
            kickWatchdog();
            
            // Process any pending interrupts
            __WFI();  // Wait for interrupt
        }
    }

    // Platform specific memory allocation
    void* allocate(size_t size) {
        // Implement platform specific allocation
        return pvPortMalloc(size);
    }

    void deallocate(void* ptr) {
        // Implement platform specific deallocation
        vPortFree(ptr);
    }

    // Task creation for bare metal
    ThreadHandle createTask(taskCallback function, 
                          const char* name, 
                          uint32_t stackSize, 
                          uint32_t priority) {
        TaskHandle_t handle;
        BaseType_t status = xTaskCreate(
            function,
            name,
            stackSize,
            NULL,
            priority,
            &handle
        );
        
        return status == pdPASS ? handle : NULL;
    }
}