#include <ch.h>
#include <hal.h>
#include <stdio.h>
#include <ff.h>
#include "gprs.h"
#include "log.h"
#include "rtc.h"
#include "rpc.h"
#include "kalman.h"

const SerialConfig debugPortConfig = {
    115200,
    0,
    USART_CR2_STOP1_BITS | USART_CR2_LINEN,
    0
};

const SerialConfig gprsPortConfig = {
    115200,
    0,
    USART_CR2_STOP1_BITS | USART_CR2_LINEN,
    USART_CR3_CTSE
};

const ADCConfig adcConfig = {};

const ADCConversionGroup adcGroup = {
    .acg_num_channels = 1,
    .acg_cr1 = 0,
    .acg_cr2 = 0,
    .acg_smpr1 = 0,
    .acg_smpr2 = 7,
    .acg_sqr1 = 1 << 20,
    .acg_sqr3 = 8
};

const SPIConfig spiConfig = {
    .spc_cr1 = (7 << 3) | SPI_CR1_MSTR | SPI_CR1_CPOL | SPI_CR1_CPHA
};

MMCDriver MMCD1;

const SPIConfig mmc_lowspeed = {
    .spc_ssport = GPIOA,
    .spc_sspad = 4,
    .spc_cr1 = (7 << 3) | SPI_CR1_MSTR
};

const SPIConfig mmc_highspeed = {
    .spc_ssport = GPIOA,
    .spc_sspad = 4,
    .spc_cr1 = (1 << 3) | SPI_CR1_MSTR
};

static bool_t mmc_is_inserted() {
    // Check pull-up resistor on CS line
    // TODO: Install external >100kohm pulldown
    bool status = !(palReadLatch(GPIOA) & PAL_PORT_BIT(4)) || palReadPad(GPIOA, 4);
    return status;
}

static bool_t mmc_is_protected() {
    return false;
}

FATFS filesystem;

uint8_t lis302_read_reg(int reg)
{
    uint8_t txbuf[2] = {0x80 | reg, 0xFF};
    uint8_t rxbuf[2] = {};
    
    palClearPad(GPIOD, 2);
    spiExchange(&SPID3, 2, txbuf, rxbuf);
    palSetPad(GPIOD, 2);
    return rxbuf[1];
}

void lis302_write_reg(int reg, uint8_t value)
{
    uint8_t txbuf[2] = {reg, value};
    
    palClearPad(GPIOD, 2);
    spiSend(&SPID3, 2, txbuf);
    palSetPad(GPIOD, 2);
}

void hm55b_start()
{
    uint32_t orig = SPI3->CR1;
    SPI3->CR1 = orig & (~SPI_CR1_CPOL);
    
    // Start measurement
    uint8_t txbuf[1] = {0x80};
    palClearPad(GPIOB, 2);
    spiSend(&SPID3, 1, txbuf);
    palSetPad(GPIOB, 2);
    
    SPI3->CR1 = orig;
}

bool hm55b_read(int *x, int *y)
{
    uint32_t orig = SPI3->CR1;
    SPI3->CR1 = orig & (~SPI_CR1_CPOL);
    
    uint8_t txbuf[4] = {0xC0};
    uint8_t rxbuf[4] = {};
    palClearPad(GPIOB, 2);
    spiExchange(&SPID3, 4, txbuf, rxbuf);
    palSetPad(GPIOB, 2);
    
    SPI3->CR1 = orig;
    
    if (!(rxbuf[0] & 0x04))
        return false;
    
    *x = (rxbuf[1] << 3) | (rxbuf[2] >> 5);
    *y = ((rxbuf[2] & 0x1F) << 6) | (rxbuf[3] >> 2);
    
    if (*x & 0x400) *x = - ((*x ^ 0x7FF) + 1);
    if (*y & 0x400) *y = - ((*y ^ 0x7FF) + 1);
    
    return true;
}

static void mmcInsertHandler(eventid_t id) {
    FRESULT err;

    LOG("MMC Card inserted");
    /*
    * On insertion MMC initialization and FS mount.
    */
    if (mmcConnect(&MMCD1)) {
        return;
    }
    err = f_mount(0, &filesystem);
    if (err != FR_OK) {
        mmcDisconnect(&MMCD1);
        return;
    }
    
    LOG("MMC Filesystem mounted successfully");
    
    DIR dir = {};
    FILINFO file = {};
    if (f_opendir(&dir, "/") == FR_OK)
    {
        while (f_readdir(&dir, &file) == FR_OK
            && file.fname[0] != 0)
        {
            LOG("Filename %s", file.fname);
        }
    }
}

static void mmcRemoveHandler(eventid_t id) {
    LOG("MMC Card removed");
}

#define PWM_MAX 1200

#include "gpio.h"
DECLARE_GPIO(mot1, GPIOA, 15);
DECLARE_GPIO(mot2, GPIOA, 12);
DECLARE_GPIO(mot3, GPIOA, 11);
DECLARE_GPIO(mot4, GPIOA, 8);

extern unsigned long __heap_end__; // From linker script
register uint32_t *stack_pointer asm("sp");
int main(void) {
    /* Clear the main thread stack */
    uint32_t *p = &__heap_end__;
    while (p < stack_pointer) *p++ = 0x55555555;
    
    /* Release the JTAG pins for other use */
    RCC->APB2ENR |= RCC_APB2ENR_AFIOEN;
    
    halInit();
    chSysInit();

    AFIO->MAPR |= AFIO_MAPR_SWJ_CFG_JTAGDISABLE;
    
    static const evhandler_t evhndl[] = {
        &mmcInsertHandler,
        &mmcRemoveHandler
    };
    struct EventListener el0, el1;
    
    mmcObjectInit(&MMCD1, &SPID1, &mmc_lowspeed, &mmc_highspeed,
                  &mmc_is_protected, &mmc_is_inserted);
    
    chEvtRegister(&MMCD1.mmc_inserted_event, &el0, 0);
    chEvtRegister(&MMCD1.mmc_removed_event, &el1, 1);
    
    mmcStart(&MMCD1, NULL);
    sdStart(&SD1, &debugPortConfig);
    sdStart(&SD3, &gprsPortConfig);
    adcStart(&ADCD1, &adcConfig);
    spiStart(&SPID3, &spiConfig);
    
    RCC->APB2ENR |= RCC_APB2ENR_AFIOEN;
    AFIO->MAPR |= AFIO_MAPR_TIM3_REMAP;
    
    // TIM3 for motor PWM
    RCC->APB1ENR |= RCC_APB1ENR_TIM3EN;
    TIM3->CR1 = 0;
    TIM3->CCER = 0;
    TIM3->CCMR1 = 0x6868; // PWM Mode 1
    TIM3->CCMR2 = 0x6868;
    TIM3->CCER = 0x1111;
    TIM3->ARR = PWM_MAX - 1;
    TIM3->CCR1 = TIM3->CCR2 = TIM3->CCR3 = 0;
    TIM3->CR1 = 0x0021;

    TIM3->CCR1 = PWM_MAX / 2;
    TIM3->CCR2 = PWM_MAX / 2;
    TIM3->CCR3 = PWM_MAX / 2;
    TIM3->CCR4 = PWM_MAX / 2;
    
//     gpio_mot1_set(1);
    
    rtc_start();
    
    chThdSleep(MS2ST(10));
    lis302_write_reg(0x20, 0x47);
    
    kalman_init();
    
    while (1)
    {
        eventmask_t event = chEvtWaitAnyTimeout(ALL_EVENTS, TIME_IMMEDIATE);
        if (event)
        {
            LOG("Got event");
            chEvtDispatch(evhndl, event);
        }
//         if (gprs_dial(&SD3))
//         {
//             rpc_server((BaseChannel*)&SD3);
//         }

//         adcsample_t result = 0;
//         adcConvert(&ADCD1, &adcGroup, &result, 1);
        
//         LOG("Pressurffffffe %d", (int)result);

        hm55b_start();

        int accX = (int8_t)lis302_read_reg(0x29);
        int accY = (int8_t)lis302_read_reg(0x2B);
        int accZ = (int8_t)lis302_read_reg(0x2D);

        chThdSleep(MS2ST(100));
        
        int magX, magY;
        hm55b_read(&magX, &magY);

        LOG("Starting update");
        kalman_update(fix16_from_int(-magX), fix16_from_int(magY),
                      fix16_from_int(accX), fix16_from_int(accY), fix16_from_int(-accZ));
        
        LOG("Acceleration %3d %3d %3d", accX, accY, accZ);
        LOG("Magnetic %3d %3d", magX, magY);
    }
}