#include <ch.h>
#include <hal.h>
#include <string.h>
#include <stdio.h>
#include <fix16.h>
#include "rf_task.h"
#include "ahrs_task.h"
#include "powersave.h"

volatile unsigned rf_heading;
volatile unsigned rf_speed;
volatile unsigned rf_packets;

fix16_t RF_TILT_MIN = (fix16_t)(0.1 * 65536);
fix16_t RF_TILT_MAX = (fix16_t)(0.6 * 65536);

static const SerialConfig config = {
    3000,
    0,
    USART_CR2_STOP1_BITS | USART_CR2_LINEN,
    0
};

static Thread *rfThread = 0;
static WORKING_AREA(rfThread_wa, 512);

static void transmit_packet(int heading, int speed)
{
    rf_heading = heading;
    rf_speed = speed;
    
    // Packet format:
    // Each byte is a separate error-correction chunk.
    // Bit 7: 0 for speed, 1 for heading
    // Bit 6: 0 for 0x0F xor, 1 for 0x15 xor (avoiding DC bias)
    // Bits 5..0: data
    // Atleast 2 chunks of each value have to be received => transmit
    // 4 of each.
    uint8_t txbuf[4] = {
        0x80 | (heading ^ 0x0F),
        0x00 | (speed ^ 0x0F),
        0xC0 | (heading ^ 0x15),
        0x40 | (speed ^ 0x15)
    };
    sdWrite(&SD2, txbuf, 4);
    sdWrite(&SD2, txbuf, 4);
    rf_packets++;
    
    // Wait for flush
    while (!chOQIsEmptyI(&SD2.oqueue))
        chThdSleepMilliseconds(10);
}

static msg_t rf_task(void *arg)
{
    rf_speed = rf_heading = 0;
    
    chRegSetThreadName("RF");
    
    while (!chThdShouldTerminate())
    {
        if (palReadPad(GPIOA, GPIOA_WKUP))
        {
            palSetPad(GPIOB, GPIOB_RF_PWR);
            
            powersave_stay_on();
            
            chSysLock();
            fix16_t north = ahrs_north;
            fix16_t east = ahrs_east;
            chSysUnlock();
            
            fix16_t speed = fix16_sqrt(fix16_mul(north, north) + fix16_mul(east, east));
            fix16_t heading = fix16_atan2(east, north);
            
            speed -= RF_TILT_MIN; // Threshold for tilt before applying speed
            speed = fix16_div(speed, RF_TILT_MAX - RF_TILT_MIN); // Speed range
            speed = fix16_mul(speed, 64); // Speed range to transmit
            if (speed < 1) speed = 1;
            if (speed > 63) speed = 63;
            
            if (speed == 1)
            {
                // Don't update heading until tilt is large enough
                // to determine it precisely.
                heading = rf_heading;
            }
            else
            {
                heading = fix16_to_int(fix16_div(heading * 32, fix16_pi));
                if (heading < 0) heading += 64;
                if (heading > 63) heading = 63;
            }
            
            transmit_packet(heading, speed);
        }
        else
        {
            // Send stop packets intermittenly
            if (!palReadPad(GPIOB, GPIOB_RF_PWR))
            {
                palSetPad(GPIOB, GPIOB_RF_PWR);
                chThdSleepMilliseconds(10);
            }
            
            transmit_packet(rf_heading, 0);
            chThdSleepMilliseconds(10);
            palClearPad(GPIOB, GPIOB_RF_PWR);
            
            // Sleep 5 seconds unless button is pressed
            for (int i = 0; i < 50; i++)
            {
                if (palReadPad(GPIOA, GPIOA_WKUP))
                    break;
                chThdSleepMilliseconds(100);
            }
        }
    }
    
    return 0;
}

void rf_start()
{
    sdStart(&SD2, &config);
    
    rf_packets = 0;
    rfThread = chThdCreateStatic(rfThread_wa, sizeof(rfThread_wa), NORMALPRIO + 20, rf_task, NULL);
}

void rf_stop()
{
    chThdTerminate(rfThread);
    sdStop(&SD2);
    chThdWait(rfThread);
    palClearPad(GPIOB, GPIOB_RF_PWR);
}