#include <string.h>

#include <libopencm3/stm32/rcc.h>
#include <libopencm3/stm32/gpio.h>
#include <libopencm3/stm32/spi.h>

#include <SEGGER_RTT.h>
#include <sx1276.h>

#include "delay.h"
#include "util.h"

#define JOY_PORT  GPIOB
#define JOY_UP    GPIO7
#define JOY_DOWN  GPIO6
#define JOY_LEFT  GPIO5
#define JOY_RIGHT GPIO4


#define AIN1_PORT GPIOB
#define AIN1_PIN  GPIO14
#define AIN2_PORT GPIOB
#define AIN2_PIN  GPIO15

#define BIN1_PORT GPIOB
#define BIN1_PIN  GPIO12
#define BIN2_PORT GPIOB
#define BIN2_PIN  GPIO13


uint8_t FIFO[FIFO_SIZE] = {0};

static void clock_setup(void)
{
    rcc_clock_setup_pll(&rcc_hse_configs[RCC_CLOCK_HSE8_72MHZ]);

    rcc_periph_clock_enable(RCC_GPIOA);
    rcc_periph_clock_enable(RCC_GPIOB);

    rcc_periph_clock_enable(RCC_SPI1);
}

static void gpio_setup(void)
{
    gpio_set_mode(GPIOB, GPIO_MODE_OUTPUT_2_MHZ, GPIO_CNF_OUTPUT_PUSHPULL,
              GPIO0 | GPIO1 | AIN1_PIN | AIN2_PIN | BIN1_PIN | BIN2_PIN);

    gpio_set_mode(JOY_PORT, GPIO_MODE_INPUT, GPIO_CNF_INPUT_PULL_UPDOWN,
                  JOY_UP | JOY_DOWN | JOY_LEFT | JOY_RIGHT);
    gpio_set(JOY_PORT, JOY_UP | JOY_DOWN | JOY_LEFT | JOY_RIGHT);
}

static void spi_setup(void)
{
    /* Configure GPIOs: SS=PA4, SCK=PA5, MISO=PA6 and MOSI=PA7 */
    gpio_set_mode(GPIOA, GPIO_MODE_OUTPUT_50_MHZ,
                  GPIO_CNF_OUTPUT_ALTFN_PUSHPULL,
                  GPIO5 | GPIO7 );

    gpio_set_mode(GPIOA, GPIO_MODE_OUTPUT_2_MHZ, GPIO_CNF_OUTPUT_PUSHPULL, GPIO4);
    gpio_set(GPIOA, GPIO4);

    gpio_set_mode(GPIOA, GPIO_MODE_INPUT, GPIO_CNF_INPUT_FLOAT,
                  GPIO6);

    /* Reset SPI, SPI_CR1 register cleared, SPI is disabled */
    rcc_periph_reset_pulse(RST_SPI1);


    /* Set up SPI in Master mode with:
     * Clock baud rate: 1/64 of peripheral clock frequency
     * Clock polarity: Idle Low
     * Clock phase: Data valid on 1st clock pulse
     * Data frame format: 8-bit
     * Frame format: MSB First
     */
    spi_init_master(SPI1, SPI_CR1_BAUDRATE_FPCLK_DIV_64,
                    SPI_CR1_CPOL_CLK_TO_0_WHEN_IDLE,
                    SPI_CR1_CPHA_CLK_TRANSITION_1, SPI_CR1_DFF_8BIT, SPI_CR1_MSBFIRST);

    /*
     * Set NSS management to software.
     *
     * Note:
     * Setting nss high is very important, even if we are controlling the GPIO
     * ourselves this bit needs to be at least set to 1, otherwise the spi
     * peripheral will not send any data out.
     */
    spi_enable_software_slave_management(SPI1);
    spi_set_nss_high(SPI1);

    /* Enable SPI1 periph. */
    spi_enable(SPI1);
}

static void print_status(void) {
    int dio = gpio_get(GPIOA, GPIO2 | GPIO3);
    SEGGER_RTT_printf(0, "DIO5: %d DIO0: %d ", (dio&GPIO3)?1:0, (dio&GPIO2)?1:0);
    int ret = sx1276_read(SX1276_REG_OP_MODE);
    SEGGER_RTT_printf(0, "MOD: 0x%02x ", ret);
    ret = sx1276_read(SX1276_REG_MODEM_STATUS);
    SEGGER_RTT_printf(0, "STAT: 0x%02x ", ret);
    ret = sx1276_read(SX1276_REG_IRQ_FLAGS);
    SEGGER_RTT_printf(0, "IRQ: 0x%02x ", ret);
    ret = sx1276_read(SX1276_REG_FIFO_RX_CURRENT_ADDR);
    SEGGER_RTT_printf(0, "FIFO_RX_START: 0x%02x ", ret);
    ret = sx1276_read(SX1276_REG_FIFO_RX_BYTE_PTR);
    SEGGER_RTT_printf(0, "FIFO_RX_END: 0x%02x\n", ret);
}


static uint8_t handle_irq_flags(void) {
    uint8_t irq_flags = sx1276_read(SX1276_REG_IRQ_FLAGS);

    if (irq_flags & SX1267_LORA_IRQ_RX_TIMEOUT) {
        sx1276_write(SX1276_REG_IRQ_FLAGS, SX1267_LORA_IRQ_RX_TIMEOUT);
        SEGGER_RTT_printf(0," RX_TIMEOUT ");
    }
    if (irq_flags & SX1267_LORA_IRQ_RX_DONE) {
        sx1276_write(SX1276_REG_IRQ_FLAGS, SX1267_LORA_IRQ_RX_DONE);
        //SEGGER_RTT_printf(0, " RX_DONE ");
    }
    if (irq_flags & SX1267_LORA_IRQ_PAYLOAD_CRC_ERR) {
        sx1276_write(SX1276_REG_IRQ_FLAGS, SX1267_LORA_IRQ_PAYLOAD_CRC_ERR);
        SEGGER_RTT_printf(0, " RX_CRC_ERR ");
    }
    if (irq_flags & SX1267_LORA_IRQ_VALID_HEADER) {
        sx1276_write(SX1276_REG_IRQ_FLAGS, SX1267_LORA_IRQ_VALID_HEADER);
        //SEGGER_RTT_printf(0, " RX_VALID_HEADER ");
    }
    if (irq_flags & SX1267_LORA_IRQ_TX_DONE) {
        sx1276_write(SX1276_REG_IRQ_FLAGS, SX1267_LORA_IRQ_TX_DONE);
        //SEGGER_RTT_printf(0, " TX_DONE ");
    }
    if (irq_flags) {
        //SEGGER_RTT_printf(0, "\n");
    }
    return irq_flags;
}

static void send(struct sx1276_state_st *state, uint8_t *msg, size_t msg_len) {
    gpio_set(GPIOB, GPIO1);

    memcpy(state->fifo, msg, msg_len);
    sx1276_fifo_load(state);
    sx1276_fifo_dump(state);
    //hexdump((char *)state->fifo, msg_len, 8);

    sx1276_write(SX1276_REG_PAYLOAD_LENGTH, msg_len);

    sx1276_write(SX1276_REG_DIO_MAPPING_1, 0b01000000); // configure DIO0 as TX done flag

    print_status();
    sx1276_write(SX1276_REG_OP_MODE, SX1276_MODE_LONG_RANGE_MODE | SX1276_MODE_TX);

    for (int i = 0; i < 10; ++i) {
        print_status();
        if (handle_irq_flags() & SX1267_LORA_IRQ_TX_DONE) {  // tx done flag check
            break;
        }
        delay_ms(1);
    }

    print_status();
    sx1276_write(SX1276_REG_DIO_MAPPING_1, 0b00000000); // configure DIO0 as RX done flag
    //SEGGER_RTT_printf(0, "Done\n");

    gpio_clear(GPIOB, GPIO1);

}


typedef enum {
    MOTOR_BRAKE,
    MOTOR_CCW,
    MOTOR_CW,
    MOTOR_STOP,
} motor_state_t;


static void drive_b(motor_state_t state) {
    switch (state) {
        case MOTOR_BRAKE:
            gpio_set(BIN1_PORT, BIN1_PIN);
            gpio_set(BIN2_PORT, BIN2_PIN);
            break;
        case MOTOR_CCW:
            gpio_clear(BIN1_PORT, BIN1_PIN);
            gpio_set(BIN2_PORT, BIN2_PIN);
            break;
        case MOTOR_CW:
            gpio_set(BIN1_PORT, BIN1_PIN);
            gpio_clear(BIN2_PORT, BIN2_PIN);
            break;
        case MOTOR_STOP:
            gpio_clear(BIN1_PORT, BIN1_PIN);
            gpio_clear(BIN2_PORT, BIN2_PIN);
            break;
    }
}

static void drive_a(motor_state_t state) {
    switch (state) {
        case MOTOR_BRAKE:
            gpio_set(AIN1_PORT, AIN1_PIN);
            gpio_set(AIN2_PORT, AIN2_PIN);
            break;
        case MOTOR_CCW:
            gpio_clear(AIN1_PORT, AIN1_PIN);
            gpio_set(AIN2_PORT, AIN2_PIN);
            break;
        case MOTOR_CW:
            gpio_set(AIN1_PORT, AIN1_PIN);
            gpio_clear(AIN2_PORT, AIN2_PIN);
            break;
        case MOTOR_STOP:
            gpio_clear(AIN1_PORT, AIN1_PIN);
            gpio_clear(AIN2_PORT, AIN2_PIN);
            break;
    }
}

static void receive(struct sx1276_state_st *state) {
    gpio_set(GPIOB, GPIO1);

    //print_status();
    sx1276_write(SX1276_REG_PAYLOAD_LENGTH, 5);
    sx1276_write(SX1276_REG_OP_MODE, SX1276_MODE_LONG_RANGE_MODE | SX1276_MODE_RX_CONTINUOUS);
    //for (int i = 0; i < 10; ++i) {
    while(1){
        //print_status();
        if (handle_irq_flags() & (
                SX1267_LORA_IRQ_RX_TIMEOUT |
                SX1267_LORA_IRQ_RX_DONE |
                SX1267_LORA_IRQ_PAYLOAD_CRC_ERR
                ))
        {
            sx1276_fifo_dump(state);
            //hexdump((char *)state->fifo, 4, 16);
            if (state->fifo[0] == 0xde && state->fifo[1] == 0xad && state->fifo[2] == 0xbe) {
                SEGGER_RTT_printf(0, "MOTOR: %x, %x\n", state->fifo[3], state->fifo[4]);
                drive_b(state->fifo[3]);
                drive_a(state->fifo[4]);
            } else {
                //hexdump((char *)state->fifo, 5, 16);
            }
            break;
        }
        delay_ms(10);
    }

    //SEGGER_RTT_printf(0, "Done\n");

    sx1276_write(SX1276_REG_OP_MODE, SX1276_MODE_LONG_RANGE_MODE | SX1276_MODE_STDBY);


    gpio_clear(GPIOB, GPIO1);
}


static uint16_t joy_state(void) {
    uint32_t joy_state = ~gpio_get(JOY_PORT, JOY_UP|JOY_DOWN|JOY_LEFT|JOY_RIGHT);
    uint8_t ret[2] = {MOTOR_BRAKE, MOTOR_BRAKE};
    if (joy_state & JOY_UP) {
        //SEGGER_RTT_printf(0, "UP\n");
        ret[0] = MOTOR_CCW;
    }
    else if (joy_state & JOY_DOWN) {
        //SEGGER_RTT_printf(0, "DOWN\n");
        ret[0] = MOTOR_CW;
    } else {
        //SEGGER_RTT_printf(0, "BREAK\n");
        ret[0] = MOTOR_BRAKE;
    }

    if (joy_state & JOY_LEFT) {
        SEGGER_RTT_printf(0, "LEFT\n");
        ret[1] = MOTOR_CCW;
    } else if (joy_state & JOY_RIGHT) {
        SEGGER_RTT_printf(0, "RIGHT\n");
        ret[1] = MOTOR_CW;
    } else {
        SEGGER_RTT_printf(0, "CENTER\n");
        ret[1] = MOTOR_BRAKE;
    }

    return (uint16_t)(ret[0] | (ret[1] << 8));
}


int main(void)
{
    clock_setup();
    spi_setup();
    gpio_setup();

    SEGGER_RTT_WriteString(0, "SX1276 LoRa test\n");
    SEGGER_RTT_printf(0, "CPU clk: %dHz\n", rcc_ahb_frequency);

    struct sx1276_state_st *sx1276_state = sx1276_init(865200000);

    //send(sx1276_state);

    /// Transmitter code
//    while(1) {
//        uint16_t motorState = joy_state();
//        SEGGER_RTT_printf(0, "Motor: %x\n", motorState);
//        uint8_t msg[5] = {0xde, 0xad, 0xbe, (uint8_t) (motorState & 0xff), (uint8_t)(motorState >> 8)};
//        send(sx1276_state, msg, 5);
//        //for (int i = 0; i < 10; i++) {
//        delay_ms(5000);
//        //}
//    }

    /// Receiver code
    while (1) {
//        char r = SEGGER_RTT_WaitKey();
//        if (r == '\n') {
            receive(sx1276_state);
//        }
        //delay_ms(50);
    }

    return 0;
}