#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"

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);
}

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 */
    spi_reset(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() {
    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\n");
    }
    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\n");
    }
    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\n");
    }
    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\n");
    }
    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\n");
    }
    return irq_flags;
}

static void send(struct sx1276_state_st *state) {

    gpio_set(GPIOB, GPIO1);


    char msg[] = "Hello World\n";
    uint32_t msg_len = strlen(msg) + 1;
    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, 0x4);

    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 < 100; ++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);

}


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

    print_status();
    sx1276_write(SX1276_REG_OP_MODE, SX1276_MODE_LONG_RANGE_MODE | SX1276_MODE_RX_CONTINUOUS);
    for (int i = 0; i < 100; ++i) {
        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, 16, 16);
        }
        delay_ms(100);
    }

    SEGGER_RTT_printf(0, "Done\n");

    sx1276_write(SX1276_REG_OP_MODE, SX1276_MODE_LONG_RANGE_MODE | SX1276_MODE_STDBY);


    gpio_clear(GPIOB, GPIO1);
}

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);

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

    return 0;
}