/*
 * start.cpp
 *
 *  Created on: Oct 31, 2024
 *      Author: Gabriel
 */

#include "start.h"
#include <cstdio>
#include "FreeRTOS.h"
#include "LogDriver.hpp"
#include "Queue_STM32.hpp"
#include "HIDRAW_STM32.hpp"

#include "main.h"

#include <Components/TelemetryDispatcher/TelemetryDispatcher.hpp>

Queue_STM32<TelemetryPacket, 1> telemetryQueue("telemetryQueue");
TelemetryDispatcher componentTelemetryDispatcher(&telemetryQueue, HIDRAW_STM32::getInstance());

void executableDispatch(void* _executable){
	Executable* executable = static_cast<Executable*>(_executable);
	executable->execute();
	vTaskDelete(NULL);
}

TelemetryPacket pkt;

void start(){
	printf("----------------INIT----------------\n");
	printf("X and Y: %u, %u\n", (uint16_t)HAL_GetUIDw0(), (uint16_t)HAL_GetUIDw0()>>16);
	printf("Wafer number: %u\n", (uint8_t)HAL_GetUIDw1());
	printf("Lot number: %c%c%c%c%c%c%c\n",
			(uint8_t)(HAL_GetUIDw1()>>8),
			(uint8_t)(HAL_GetUIDw1()>>16),
			(uint8_t)(HAL_GetUIDw1()>>24),
			(uint8_t)(HAL_GetUIDw2()),
			(uint8_t)(HAL_GetUIDw2()>>8),
			(uint8_t)(HAL_GetUIDw2()>>16),
			(uint8_t)(HAL_GetUIDw2()>>24)
			);
	// Init log facility
	LogDriver::getInstance()->init();
	TaskHandle_t hTaskLogDriver;
	xTaskCreate(executableDispatch, "LogDriver", 512, LogDriver::getInstance(), 5, &hTaskLogDriver);

	// Init queues
	telemetryQueue.init();

	// Init shared resources
	HIDRAW_STM32::getInstance()->init();

	// Init components
	componentTelemetryDispatcher.init();

	// Init tasks
	TaskHandle_t hTaskTelemetryDispatcher;
	xTaskCreate(executableDispatch, "TelemetryDispatcher", 512, &componentTelemetryDispatcher, 20, &hTaskTelemetryDispatcher);

	vTaskDelete(nullptr);
}
