#include <ch.h>
#include <hal.h>
#include <stm32l1xx.h>
#include <stdbool.h>
#include "speedsensor.h"
#include "usb_usart.h"
#include <chprintf.h>
#include <stdlib.h>

fix16_t speedsensor_rad_per_sec;

volatile unsigned speedsensor_odometer;
volatile int speedsensor_steps;
static int oldstate;

static int getstate() {
    return palReadGroup(GPIOA, 0x3, GPIOA_ENCODER_IN1);
}

static void callback(EXTDriver *extp, expchannel_t channel) {
    int newstate = getstate();
    
    switch (newstate | (oldstate << 4))
    {
        case 0x31: // 11 -> 01
        case 0x10: // 01 -> 00
        case 0x02: // 00 -> 10
        case 0x23: // 10 -> 11
            speedsensor_steps++;
            break;
        
        case 0x32: // 11 -> 10
        case 0x20: // 10 -> 00
        case 0x01: // 00 -> 01
        case 0x13: // 01 -> 11
            speedsensor_steps--;
            break;
    }
    
    oldstate = newstate;
}

static const EXTConfig extconfig = {
    {[GPIOA_ENCODER_IN1] = {EXT_CH_MODE_BOTH_EDGES | EXT_CH_MODE_AUTOSTART, callback},
     [GPIOA_ENCODER_IN2] = {EXT_CH_MODE_BOTH_EDGES | EXT_CH_MODE_AUTOSTART, callback}},
    EXT_MODE_EXTI(0, 0, 0, 0,    0, 0, EXT_MODE_GPIOA, EXT_MODE_GPIOA,
                  0, 0, 0, 0,    0, 0, 0, 0)
};

void speedsensor_start()
{
    palSetPad(GPIOB, GPIOB_ENCODER_VCC);
    
    // Unfortunately, TIM3 is already used for servo.
    // Therefore we do the step counting through interrupts.
    extStart(&EXTD1, &extconfig);
}

void speedsensor_stop()
{
    extStop(&EXTD1);
    palClearPad(GPIOB, GPIOB_ENCODER_VCC);
}

// Speed calculation is done over a 500 ms sliding window.
// This function should be called every 100 ms.
void speedsensor_update()
{
    static int fifo[5];

    // Rotate the fifo
    int old = fifo[0];
    int new = speedsensor_steps;
    for (int i = 0; i < 4; i++)
        fifo[i] = fifo[i + 1];
    fifo[4] = new;
    
    speedsensor_odometer += abs(fifo[4] - fifo[3]);
    
    // Update speed estimate
    int steps_in_500ms = new - old;
    fix16_t radians = steps_in_500ms * F16(0.2618); // 15 deg per step
    speedsensor_rad_per_sec = radians * 2;
    
    //chprintf(&SDU1, "%6d  Steps %d   %d\r\n", chTimeNow(), speedsensor_steps, steps_in_500ms);
}