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

volatile unsigned rf_heading;
volatile unsigned rf_speed;
volatile unsigned rf_packets;
volatile unsigned rf_dropped_flags;
int32_t rf_max_delay_ms = 200; // Max time between packets

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 msg_t rf_task(void *arg)
{
    int headings[2] = {-1, -1};
    int speeds[2] = {-1, -1};
    rf_speed = rf_heading = 0;
    systime_t last_valid_packet = 0;
    
    chRegSetThreadName("RF");
    
    while (!chThdShouldTerminate())
    {
        if (last_valid_packet != 0 &&
            chTimeNow() - last_valid_packet > MS2ST(rf_max_delay_ms))
        {
            rf_speed = 0;
            last_valid_packet = 0;
            speeds[0] = speeds[1] = headings[0] = headings[1] = -1;
        }
        
        // 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.
        
        int result = chnGetTimeout(&SD2, MS2ST(50));
        int flags = chnGetAndClearFlags(&SD2);
        
        if (result < 0 ||
            (flags & (SD_FRAMING_ERROR | SD_NOISE_ERROR | SD_BREAK_DETECTED)))
        {
            // Some error happened, discard chunk
            if (result >= 0)
                rf_dropped_flags++;
            
            continue;
        }
        
        // Valid chunk
        if (result & 0x80)
        {
            // Heading
            if (result & 0x40)
                headings[1] = (result & 0x3F) ^ 0x15;
            else
                headings[0] = (result & 0x3F) ^ 0x0F;
        }
        else
        {
            // Speed
            if (result & 0x40)
                speeds[1] = (result & 0x3F) ^ 0x15;
            else
                speeds[0] = (result & 0x3F) ^ 0x0F;
        }
        
        if (speeds[0] == speeds[1] && speeds[0] != -1 &&
            headings[0] == headings[1] && headings[0] != -1)
        {
            chSysLock();
            rf_speed = speeds[0];
            rf_heading = headings[0];
            rf_packets++;
            last_valid_packet = chTimeNow();
            chSysUnlock();
            speeds[0] = speeds[1] = headings[0] = headings[1] = -1;
        }
    }
    
    return 0;
}

// PWM signal for capacitor pump
const PWMConfig pwmconfig_tim2 = {
    1000000,
    2,
    NULL,
    {{0,NULL}, {1,NULL}, {1,NULL}}
};

void rf_start()
{
    pwmStart(&PWMD2, &pwmconfig_tim2);
    pwmEnableChannel(&PWMD2, 1, 1);
    pwmEnableChannel(&PWMD2, 2, 1);
    
    chThdSleepMilliseconds(100);
    
    sdStart(&SD2, &config);
    palSetPad(GPIOC, GPIOC_RF_PWR);
    
    rf_packets = rf_dropped_flags = 0;
    rfThread = chThdCreateStatic(rfThread_wa, sizeof(rfThread_wa), NORMALPRIO + 20, rf_task, NULL);
}

void rf_stop()
{
    chThdTerminate(rfThread);
    palClearPad(GPIOC, GPIOC_RF_PWR);
    sdStop(&SD2);
    chThdWait(rfThread);
}