/*
 * Copyright (C) 2023 Daniele Lacamera <root@danielinux.net>
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <unicore-mx/stm32/memorymap.h>
#include <unicore-mx/stm32/rcc.h>
#include <unicore-mx/stm32/gpio.h>
#include <unicore-mx/stm32/usart.h>
#include <unicore-mx/stm32/f7/nvic.h>
#include <stddef.h>
#include <string.h>
#include "systick.h"
#include "task.h"
#include "ksp-serial.h"

static void ksp_serial_poll(uint32_t ev, void *arg);
static void ksp_serial_init(void);
static uint32_t last_poll = 0;
static volatile uint8_t buf[4096]; /* Reserve a page for serial comm */
static volatile uint32_t buf_count = 0;
static uint8_t kspio_rxlen;
static int kspio_busy;

    

#define KSP_UART USART1

const char ksp_name[] = "KSPSerial";

struct task ksp_task = {
    .init = ksp_serial_init,
    .run = ksp_serial_poll,
    .events = EV_SERIAL,
    .name = ksp_name
};


static uint8_t cur_vdata_buf[204];
vesselData_t *cur_vdata = (vesselData_t *)(cur_vdata_buf + 3);


const char vesselDataName[][18] = {
	"id",             //1
	"AP",               //2
	"PE",               //3
	"SemiMajorAxis",    //4
	"SemiMinorAxis",    //5
	"VVI",              //6
	"e",                //7
	"inc",              //8
	"G",                //9
	"TAp",            //10
	"TPe",            //11
	"TrueAnomaly",      //12
	"Density",          //13
	"t period",         //14
	"RAlt",             //15
	"Alt",              //16
	"Vsurf",            //17
	"Lat",              //18
	"Lon",              //19
	"LiquidFuelTot",    //20
	"LiquidFuel",       //21
	"OxidizerTot",      //22
	"Oxidizer",         //23
	"EChargeTot",       //24
	"ECharge",          //25
	"MonoPropTot",      //26
	"MonoProp",         //27
	"IntakeAirTot",     //28
	"IntakeAir",        //29
	"SolidFuelTot",     //30
	"SolidFuel",        //31
	"XenonGasTot",      //32
	"XenonGas",         //33
	"LiquidFuelTotS",   //34
	"LiquidFuelS",      //35
	"OxidizerTotS",     //36
	"OxidizerS",        //37
	"MissionTime",   //38
	"deltaTime",        //39
	"VOrbit",           //40
	"MNTime",        //41
	"MNDeltaV",         //42
	"Pitch",            //43
	"Roll",             //44
	"Heading",          //45
};

uint8_t ksp_serial_checksum(uint8_t *bytes, uint8_t sz)
{
    uint8_t chk = sz;
    uint8_t i;
    for (i = 0; i < sz; i++) {
        chk ^= bytes[i];
    }
    return chk;
}


void ksp_serial_init(void)
{
    last_poll = jiffies;
	rcc_periph_clock_enable(RCC_USART1);
	rcc_periph_clock_enable(RCC_GPIOA);
	rcc_periph_clock_enable(RCC_GPIOB);

	gpio_mode_setup(GPIOA, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO9);
	gpio_mode_setup(GPIOB, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO7);
	gpio_set_af(GPIOA, GPIO_AF7, GPIO9);
	gpio_set_af(GPIOB, GPIO_AF7, GPIO7);
    

	/* Set up USART/UART parameters using the unicore-mx helper functions */
    usart_enable_rx_interrupt(KSP_UART);
	usart_set_baudrate(KSP_UART, 115200);
	usart_set_databits(KSP_UART, 8);
	usart_set_stopbits(KSP_UART, USART_STOPBITS_1);
	usart_set_mode(KSP_UART, USART_MODE_TX_RX);
	usart_set_parity(KSP_UART, USART_PARITY_NONE);
	usart_set_flow_control(KSP_UART, USART_FLOWCONTROL_NONE);
    usart_enable_rx_interrupt(KSP_UART);
    USART_CR1(KSP_UART) &= ~(USART_CR1_TCIE);
    nvic_enable_irq(NVIC_USART1_IRQ);
	usart_enable(KSP_UART);
}

void kspserial_setup(void)
{
    register_task(&ksp_task);
}

void ksp_serial_send(const void *data, uint8_t len)
{
    int i;
    unsigned char *bytes = (unsigned char *)data;
    uint8_t chk = ksp_serial_checksum(bytes, len);
    usart_send_blocking(KSP_UART, 0xBE);
    usart_send_blocking(KSP_UART, 0xEF);
    usart_send_blocking(KSP_UART, len);
    for(i = 0; i < len; i++)
        usart_send_blocking(KSP_UART, bytes[i]);
    usart_send_blocking(KSP_UART, chk);
}

static void ksp_serial_handshake(void)
{
    handShakePacket_t hs;
    hs.id = 0;
    hs.M1 = 3;
    hs.M2 = 1;
    hs.M3 = 4;
    ksp_serial_send(&hs, sizeof(hs));
}

static void ksp_serial_rxdata(void)
{
    uint8_t chk_exp, chk_rx;
    if (buf[2] == sizeof(vesselData_t)) {
        chk_rx = buf[sizeof(vesselData_t) + 3];
        chk_exp = ksp_serial_checksum(buf + 3, buf[2]);
        if (chk_rx != chk_exp) {
            return;
        }
        memcpy(cur_vdata_buf, buf, buf[2] + 3);
        memset(buf, 0, sizeof(buf));
        buf_count = 0;
        trigger_event(EV_VESSELDATA);
    }
}

static void ksp_serial_poll(uint32_t ev, void *arg)
{
    if (buf_count == 0) {
        clear_event(EV_SERIAL);
        return;
    }

    while((buf_count < (buf[2] + 4)) && usart_is_recv_ready(KSP_UART)) {
        buf[buf_count++] = usart_recv(KSP_UART);
    }
    if (buf_count < buf[2] + 4)
        return;

    if (buf[3] == (uint8_t)0) {
        ksp_serial_handshake();
    }
    else if (buf[3] == (uint8_t)1) {
        ksp_serial_rxdata();
    }
    clear_event(EV_SERIAL);
    buf_count = 0;
    usart_enable_rx_interrupt(KSP_UART);
    kspio_busy = 0;
    last_poll = jiffies;
}

#include <unicore-mx/stm32/usart.h>
#include "task.h"
void isr_usart1(void)
{
    uint16_t rx;
    usart_clear_rx_interrupt(KSP_UART);
    usart_clear_tx_interrupt(KSP_UART);
   
    if (usart_is_recv_ready(KSP_UART)) {
        rx = usart_recv(KSP_UART);
        buf[buf_count++] = (unsigned char)(rx & 0xFF);
        if (kspio_busy) {
            trigger_event(EV_SERIAL);
            return;
        }
        
        if ((buf_count == 1) && (buf[0] != 0xBE)) {
            buf_count = 0;
            return;
        }

        if ((buf_count == 2) && (buf[1] != 0xEF)) {
            buf_count = 0;
            return;
        }

        if (buf_count == 3) {
            kspio_rxlen = buf[2];
            kspio_busy++;
            trigger_event(EV_SERIAL);
            return;
        }
    }
}