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

static qf16 smoothed_orientation;

void orientation_update()
{
    qf16 new;
    ahrs_get_orientation(&new);
    
    qf16_avg(&new, &smoothed_orientation, &new, F16(0.95));
    
    chSysLock();
    smoothed_orientation = new;
    chSysUnlock();
}

volatile unsigned rf_packets;

void rf_get_speed(v2d *speed)
{
    v3d vector_up = {0, 0, F16(1)};
    
    chSysLock();
    qf16 rot = smoothed_orientation;
    chSysUnlock();
    
    qf16_rotate(&vector_up, &rot, &vector_up);
    
    speed->x = vector_up.x;
    speed->y = vector_up.y;
}

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(const v2d *speed)
{
    int east = fix16_mul(speed->x, 32);
    int north = fix16_mul(speed->y, 32);
    
    if (east > 31) east = 31;
    if (east < -32) east = -32;
    
    if (north > 31) north = 31;
    if (north < -32) north = -32;
    
    // Packet format:
    // Each byte is a separate error-correction chunk.
    // Bit 7: 0 for EAST/X, 1 for NORTH/Y
    // 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 | ((int8_t)(east ^ 0x0F) & 0x3F),
        0x00 | ((int8_t)(north ^ 0x0F) & 0x3F),
        0xC0 | ((int8_t)(east ^ 0x15) & 0x3F),
        0x40 | ((int8_t)(north ^ 0x15) & 0x3F)
    };
    sdWrite(&SD2, txbuf, 4);
    sdWrite(&SD2, txbuf, 4);
    rf_packets++;
    
    // Wait for flush
    while (!chOQIsEmptyI(&SD2.oqueue))
        chThdSleepMilliseconds(5);
}

static msg_t rf_task(void *arg)
{
    chRegSetThreadName("RF");
    
    while (!chThdShouldTerminate())
    {
        if (palReadPad(GPIOA, GPIOA_WKUP))
        {
            palSetPad(GPIOB, GPIOB_RF_PWR);
            
            powersave_stay_on();
            
            v2d speed;
            rf_get_speed(&speed);
            transmit_packet(&speed);
        }
        else
        {
            // Send stop packets intermittenly
            if (!palReadPad(GPIOB, GPIOB_RF_PWR))
            {
                palSetPad(GPIOB, GPIOB_RF_PWR);
                chThdSleepMilliseconds(10);
            }
            
            v2d speed = {0,0};
            transmit_packet(&speed);
            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) || chThdShouldTerminate())
                    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);
}