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

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

void rf_get_speed(v2d *speed)
{
    chSysLock();
    *speed = rf_speed;
    chSysUnlock();
}

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 x_packets[2] = {-1, -1};
    int y_packets[2] = {-1, -1};
    unsigned stay_on_packets = 0;
    
    rf_speed.x = rf_speed.y = 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.x = rf_speed.y = 0;
            last_valid_packet = 0;
            x_packets[0] = x_packets[1] = y_packets[0] = y_packets[1] = -1;
            
            v2d speed = {0,0};
            control_set_target(&speed);
        }
        
        // 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.
        
        int result = chIOGetTimeout(&SD2, MS2ST(50));
        int flags = chIOGetAndClearFlags(&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)
        {
            // X axis
            if (result & 0x40)
                x_packets[1] = (result & 0x3F) ^ 0x15;
            else
                x_packets[0] = (result & 0x3F) ^ 0x0F;
        }
        else
        {
            // Y axis
            if (result & 0x40)
                y_packets[1] = (result & 0x3F) ^ 0x15;
            else
                y_packets[0] = (result & 0x3F) ^ 0x0F;
        }
        
        if (x_packets[0] == x_packets[1] && x_packets[0] != -1 &&
            y_packets[0] == y_packets[1] && y_packets[0] != -1)
        {
            // Valid packets received
            if (rf_packets - stay_on_packets > 50)
            {
                // Minimum of 50 packets per keepalive,
                // so that a single spurious
                // packet won't keep us up for ever.
                powersave_stay_on();
                stay_on_packets = rf_packets;
            }
            
            int x = x_packets[0];
            int y = y_packets[0];
            
            if (x & 0x20) x |= 0xFFFFFFC0;
            if (y & 0x20) y |= 0xFFFFFFC0;
            
            v2d speed = {
                fix16_div(x, 32),
                fix16_div(y, 32)
            };
            
            chSysLock();
            rf_speed = speed;
            rf_packets++;
            last_valid_packet = chTimeNow();
            chSysUnlock();
            
            v2d_mul_s(&speed, &speed, params.max_speed);
            control_set_target(&speed);
            
            x_packets[0] = x_packets[1] = y_packets[0] = y_packets[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);
    
    pwmDisableChannel(&PWMD2, 1);
    pwmDisableChannel(&PWMD2, 2);
    pwmStop(&PWMD2);
    
    chThdWait(rfThread);
}