//
// WSPRHAB: Minimal high-altitude balloon tracker with WSPR telemetry
//

#include "stm32f0xx_hal.h"
#include "adc.h"
#include "system.h"
#include "i2c.h"
#include "uart.h"
#include "gpio.h"
#include "wspr.h"
#include "rtc.h"
#include "gps.h"
#include "config.h"


// We have access to the 1PPS pin of the gps... could have trim routine for internal oscillator based on this when we have a fix
// Probable wake up 1 minute early -- 0.45min possible +/- on wakeup time with 15min sync intervals


// TODO: Add JT9 message with more grid locator digits + altitude + vbatt + temp
// MSG13charmax:
// 	X: gridloc
//  Y: altitude
//  Z: temperature
//  KD8TDF XXYYZZ // could use alt callsign thing

enum _state
{
    SYSTEM_IDLE = 0, // awaiting RTC interrupt for wakeup TODO wake up before scheduled time to get fix?
    SYSTEM_GPSACQ, // RTC interrupted
    SYSTEM_WSPRTX, // Wait for timeslot and actually transmit the message
};

static void __calc_gridloc(char *dst, double lat, double lon);
static void ledpulse(void);

uint32_t statled_ontime = 0;


int main(void)
{
    HAL_Init();
    HAL_Delay(1000); // startup delay before infinisleep

    sysclk_init();
    gpio_init();
    rtc_init();
    adc_init();
    wspr_init();

    uint32_t led_timer = HAL_GetTick();

    led_blink(4);

    uint16_t blink_rate = BLINK_FAST;
    uint8_t state = SYSTEM_GPSACQ;

    // DEBUG EMZ FIXME
    state = SYSTEM_IDLE;

    uint32_t gps_polltimer = 0;
    uint32_t fix_acq_starttime = 0;
    uint32_t nextwspr_time = 0;
    uint8_t nextwspr_time_valid = 0;
    uint32_t last_wspr_tx_time = 0;
    uint64_t idle_blink_last = 0;

    uint8_t packet_type = 0;

    // Transmit pilot tone to test TX on bootup
    HAL_Delay(1000);
    wspr_pilot_tone();
    adc_stop();
    HAL_Delay(1000);

//    __DBGMCU_CLK_ENABLE() ; // (RCC->APB2ENR |= (RCC_APB2ENR_DBGMCUEN))
//    HAL_EnableDBGStopMode();  //  SET_BIT(DBGMCU->CR, DBGMCU_CR_DBG_STOP);


    while (1)
    {
    	// Every 10 minutes, wake up and try to wspr
    	if(state == SYSTEM_IDLE && (HAL_GetTick() - last_wspr_tx_time > 60000 * 10))
    	{
    		state = SYSTEM_GPSACQ;
    	}

        // Update fix status every 2 seconds, only if the GPS is powered on
        if(HAL_GetTick() - gps_polltimer > 2000)
        {
            if(gps_ison())
            {
            	gps_update_data();

            	// If odd minute
            	if(gps_getdata()->minute % 2)
            	{
            		// Wait until even minute plus one second, coming soon
            		nextwspr_time = HAL_GetTick() + (60000 - (gps_getdata()->second * 1000));
                    nextwspr_time_valid = 1;

            	}
            	// If even minute
            	else
            	{
            		// Wait until odd minute, one minute and some change away
            		nextwspr_time = HAL_GetTick() + 60000 + (60000 - (gps_getdata()->second * 1000));
                    nextwspr_time_valid = 1;
            	}
            }
            gps_polltimer = HAL_GetTick();
        }



        switch(state)
        {

            // Idling: sleep and wait for RTC timeslot trigger
            case SYSTEM_IDLE:
            {
            	// Don't blink with the usual method
                blink_rate = BLINK_DISABLE;

                // If we haven't blinked for a while, blink now
                if(rtc_timestamp() - idle_blink_last > 10 * 1000)
                {
                	HAL_GPIO_WritePin(LED_BLUE, 1);
                	HAL_Delay(20);
                	HAL_GPIO_WritePin(LED_BLUE, 0);
                	idle_blink_last = rtc_timestamp();
                }


                // Enter STOP mode for one second using RTC for wakeup
                // 1s might not be accurate because we're entering STOP
                // mode any time between 1s interrupt ticks...

                // TODO: Calculate full RTC timestamp before entering
                // and after entering, find delta, this is the amount
                // we use to increment SYSTICK


                // Enter sleep mode: wait for interrupt
                //HAL_PWR_EnterSLEEPMode(0, PWR_SLEEPENTRY_WFI);

                uint64_t start = rtc_timestamp();

                __HAL_PWR_CLEAR_FLAG(PWR_FLAG_WU);
                HAL_SuspendTick();

        		HAL_PWR_EnterSTOPMode(PWR_LOWPOWERREGULATOR_ON, PWR_STOPENTRY_WFI);

        		// We probably stopped for a second
        		uint32_t timedelta = rtc_timestamp() - start;
        		HAL_IncTickBy(timedelta);
        		//HAL_IncTickBy(1000); // maybe check the RTC before and after this, increment tick by the delta?
                HAL_ResumeTick();

                // We have woken up! Clear wakeup flag
        		__HAL_PWR_CLEAR_FLAG(PWR_FLAG_WU);

                // This is hopefully the only timer that needs to stay alive in idle mode
 //               last_wspr_tx_time += 0; // move this timer forward based on sleep length

 //               HAL_ResumeTick();

                // TODO: Eventually use GPS time to calibrate the RTC maybe

            } break;


            // Attempt to acquire GPS fix
            case SYSTEM_GPSACQ:
            {
                blink_rate = BLINK_FAST;
                HAL_PWR_EnterSLEEPMode(0, PWR_SLEEPENTRY_WFI);
                HAL_PWR_EnterSLEEPMode(0, PWR_SLEEPENTRY_WFI);
                HAL_PWR_EnterSLEEPMode(0, PWR_SLEEPENTRY_WFI);
                HAL_PWR_EnterSLEEPMode(0, PWR_SLEEPENTRY_WFI);


                if(!gps_ison())
                {
                	fix_acq_starttime = HAL_GetTick();
                    gps_poweron(); // power on and initialize GPS module
                }

                // TODO: Move GPS processing into here from above!


                // If 3d fix with a decent enough precision
                if( ((gps_getdata()->fixtype == 2) || (gps_getdata()->fixtype == 3)) && gps_getdata()->pdop < 10 && nextwspr_time_valid == 1)
                {
                    // Disable GPS module
                    gps_poweroff();

                    // TODO: Set RTC from GPS time

                    // TODO: Set RTC for countdown to next transmission timeslot!

                    // TODO: Set wspr countdown timer for this transmission!
                    fix_acq_starttime = 0;
                    state = SYSTEM_WSPRTX;
                    adc_start();
                }
                // If no decent fix in 3 minutes
                else if(HAL_GetTick() - fix_acq_starttime > 60000 * 3)
                {
                	// Flash error code and go to idle, try again next time
                	led_blink(4);
                    gps_poweroff();
                    fix_acq_starttime = 0;
                    last_wspr_tx_time = HAL_GetTick(); // repeat acq/tx cycle after big time delay
                	state = SYSTEM_IDLE;
                }
            } break;


            // Wait for wspr timeslot and start transmitting
            case SYSTEM_WSPRTX:
            {
            	blink_rate = BLINK_MED;
                // Wait for wspr countdown timer to expire and go to tx
//                if(timeout_expired)
//                {

            	// If we're after the minute but not more than 2s after the minute, start tx
            	if(HAL_GetTick() >= nextwspr_time)
            	{
            		if(HAL_GetTick() < nextwspr_time + 2000)
            		{
            			volatile double latitude_flt = (double)gps_getdata()->latitude / 10000000.0;
            			volatile double longitude_flt = (double)gps_getdata()->longitude / 10000000.0;
            			volatile uint8_t grid_locator[7];

            			__calc_gridloc(grid_locator, latitude_flt, longitude_flt);

                        // TODO: Switch between alternate and standard packet
						wspr_transmit(grid_locator, packet_type);
                        packet_type = !packet_type; // alternate packet type
						last_wspr_tx_time = HAL_GetTick();
						state = SYSTEM_IDLE;
                        adc_stop();
            		}
            		else
            		{
            			// Window was missed, go back to idle, and try again after time delay
						last_wspr_tx_time = HAL_GetTick();
            			state = SYSTEM_IDLE;
                        adc_stop();
            		}
                    nextwspr_time_valid = 0; // invalidate wspr time
                }
            	else
            	{
                    HAL_PWR_EnterSLEEPMode(0, PWR_SLEEPENTRY_WFI);
                    HAL_PWR_EnterSLEEPMode(0, PWR_SLEEPENTRY_WFI);
                    HAL_PWR_EnterSLEEPMode(0, PWR_SLEEPENTRY_WFI);
                    HAL_PWR_EnterSLEEPMode(0, PWR_SLEEPENTRY_WFI);
            	}

                // Schedule next wakeup (maybe 2mins prior to timeslot if no osc trim)
                // Next wakeup should enter SYSTEM_GPSACQ state...

            } break;

        }

		#ifndef LED_DISABLE
			if((blink_rate != BLINK_DISABLE) && (HAL_GetTick() - led_timer > blink_rate))
			{
				ledpulse();
				led_timer = HAL_GetTick();
			}

			if((blink_rate != BLINK_DISABLE) && (statled_ontime && HAL_GetTick() - statled_ontime > 10))
			{
				HAL_GPIO_WritePin(LED_BLUE, 0);
				statled_ontime = 0;
			}
		#endif

    }
}



static void ledpulse(void)
{
    HAL_GPIO_WritePin(LED_BLUE, 1);
	statled_ontime = HAL_GetTick();
}

static void __calc_gridloc(char *dst, double lat, double lon)
{
	int o1, o2, o3;
	int a1, a2, a3;
	double remainder;
	// longitude
	remainder = lon + 180.0;
	o1 = (int)(remainder / 20.0);
	remainder = remainder - (double)o1 * 20.0;
	o2 = (int)(remainder / 2.0);
	remainder = remainder - 2.0 * (double)o2;
	o3 = (int)(12.0 * remainder);

	// latitude
	remainder = lat + 90.0;
	a1 = (int)(remainder / 10.0);
	remainder = remainder - (double)a1 * 10.0;
	a2 = (int)(remainder);
	remainder = remainder - (double)a2;
	a3 = (int)(24.0 * remainder);
	dst[0] = (char)o1 + 'A';
	dst[1] = (char)a1 + 'A';
	dst[2] = (char)o2 + '0';
	dst[3] = (char)a2 + '0';
	dst[4] = (char)o3 + 'A';
	dst[5] = (char)a3 + 'A';
	dst[6] = (char)0;
}