/*
 * Start.cpp
 *
 *  Created on: Feb 10, 2024
 *      Author: Gabriel
 */

#include "start.hpp"
#include "GPIO_Pin_STM32.hpp"
#include "SPI_Peripheral_STM32.hpp"
#include "PWM_Pin_STM32.hpp"
#include "BinLeds.hpp"
#include "ILI9341.hpp"
#include "SSL_Display.hpp"

extern SPI_HandleTypeDef hspi5;
extern TIM_HandleTypeDef htim10;

GPIO_Pin_STM32 ledOrange(LD3_GPIO_Port, LD3_Pin);
GPIO_Pin_STM32 ledGreen(LD4_GPIO_Port, LD4_Pin);
GPIO_Pin_STM32 ledRed(LD5_GPIO_Port, LD5_Pin);
GPIO_Pin_STM32 ledBlue(LD6_GPIO_Port, LD6_Pin);
//GPIO_Pin_STM32* discoveryLedArray[4] = {&ledOrange, &ledGreen, &ledRed, &ledBlue};
BinLeds discoveryLeds((GPIO_Pin*[]){&ledOrange, &ledGreen, &ledRed, &ledBlue}, 4);

SPI_Peripheral_STM32 spi_a(&hspi5, 1000);
GPIO_Pin_STM32 displayNss(DISPLAY_NSS_GPIO_Port, DISPLAY_NSS_Pin);
GPIO_Pin_STM32 displayReset(DISPLAY_RESET_GPIO_Port, DISPLAY_RESET_Pin);
GPIO_Pin_STM32 displayDcrs(DISPLAY_DCRS_GPIO_Port, DISPLAY_DCRS_Pin);
PWM_Pin_STM32 displayLed(&htim10, TIM_CHANNEL_1);
ILI9341 ili9341(&spi_a, &displayNss, &displayReset, &displayDcrs, &displayLed);
SSL_Display sslDisplay(&ili9341);

void start(){
	spi_a.init();
	sslDisplay.init();
	RobotStatus status;
	for(uint32_t i=0; i<12; i++){
		status.batteryLevel = ((float)i)/11;
		status.connected = i%2;
		status.robotId = i;
		status.status = i%2;
		status.team = i%2;
		sslDisplay.update(status);
	}
	for(uint32_t i=0; i<16; i++){
		discoveryLeds.set(i);
		vTaskDelay(1000);
	}
}