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

#include <SEGGER_RTT.h>
#include "delay.h"
#include "util.h"
#include "sx1276.h"


static uint8_t sx1276_spi_transaction(uint8_t reg, uint8_t val) {
    uint8_t ret;
    gpio_clear(SX1276_SS_PORT, SX1276_SS_PIN);
    spi_send(SX1276_SPI, reg);
    spi_read(SX1276_SPI);
    spi_send(SX1276_SPI, val);
    ret = spi_read(SX1276_SPI);
    gpio_set(SX1276_SS_PORT, SX1276_SS_PIN);
    return ret;
}

uint8_t sx1276_read(uint8_t reg) {
    reg &= 0x7f;  // clear upper bit to make it read request
    return sx1276_spi_transaction(reg, 0x00);
}

uint8_t sx1276_write(uint8_t reg, uint8_t val) {
    reg |= 0x80;  // set upper bit to make it write request
    return sx1276_spi_transaction(reg, val);
}

void sx1276_fifo_dump(struct sx1276_state_st *state) {
    sx1276_write(SX1276_REG_FIFO_ADDR_PTR, 0x00);
    gpio_clear(SX1276_SS_PORT, SX1276_SS_PIN);
    spi_send(SX1276_SPI, SX1276_REG_FIFO & 0x7f);  // read
    spi_read(SX1276_SPI);
    for (int i=0; i < FIFO_SIZE; i++){
        spi_send(SX1276_SPI, 0x00);
        int val = spi_read(SX1276_SPI);
        state->fifo[i] = val;
    }
    gpio_set(GPIOA, GPIO4);
}

void sx1276_fifo_load(struct sx1276_state_st *state) {
    sx1276_write(SX1276_REG_FIFO_ADDR_PTR, 0x00);
    gpio_clear(SX1276_SS_PORT, SX1276_SS_PIN);
    spi_send(SX1276_SPI, SX1276_REG_FIFO | 0x80);  // write
    spi_read(SX1276_SPI);
    for (int i=0; i < FIFO_SIZE; i++){
        spi_send(SX1276_SPI, state->fifo[i]);
        spi_read(SX1276_SPI);
    }
    gpio_set(GPIOA, GPIO4);
}

void sx1276_fifo_write(uint8_t addr, uint8_t val) {
    sx1276_write(SX1276_REG_FIFO_ADDR_PTR, addr);
    sx1276_write(SX1276_REG_FIFO, val);
}

void sx1276_set_frequency(uint64_t frequency) {
    uint64_t frf = ((uint64_t)frequency << 19) / 32000000;

    sx1276_write(SX1276_REG_FRF_MSB, (uint8_t)(frf >> 16));
    sx1276_write(SX1276_REG_FRF_MID, (uint8_t)(frf >> 8));
    sx1276_write(SX1276_REG_FRF_LSB, (uint8_t)(frf >> 0));
}

struct sx1276_state_st *sx1276_init(uint64_t frequency) {
    struct sx1276_state_st *sx1276_state = (struct sx1276_state_st*) malloc(sizeof(struct sx1276_state_st));
    if (sx1276_state == NULL) {
        return NULL;
    }

    SEGGER_RTT_printf(0, "SX1276: Reset\n");
    gpio_clear(GPIOB, GPIO0);
    delay_us(100);
    gpio_set(GPIOB, GPIO0);
    delay_ms(5);

    uint8_t ret = sx1276_read(SX1276_REG_VERSION);
//    if (ret != 0x12) {
//        SEGGER_RTT_printf(0, "Invalid Chip Version, got: 0x%02x, expected: 0x12\n", ret);
//        //free(sx1276_state);
//        //return NULL;
//    }
    SEGGER_RTT_printf(0, "Chip Version: 0x%02X\n", ret);

    uint8_t set = SX1276_MODE_LONG_RANGE_MODE | SX1276_MODE_SLEEP;
    ret = sx1276_write(SX1276_REG_OP_MODE, set);
    SEGGER_RTT_printf(0, "Going to LoRa SLEEP from: 0x%02X, to: 0x%02X\n",
                      ret, set);

    sx1276_set_frequency(frequency);

    sx1276_write(SX1276_REG_FIFO_TX_BASE_ADDR, 0);
    sx1276_write(SX1276_REG_FIFO_RX_BASE_ADDR, 0);

    sx1276_write(SX1276_REG_LNA, 0b11000000);  // minimum strenght
    sx1276_write(SX1276_REG_PA_CONFIG, 1<<7); // minimum 2dBm

    // Bw 7 125khz; CodingRate 1 4/5; implicit header mode
    sx1276_write(SX1276_REG_MODEM_CONFIG_1, (7 << 4) | (1 << 1) | 1);
    // SpreadingFactor 7 128 chips
    sx1276_write(SX1276_REG_MODEM_CONFIG_2, (7 << 4));


    set = SX1276_MODE_LONG_RANGE_MODE | SX1276_MODE_STDBY;
    ret = sx1276_write(SX1276_REG_OP_MODE, set);
    SEGGER_RTT_printf(0, "Going to LoRa STDBY from: 0x%02X, to: 0x%02x\n",
                      ret, set);


    // We need to wait ~250us for TS_OSC to startup before we can access FIFO and some other things
    delay_us(250);
    return sx1276_state;
};