/*
 * startup.cpp
 *
 *  Created on: 22 Sep 2023
 *      Author: Dario
 */


#include <cmsis_os2.h>
#include <usart.h>
#include <sta/rtos/mutex.hpp>

// sta-core-specific imports.
#include <sta/devices/stm32/bus/uart.hpp>
#include <sta/debug/printing/printable_uart.hpp>
#include <sta/debug/debug.hpp>
#include <sta/lang.hpp>

// Tacos-specific includes.
#include <sta/tacos/manager.hpp>
#include <sta/tacos/statemachine.hpp>
#include <sta/tacos/startup.hpp>


// The UART mutex defined in freertos.c
extern osMutexId_t uartMutexHandle;


namespace sta
{
	// Here the printable used for debugging is defined.
	Printable * Debug;

	namespace tacos
	{
		void initPrintable()
		{
			// Initialize the mutex for UART communication.
			RtosMutex * mutex = new RtosMutex(&uartMutexHandle);

			// Initialize the UART interface and printable object.
			UARTSettings settings = { .mode =  UARTMode::RX_TX };
			STM32UART * intf_ptr = new STM32UART(&huart2, settings, mutex);
			Debug = new PrintableUART(intf_ptr);

			STA_DEBUG_PRINTLN("UART SUCCESSFULLY INITIALIZED");
		}


		STA_WEAK
		void onStatemachineInit()
		{}


		void initStatemachine()
		{
			onStatemachineInit();

			Statemachine::instance()->start();
		}


		STA_WEAK
		void onManagerInit()
		{}


		void initManager()
		{
			onManagerInit();

			Manager::instance()->start();
		}
	}  // namespace tacos


	namespace rtos
	{
		// Override the weak implementation of startupExtras provided in rtos2-utils.
		void startupExtras(void * argument)
		{
			tacos::initPrintable();

			tacos::initStatemachine();

			tacos::initManager();
		}
	} // namespace rtos
} // namespace sta