#include "stm32f0xx_hal.h"
#include "si5351.h"
#include "jtencode.h"
#include "gpio.h"
#include "wspr.h"
#include "i2c.h"
#include "gps.h"
#include "adc.h"
#include "config.h"


#define WSPR_DEFAULT_FREQ 10140100UL
#define WSPR_TONE_SPACING 146 // ~1.46 Hz
#define WSPR_CTC 10672 // CTC value for WSPR

char call_orig[7] = "KD8TDF";
#define DBM_ORIG 10


// Test stuff
char call[7] = "KD8TDF";
char loc[7] = "EN82";
uint8_t dbm = 10;
uint8_t tx_buffer[255];

// Frequencies and channel info
uint32_t freq = WSPR_DEFAULT_FREQ;
uint8_t symbol_count = WSPR_SYMBOL_COUNT;
uint16_t ctc = WSPR_CTC;
uint16_t tone_spacing = WSPR_TONE_SPACING;
volatile uint8_t proceed = 0;

TIM_HandleTypeDef htim1;



void wspr_init(void)
{
    // Turn off ICs
    HAL_GPIO_WritePin(OSC_NOTEN, 1);
    HAL_GPIO_WritePin(TCXO_EN, 0);
}

// Do anything needed to prepare for sleep
void wspr_sleep(void)
{
    HAL_TIM_Base_Stop_IT(&htim1);
}

void wspr_wakeup(void)
{
    HAL_TIM_Base_Start_IT(&htim1);
}


// Bring up TCXO and oscillator IC
void wspr_transmit(uint8_t* grid_locator, uint8_t send_alternate)
{
	// Copy 4 digit grid locator to local buffer; null terminate
	for(uint8_t i=0; i<4; i++)
		loc[i] = grid_locator[i];
	loc[4] = '\0';

    // Set power to altitude in m / 150
    uint8_t altscaled = gps_getdata()->altitude / 150;
    if(altscaled > 60)
        altscaled = 60;


    // If alternate packet, send 0XFXXEN82XX
    if(send_alternate)
    {
        /////////////////////////////////////////////////
        // Composite altitude and sub-maidenhead locator
        ///////////////////////////////////////////////// 
        // Use untrimmed locator
        uint32_t maiden_ext = (grid_locator[4] - 'A') + ((grid_locator[5] - 'A') * 24); // 0-575
        uint32_t altitude_mod = gps_getdata()->altitude / 20;   

        // Ciel at 21,340 meters
        if(altitude_mod > 1067)
            altitude_mod = 1067; // Don't overflow into maidenhead!
    
        // Compose composite altitude (lsbs) with maidenhead locator (msbs)
        uint32_t subalt = (1068 * maiden_ext) + (gps_getdata()->altitude / 20);



        ////////////////////////////////////////////
        // Encode extended maidenhead and altitude
        ////////////////////////////////////////////

        // Static set first char: balloon ID (invalid call)
        call[0] = '0';

        // Split into chunks of valmax 36, 26, 26, 26

        // Mask off following 3 26valmax fields
        uint32_t chunk = subalt / 26 / 26 / 26; 

        // Encode to callsign
        if(chunk < 10)
            call[1] = '0' + chunk;
        else
            call[1] = chunk - 10 + 'A';


        // Static set ID
        call[2] = '3'; // balloon ID #4

        // Subtract off previous portion
        subalt -= (chunk * 26 * 26 * 26);

        // Mask off following 2 26bit values
        chunk = (subalt / 26 / 26);

        call[3] = 'A' + chunk;

        // Subtract off previous portion
        subalt -= (chunk * 26 * 26);

        // Mask off following 1 26bit values
        chunk = (subalt / 26);

        call[4] = 'A' + chunk;

        // Subtract off previous portion
        subalt -= (chunk * 26);

        // Remainder is the last call char
        call[5] = 'A' + subalt;
 

        ////////////////////////////////////////
        // Composite temp/batt/speed/gps 
        ////////////////////////////////////////

        // Encode value from -50C to 39C => 0-89. TODO: Bounds!
        uint32_t temp_enc = adc_get_dietemp() + 50;
        if(temp_enc > 89)
            temp_enc = 89;

        // Encode value from 0-39 with some scalar/offset/etc
        uint32_t batt_enc = adc_get_vbatt();  // Hopefully in decivolts
        if(batt_enc > 39)
            batt_enc = 39;

        // Encode speed in knots from 0-82 to 0-41
        uint32_t speed_enc = gps_getdata()->speed / 2;
        if(speed_enc > 41)
            speed_enc = 41;

        // Encode GPS status
        uint32_t gps_status = 0b0; // valid fix
        uint32_t gps_sats = 0b0;  // lats > 8


        // We always have a fix if we got to this point; and I think we zero out that we had a fix when turning the GPS off before entering this function
//        if(gps_getdata()->fixtype == 2 || gps_getdata()->fixtype == 3)
            gps_status = 0b1;

        if(gps_getdata()->sats_in_solution > 5)
            gps_sats = 0b1;

        uint32_t engdata = gps_sats + 2 * (gps_status + 2 * (speed_enc + 42 * (batt_enc + 40 * temp_enc)));

        ////////////////////////////////////////////
        // Encode temp/batt/speed/gps
        ////////////////////////////////////////////

        // Mask off fields
        chunk = engdata / 18 / 10 / 10 / 19;  

        // Encode to grid locator
        loc[0] = 'A' + chunk;

        // Subtract off previous portion 
        engdata -= (chunk * 18 * 10 * 10 * 19);  

        // Mask of fields
        chunk = engdata / 10 / 10 / 19;  

        // Encode to grid locator
        loc[1] = 'A' + chunk;

        // Subtract off previous portion
        engdata -= (chunk * 10 * 10 * 19);  

        // Mask off fields
        chunk = engdata / 10 / 19;  

        // Encode
        loc[2] = '0' + chunk;

        // Subtract
        engdata -= (chunk * 10 * 19);  

        // Mask off
        chunk = engdata / 19;  

        // Encode
        loc[3] = '0' + chunk;

        // Subtract
        engdata -= (chunk * 19);  

        // Mask off
        chunk = engdata;  

        // Encode
        uint8_t powers[] = {0, 3, 7, 10, 13, 17, 20, 23, 27, 30, 33, 37, 40, 43, 47, 50, 53, 57, 60};
        dbm = powers[chunk];


    }
    else
    {
        call[0] = call_orig[0];
        call[1] = call_orig[1];
        call[2] = call_orig[2];
        call[3] = call_orig[3];
        call[4] = call_orig[4];
        call[5] = call_orig[5];
        call[6] = call_orig[6];

        dbm = DBM_ORIG; 
    }

    // Start timer for WSPR
    __TIM1_CLK_ENABLE();
    htim1.Instance = TIM1;
    htim1.Init.Prescaler = 512 / 4; // FIXED gives 64us ticks from 2mhz clock // gives 64uS ticks from 8MHz ahbclk
    htim1.Init.CounterMode = TIM_COUNTERMODE_UP;
    htim1.Init.Period = ctc; // Count up to this value (how many 64uS ticks per symbol)
    htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
    htim1.Init.RepetitionCounter = 0;
    HAL_TIM_Base_Init(&htim1);
    HAL_TIM_Base_Start_IT(&htim1);
    HAL_NVIC_SetPriority(TIM1_BRK_UP_TRG_COM_IRQn, 0, 0);
    HAL_NVIC_EnableIRQ(TIM1_BRK_UP_TRG_COM_IRQn);



    // TODO: Bring up TCXO sooner! Gotta let it warm up or something

    HAL_GPIO_WritePin(OSC_NOTEN, 0);
    HAL_GPIO_WritePin(TCXO_EN, 1);
    HAL_Delay(100);

    // Bring up the chip
    i2c_init();
    si5351_init(i2c_get(), SI5351_CRYSTAL_LOAD_8PF, 0);
    si5351_set_correction(0);
    //si5351_set_pll(SI5351_PLL_FIXED, SI5351_PLLA);
    //si5351_set_ms_source(SI5351_CLK0, SI5351_PLLA);
    si5351_set_freq(WSPR_DEFAULT_FREQ * 100, 0, SI5351_CLK0);
//    si5351_drive_strength(SI5351_CLK0, SI5351_DRIVE_8MA); // Set for max power if desired (8ma max)
    si5351_drive_strength(SI5351_CLK0, SI5351_DRIVE_6MA); // Set for max power if desired (8ma max)
//    si5351_drive_strength(SI5351_CLK0, SI5351_DRIVE_2MA); // Set for max power if desired (8ma max)
    si5351_output_enable(SI5351_CLK0, 1);
    //si5351_pll_reset(SI5351_PLLA);

    // Make sure the other outputs of the SI5351 are disabled
    si5351_output_enable(SI5351_CLK1, 0); // Disable the clock initially
    si5351_output_enable(SI5351_CLK2, 0); // Disable the clock initially

    // disable clock powers
    si5351_set_clock_pwr(SI5351_CLK1, 0);
    si5351_set_clock_pwr(SI5351_CLK2, 0);


    // Encode message to transmit
    wspr_encode(call, loc, dbm, tx_buffer);

    // Key transmitter
    si5351_output_enable(SI5351_CLK0, 1);

    // Loop through and transmit symbols TODO: Do this from an ISR or ISR-triggered main loop function call (optimal)
    uint8_t i;
    for(i=0; i<symbol_count; i++)
    {
        uint32_t freq2 = (freq * 100) + (tx_buffer[i] * tone_spacing);
        si5351_set_freq(freq2, 0, SI5351_CLK0);
        HAL_GPIO_TogglePin(LED_BLUE);

        proceed = 0;
        while(!proceed);
    }

    // Disable transmitter
    si5351_output_enable(SI5351_CLK0, 0);

    HAL_GPIO_WritePin(OSC_NOTEN, 1);
    HAL_GPIO_WritePin(TCXO_EN, 0);

    i2c_deinit();

    // Disable timer
    HAL_NVIC_DisableIRQ(TIM1_BRK_UP_TRG_COM_IRQn);
    HAL_TIM_Base_Stop_IT(&htim1);
    HAL_TIM_Base_DeInit(&htim1);

    __TIM1_CLK_DISABLE();


}


// Transmit boot-up test tones to check tx
void wspr_pilot_tone(void)
{
	// Bring up TCXO
    HAL_GPIO_WritePin(OSC_NOTEN, 0);
    HAL_GPIO_WritePin(TCXO_EN, 1);
    HAL_Delay(100);

    // Bring up the chip
    i2c_init();
    si5351_init(i2c_get(), SI5351_CRYSTAL_LOAD_8PF, 0);
    si5351_set_correction(0);
    si5351_set_freq(WSPR_DEFAULT_FREQ * 100, 0, SI5351_CLK0);
    si5351_drive_strength(SI5351_CLK0, SI5351_DRIVE_6MA); // Set for max power if desired (8ma max)
    si5351_output_enable(SI5351_CLK0, 1);

    // Make sure the other outputs of the SI5351 are disabled
    si5351_output_enable(SI5351_CLK1, 0); // Disable the clock initially
    si5351_output_enable(SI5351_CLK2, 0); // Disable the clock initially

    // disable clock powers
    si5351_set_clock_pwr(SI5351_CLK1, 0);
    si5351_set_clock_pwr(SI5351_CLK2, 0);

    // Key transmitter
    si5351_output_enable(SI5351_CLK0, 1);

    uint8_t tone_table[7] = {50, 100, 70, 50, 0, 10, 0};

    // Boot-up pilot tones
    for(uint8_t i=0; i<7; i++)
    {
        uint32_t freq2 = (WSPR_DEFAULT_FREQ + 10*tone_table[i]) * 100;
        si5351_set_freq(freq2, 0, SI5351_CLK0);
		#ifndef LED_DISABLE
        	HAL_GPIO_TogglePin(LED_BLUE);
		#endif
        HAL_Delay(500);
    }


    // Disable transmitter
    si5351_output_enable(SI5351_CLK0, 0);

    HAL_GPIO_WritePin(OSC_NOTEN, 1);
    HAL_GPIO_WritePin(TCXO_EN, 0);

    i2c_deinit();

    // Make sure LED is off if we had an odd number of toggles above
	HAL_GPIO_WritePin(LED_BLUE, 0);

}