#include <Arduino.h>
#include <Adafruit_ST7735.h>
#include <TinyGPSPlus.h>
#include <logger.h>
#include "board.h"
#include "sx1262.h"

TinyGPSPlus gps;
Adafruit_ST7735 tft = Adafruit_ST7735(TFT_CS,  TFT_DC, TFT_MOSI, TFT_SCLK, TFT_RST);
logging::Logger logger;
Sx1262 radio = Sx1262(SX1262_RST, SX1262_SCK, SX1262_MOSI, SX1262_MISO, SX1262_NSS ,SX1262_DIO1, SX1262_BUSY);

void setup_radio()
{
    tft.setTextColor(ST77XX_GREEN, ST77XX_BLACK);
    tft.setCursor(0, 12);
    tft.print("[Sx1262] ");
    tft.setTextColor(ST77XX_WHITE, ST77XX_BLACK);
    delay(1500);

    /*
    C'est le bout trouvé dans le code kikiwi
    pour la config
    ==============
        SX1276FskSetGMSK(GAUSSIAN_FILTER_0_5);	// FSK modulation shaping
		SX1276FskBaudRate(BAUDRATE_1200);		// Data baudrate
		SX1276FskSetRFPower(14);
		SX1276FskSetPayloadLength( DATA_FRAME_SIZE );
		SX1276FskPacketFormat(RF_PACKETCONFIG1_PACKETFORMAT_FIXED);
		SX1276FskSetPacketCrcOn( true );
    pour le Tx
    ==========
    	SX1276FskSetOpMode(RF_OPMODE_STANDBY);
        SX1276WriteFIFO(RadioFrame, DATA_FRAME_SIZE);
        SX1276FskSetOpMode(RF_OPMODE_TRANSMITTER);	
        SYS_Delay(500); /// Validé 500
        SX1276FskSetOpMode(RF_OPMODE_STANDBY);
    */

    radio.beginSPI();
    radio.reset();
    //radio.readStatusCommand(SX126X_GET_STATUS);
    //radio.SetSleep();
    radio.SetStandby();
    radio.SetPacketType(SX126X_PACKET_TYPE_LORA);
    logger.log(logging::LoggerLevel::LOGGER_LEVEL_INFO, "SX1262", "Setup done");
    radio.GetPacketType();
    radio.SetRfFrequency(868000000);
    /*
     * Table 13-21: PA Operating Modes with Optimal Settings
     */
    radio.SetPaConfig(0x02, 0x02, 0x00, 0x01);

    /*
     * Table 13-40: SetTxParams SPI Transaction
     */
    radio.SetTxParams(0x0A, 0x09);

    /*
     * Table 13-74: SetBufferBaseAddress SPI Transaction
     * De toute façon le bousin ne fera que du Tx
     */
    radio.SetBufferBaseAddress(0x00, 0x00);

    /*
     * Table 13-42: Set Modulation Prameters in GFSK Mode
     */
    radio.SetModulationParams(1200, 0x09, 0x16, 0x10);

    /*
     * Table 13-36: SetRfFrequency SPI Transaction
main/sx1276-Fsk.c:146:    SX1276->RegSyncConfig = RF_SYNCCONFIG_AUTORESTARTRXMODE_WAITPLL_ON | RF_SYNCCONFIG_PREAMBLEPOLARITY_AA |
main/sx1276-Fsk.c:147:                            RF_SYNCCONFIG_SYNC_ON |
main/sx1276-Fsk.c:148:                            RF_SYNCCONFIG_SYNCSIZE_4;
main/sx1276-Fsk.c:150:    SX1276->RegSyncValue1 = 0x69;
main/sx1276-Fsk.c:151:    SX1276->RegSyncValue2 = 0x81;
main/sx1276-Fsk.c:152:    SX1276->RegSyncValue3 = 0x7E;
main/sx1276-Fsk.c:153:    SX1276->RegSyncValue4 = 0x96;
     */

    radio.SetPacketParams(0x100, 0x05, 4, 0x00, 0x00, 50, 1);
    radio.SetDIO3AsTCXOCtrl(0x00, 0x140);
    // on écrit les valeurs de sync_words voulu, comme dans kikiwi
    radio.regWrite(0x06C0, 0x69);
    radio.regWrite(0x06C1, 0x81);
    radio.regWrite(0x06C2, 0x7E);
    radio.regWrite(0x06C3, 0x96);

}

void setup_gps()
{    
    Serial1.begin(115200, SERIAL_8N1, GPS_RX, GPS_TX);
    tft.setCursor(0, 0);
    tft.setTextColor(ST77XX_GREEN, ST77XX_BLACK);
    tft.print("[GPS] ");
    tft.setTextColor(ST77XX_WHITE, ST77XX_BLACK);
    tft.print("Setup done.");
    logger.log(logging::LoggerLevel::LOGGER_LEVEL_INFO, "GPS", "Setup done");
}

void setup_tft()
{
    pinMode(TFT_CS, OUTPUT);
    pinMode(TFT_RST, OUTPUT);
    pinMode(TFT_CS, OUTPUT);

    tft.initR(INITR_MINI160x80_PLUGIN);
    tft.setRotation(1); // Landscape
    tft.fillScreen(ST77XX_BLACK);
    tft.setTextColor(ST77XX_WHITE, ST77XX_BLACK);
    tft.setTextSize(1.5);

    delay(50); // Wait for screen to clear

    // Enable backlight
    pinMode(TFT_LED, OUTPUT);
    digitalWrite(TFT_LED, HIGH);
}


void setup() 
{
    pinMode(LED_BUILTIN, OUTPUT);
    digitalWrite(LED_BUILTIN, HIGH);

    USBSerial.begin(115200);
    logger.setSerial(&USBSerial);

    // Enable power to peripherals
    pinMode(VEXT_CTL, OUTPUT);
    digitalWrite(VEXT_CTL, HIGH);
    
    setup_tft();
    setup_gps();
    setup_radio();
    
    digitalWrite(LED_BUILTIN, LOW);
    tft.fillScreen(ST77XX_BLACK);
    tft.fillCircle(150, 10, 2, ST77XX_WHITE);
}

void read_nmea(uint32_t ms)
{
    unsigned long start = millis();
    char c;
    logger.log(logging::LoggerLevel::LOGGER_LEVEL_INFO, "GPS", "Reading NMEA data :");
    do
    {

        while (Serial1.available()) {
            c = Serial1.read();
            gps.encode(c);
            USBSerial.write(c);
        }
    } while (millis() - start < ms);
}

void loop() 
{
    tft.setTextColor(ST7735_GREEN, ST77XX_BLACK);
    tft.setCursor(0, 0);
    tft.print("[GPS NMEA] ");
    tft.setTextColor(ST7735_WHITE, ST77XX_BLACK);

        
    if (gps.time.isUpdated())
    {
        tft.setCursor(0, 9);
        tft.printf("%04u-%02u-%02u %02u:%02u:%02u",
                   gps.date.year(), gps.date.month(), gps.date.day(),
                   gps.time.hour(), gps.time.minute(), gps.time.second());
    }

    if (gps.location.isUpdated())
    {
        tft.setCursor(0, 24);
        tft.printf("Lat: % 3.6f\nLon: % 3.6f\n", gps.location.lat(), gps.location.lng());
        logger.log(logging::LoggerLevel::LOGGER_LEVEL_INFO, "GPS", "Lat: % 3.6f, Lon: % 3.6f", gps.location.lat(), gps.location.lng());
    }

    if (gps.altitude.isUpdated())
    {
        tft.setCursor(0, 28);
        tft.printf("Alt: %3.0fft ", gps.altitude.feet());
        logger.log(logging::LoggerLevel::LOGGER_LEVEL_INFO, "GPS", "Alt: %3.0fft", gps.altitude.feet());
    }
    tft.fillCircle(150, 10, 2, ST77XX_GREEN);
    read_nmea(1000);
    tft.fillCircle(150, 10, 2, ST77XX_WHITE);
    
    

}