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

#include "start.hpp"
#include "GPIO_Pin_STM32.hpp"
#include "SPI_Peripheral_STM32.hpp"
#include "BinLeds.hpp"
#include "SSL_Comms.hpp"
#include "SX1280Lib/sx1280-hal.h"

#include <cstdio>

extern SPI_HandleTypeDef hspi1;

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);
BinLeds discoveryLeds((GPIO_Pin*[]){&ledOrange, &ledGreen, &ledRed, &ledBlue}, 4);

SPI_Peripheral_STM32 spiRadio(&hspi1, 1000);
GPIO_Pin_STM32 sx1280_a_nss(SX1280_A_NSS_GPIO_Port, SX1280_A_NSS_Pin);
GPIO_Pin_STM32 sx1280_a_rxen(SX1280_A_RXEN_GPIO_Port, SX1280_A_RXEN_Pin);
GPIO_Pin_STM32 sx1280_a_txen(SX1280_A_TXEN_GPIO_Port, SX1280_A_TXEN_Pin);
GPIO_Pin_STM32 sx1280_a_rst(SX1280_A_RST_GPIO_Port, SX1280_A_RST_Pin);
GPIO_Pin_STM32 sx1280_a_busy(SX1280_A_BUSY_GPIO_Port, SX1280_A_BUSY_Pin);
GPIO_Pin_STM32 sx1280_a_irq(SX1280_A_IRQ_GPIO_Port, SX1280_A_IRQ_Pin);
SX1280Hal pRadio(&spiRadio, &sx1280_a_nss, &sx1280_a_busy, &sx1280_a_irq, &sx1280_a_rst, nullptr);
GPIO_Pin_STM32 sx1280_b_nss(SX1280_B_NSS_GPIO_Port, SX1280_B_NSS_Pin);
GPIO_Pin_STM32 sx1280_b_rxen(SX1280_B_RXEN_GPIO_Port, SX1280_B_RXEN_Pin);
GPIO_Pin_STM32 sx1280_b_txen(SX1280_B_TXEN_GPIO_Port, SX1280_B_TXEN_Pin);
GPIO_Pin_STM32 sx1280_b_rst(SX1280_B_RST_GPIO_Port, SX1280_B_RST_Pin);
GPIO_Pin_STM32 sx1280_b_busy(SX1280_B_BUSY_GPIO_Port, SX1280_B_BUSY_Pin);
GPIO_Pin_STM32 sx1280_b_irq(SX1280_B_IRQ_GPIO_Port, SX1280_B_IRQ_Pin);
SX1280Hal pRadio2(&spiRadio, &sx1280_b_nss, &sx1280_b_busy, &sx1280_b_irq, &sx1280_b_rst, nullptr);

GPIO_Pin_STM32 blueButton(B1_GPIO_Port, B1_Pin);
SSL_Comms radio0(&blueButton);

void start(){
	discoveryLeds.set(15);
	spiRadio.init();
	radio0.init();
	pRadio.Init();
	RadioRegulatorModes_t regulatorMode = USE_LDO;
	pRadio.SetRegulatorMode(regulatorMode);
	RadioStandbyModes_t standbyMode = STDBY_XOSC;
	pRadio.SetStandby(standbyMode);
	uint16_t firmwareVersion = pRadio.GetFirmwareVersion();
	printf("Radio firmware version: %u\n", firmwareVersion);
	TickTime_t timeout = {RADIO_TICK_SIZE_0015_US, 0xFFFF};
	pRadio.SetRx(timeout);
	int8_t rssiInst = pRadio.GetRssiInst();
	printf("Radio RSSI: %d\n", rssiInst);
	RadioStatus_t status = pRadio.GetStatus();
	printf("CMD status: 0x%X, Chip mode: 0x%X\n", status.Fields.CmdStatus, status.Fields.ChipMode);
	pRadio2.Init();
	RadioRegulatorModes_t regulatorMode2 = USE_LDO;
	pRadio2.SetRegulatorMode(regulatorMode2);
	RadioStandbyModes_t standbyMode2 = STDBY_XOSC;
	pRadio2.SetStandby(standbyMode2);
	uint16_t firmwareVersion2 = pRadio2.GetFirmwareVersion();
	printf("Radio2 firmware version: %u\n", firmwareVersion2);
	int8_t rssiInst2 = pRadio2.GetRssiInst();
	printf("Radio2 RSSI: %d\n", rssiInst2);
	RadioStatus_t status2 = pRadio2.GetStatus();
	printf("CMD2 status: 0x%X, Chip2 mode: 0x%X\n", status2.Fields.CmdStatus, status2.Fields.ChipMode);
}