#include "rfm69.h"

uint32_t msg_hash;
uint8_t p_hash_1;
uint8_t p_hash_2;
uint8_t p_hash_3;

uint8_t c_hash_1;
uint8_t c_hash_2;
uint8_t c_hash_3;

bool cond_1;
bool cond_2;
bool cond_3;

DATA_SEND_STATUS send_message(tx_rx_data_struct tx_data)
{
    
    
    rfm69_write_msg(tx_data);

    
    p_hash_1 = tx_data.msg[57];
    p_hash_2 = tx_data.msg[58];
    p_hash_3 = tx_data.msg[59];

    for (uint8_t i = 0; i < 10; i++) {
        bool result = wait_rx_payload_ready_timeout(50);
        if (result) {
            RX_DATA = rfm69_read_msg();
#if DO_UART
            uart_sendString("RX DATA\n");
            uart_print_tx_rx_data(RX_DATA);
#endif
            c_hash_1 = RX_DATA.msg[0];
            c_hash_2 = RX_DATA.msg[1];
            c_hash_3 = RX_DATA.msg[2];

            cond_1 = (p_hash_1 == c_hash_1) && (p_hash_2 == c_hash_2) && (p_hash_3 == c_hash_3);
            cond_2 = (tx_data.from == RX_DATA.to) && (tx_data.to == RX_DATA.from);
            cond_3 = cond_1 && cond_2;

            if (cond_3 && (RX_DATA.flags == MSG_RECV_COUNTS_SUCCESS) && (RX_DATA.msg[3] == 0xFF)) {
#if DO_UART
                uart_sendString("       RX DATA SUCCESS\n");
#endif
                return DATA_SEND_SUCCESS;
            }
#if DO_UART
            else if (
                cond_3 && (RX_DATA.flags == MSG_RECV_COUNTS_FAIL) && (RX_DATA.msg[3] == 0x00)) {
                uart_sendString("       RX DATA FAILED\n");
            } else {
                uart_sendString("       RX DATA ANOTHER ERROR\n");
            }
#endif
        }
    }
    return DATA_NOT_SENT;
}

void uart_print_tx_rx_data(tx_rx_data_struct tx_rx_print)
{

    // uart_print_uint8(tx_rx_print.len, "LEN; ");
    // uart_print_uint8(tx_rx_print.to, "TO;");
    // uart_print_uint8(tx_rx_print.from, "FROM;");
    // uart_print_uint8(tx_rx_print.dtype, "DTYPE;");
    // uart_print_uint8(tx_rx_print.flags, "FLAGS;");
    DATA_BUFFER_7[0] = tx_rx_print.len;
    DATA_BUFFER_7[1] = tx_rx_print.to;
    DATA_BUFFER_7[2] = tx_rx_print.from;
    DATA_BUFFER_7[3] = tx_rx_print.dtype;
    DATA_BUFFER_7[4] = tx_rx_print.flags;
    uart_sendString("    ");
    uart_print_uint8_array(DATA_BUFFER_7, 5, "LEN,TO,FROM,DTYPE,FLAGS\n");
    uart_sendString("    ");
    uart_sendStringArray(tx_rx_print.msg, 20);
    uart_sendChar('\n');
    uart_sendString("    ");
    uart_print_uint8_array(tx_rx_print.msg, tx_rx_print.len, "\n");
}

void rfm69_write_msg(tx_rx_data_struct txrxd)
{
    // TxStart is configured as FifoNotEmpty, so the radio begins transmitting
    // the moment the first byte lands. Fill the FIFO from standby and only then
    // switch to TX, otherwise the packet goes out ahead of its own payload.
    set_rfm69_standby();

    spi_rfm69_select(true);
    spi_write(REG_FIFO | RFM69_SPI_WRITE);
    if (txrxd.len > (60)) {
        txrxd.len = 60;
    }

    spi_write(txrxd.len + 4);
    spi_write(txrxd.to);
    spi_write(txrxd.from);
    spi_write(txrxd.dtype);
    spi_write(txrxd.flags);
    for (uint8_t x = 0; x < txrxd.len; x++) {
        spi_write(txrxd.msg[x]);
    }
    spi_rfm69_select(false);

    set_rfm69_tx_mode();
    wait_tx_sent();
    set_rfm69_rx_mode();
}

tx_rx_data_struct rfm69_read_msg(void)
{
    memset(RX_DATA.msg, ' ', sizeof(RX_DATA.msg));
    spi_rfm69_select(true);
    spi_write(REG_FIFO);

    uint8_t raw_len = spi_read();
    RX_DATA.to = spi_read();
    RX_DATA.from = spi_read();
    RX_DATA.dtype = spi_read();
    RX_DATA.flags = spi_read();

    // The length byte comes off the air and is not trustworthy: below 4 it
    // underflows to ~252, above 60 it walks off the end of msg[].
    uint8_t len_f = (raw_len < 4) ? 0 : (uint8_t)(raw_len - 4);
    if (len_f > sizeof(RX_DATA.msg)) {
        len_f = sizeof(RX_DATA.msg);
    }
    RX_DATA.len = len_f;

    for (uint8_t idx_f = 0; idx_f < len_f; idx_f++) {
        RX_DATA.msg[idx_f] = spi_read();
    }
    spi_rfm69_select(false);
    set_rfm69_idle();

    return RX_DATA;
}

uint8_t spi_read_rfm69_rt(uint8_t reg)
{
    spi_rfm69_select(true);
    spi_write(reg);
    uint8_t data_read = spi_read();
    spi_rfm69_select(false);
    return data_read;
}

uint8_t spi_write_rfm69_rt(uint8_t reg, uint8_t val)
{
    spi_rfm69_select(true);
    spi_write(reg | RFM69_SPI_WRITE);
    uint8_t data_read = spi_write(val);
    spi_rfm69_select(false);
    return data_read;
}

uint8_t spi_write_rfm69_multiple_rt(uint8_t reg, const char* vals, uint8_t len)
{
    spi_rfm69_select(true);
    uint8_t data_init = spi_write(reg | RFM69_SPI_WRITE);
    while (len--)
        spi_write(*vals++);

    spi_rfm69_select(false);
    return data_init;
}

// name (max 10),           10
// diameter (max 10),       20
// battery_value 16-bit,    22
// time_reading (min)       23
// time_reading (hour)      24
// time_reading (day)       25
// time_reading (month)     26
// time_reading (year)      27
// 15 * per-min  +30        57
// three byte hash check     3

tx_rx_data_struct generate_wheel_counts_message(
    identifier_results idd, time_struct time, uint16_t battery_value, volatile uint16_t counts[15])
{

    reset_txrx_struct(&TX_DATA);
    memcpy(TX_DATA.msg, idd.name_str, MIN(10, idd.name_len));
    memcpy(TX_DATA.msg + 10, idd.diameter_str, MIN(10, idd.diameter_len));

    TX_DATA.msg[20] = battery_value & 0xFF;
    TX_DATA.msg[21] = (battery_value >> 8) & 0xFF;

    TX_DATA.msg[22] = time.Minute;
    TX_DATA.msg[23] = time.Hour;
    TX_DATA.msg[24] = time.Day;
    TX_DATA.msg[25] = time.Month;
    TX_DATA.msg[26] = time.Year;

    for (uint8_t idx = 0; idx < 15; idx++) {
        TX_DATA.msg[26 + (2 * idx + 1)] = counts[idx] & 0xFF; // LSB first
        TX_DATA.msg[26 + (2 * idx + 2)] = (counts[idx] >> 8) & 0xFF; // MSB second
    }

    msg_hash = hash_3bytes(TX_DATA.msg, 57);
    TX_DATA.msg[57] = msg_hash & 0xFF;
    TX_DATA.msg[58] = (msg_hash >> 8) & 0xFF;
    TX_DATA.msg[59] = (msg_hash >> 16) & 0xFF;

    TX_DATA.len = sizeof(TX_DATA.msg);
    TX_DATA.flags = MSG_SEND_COUNTS;
    TX_DATA.from = idd.hashed;
    TX_DATA.to = 255;
    TX_DATA.dtype = MSG_TYPE_BINARY;
    return TX_DATA;
}

uint32_t hash_3bytes(unsigned const char* str, uint8_t str_len)
{
    uint32_t hash = 0;

    for (uint8_t i = 0; i < str_len; i++) {
        hash = (hash * 31 + str[i]) % 0xFFFFFF;
    }
    return hash;
}

void set_rfm69_power_amp_boost(void)
{
    spi_write_rfm69_rt(REG_OCP, VAL_OCP_OFF);
    spi_write_rfm69_rt(REG_TEST_PA1, VAL_TEST_PA1_BOOST);
    spi_write_rfm69_rt(REG_TEST_PA2, VAL_TEST_PA2_BOOST);
}

void set_rfm69_power_amp_normal(void)
{
    spi_write_rfm69_rt(REG_TEST_PA1, VAL_TEST_PA1_NORMAL);
    spi_write_rfm69_rt(REG_TEST_PA2, VAL_TEST_PA2_NORMAL);
    spi_write_rfm69_rt(REG_OCP, VAL_OCP_ON);
}

void reset_txrx_struct(tx_rx_data_struct* s)
{
    s->len = 0;
    s->to = 255;
    s->from = 255;
    s->dtype = 0;
    s->flags = 0;
    memset(s->msg, ' ', 60);
}

void set_rfm69_mode(uint8_t target_mode)
{

    uint8_t mode = spi_read_rfm69_rt(REG_OP_MODE);
    mode &= ~VAL_OPMODE_MASK;
    mode |= (target_mode & VAL_OPMODE_MASK);
    spi_write_rfm69_rt(REG_OP_MODE, mode);
}

bool wait_tx_sent(void)
{
    for (uint16_t attempts = 0; attempts < RFM69_TIMEOUT_MS; attempts++) {
        if (TX_SENT) {
            return true;
        }
        _delay_ms(1);
    }
    return false;
}

uint8_t hash(const char* str, uint8_t min, uint8_t max)
{
    unsigned int hash = 0;
    while (*str) {
        hash = (hash * 31) + (unsigned char)(*str);
        str++;
    }

    unsigned int range = max - min + 1;
    return (hash % range) + min;
}

bool wait_rx_payload_ready_timeout(uint16_t attempts)
{
    set_rfm69_rx_mode();
    // Test the flag before spending the tick, so a payload that arrives on the
    // last attempt is not thrown away.
    for (uint16_t counter = 0; counter < attempts; counter++) {
        if (RX_PAYLOAD_READY) {
            return true;
        }
        _delay_ms(1);
    }
    return RX_PAYLOAD_READY != 0;
}

bool wait_rx_payload_ready(void)
{
    return wait_rx_payload_ready_timeout(RFM69_TIMEOUT_MS);
}

bool wait_rfm69_mode_ready(void)
{
    for (uint16_t attempts = 0; attempts < RFM69_TIMEOUT_MS; attempts++) {
        if (MODE_READY) {
            return true;
        }
        _delay_ms(1);
    }
    return false;
}

void set_rfm69_tx_mode(void)
{
    set_rfm69_power_amp_boost();
    set_rfm69_mode(VAL_OPMODE_TX);
    wait_rfm69_mode_ready();
}

void set_rfm69_rx_mode(void)
{
    set_rfm69_power_amp_normal();
    set_rfm69_mode(VAL_OPMODE_RX);
    wait_rfm69_mode_ready();
}

void set_rfm69_standby(void)
{
    set_rfm69_power_amp_normal();
    set_rfm69_mode(VAL_OPMODE_STDBY);
    wait_rfm69_mode_ready();
}

void set_rfm69_sleep(void)
{
    set_rfm69_power_amp_normal();
    set_rfm69_mode(VAL_OPMODE_SLEEP);
    wait_rfm69_mode_ready();
}

void set_rfm69_idle(void)
{
    set_rfm69_power_amp_normal();
    set_rfm69_mode(VAL_OPMODE_STDBY);
    wait_rfm69_mode_ready();
}

void reset_rfm69(void)
{
    rfm69_reset_state(true);
    _delay_ms(10);
    rfm69_reset_state(false);
    _delay_ms(10);
}

void rfm69_init(void)
{
    reset_rfm69();
    _delay_ms(100);
    set_rfm69_idle();
    spi_write_rfm69_rt(REG_FREQ_MSB, VAL_FREQ_433MHz_MSB);
    spi_write_rfm69_rt(REG_FREQ_MIDDLE_SB, VAL_FREQ_433MHz_MID_SB);
    spi_write_rfm69_rt(REG_FREQ_LSB, VAL_FREQ_433MHz_LSB);

    spi_write_rfm69_rt(
        REG_FIFO_THRESH,
        VAL_TX_START_FIFO_NOT_EMPTY | VAL_FIFO_LEVEL_INTERRUPT); // TX condition
    spi_write_rfm69_rt(REG_TEST_DAGC,
                       VAL_TEST_DAGC_DEFAULT); // Fading margin improvement

    char sync_words[] = { 0x2d, 0xd4 };
    spi_write_rfm69_multiple_rt(REG_SYNC_VALUE_1, sync_words, 2);
    spi_write_rfm69_rt(REG_SYNC_CONFIG, VAL_SYNCWORDS_ON | VAL_SYNCWORDS_SIZE_2_BYTES);

    spi_write_rfm69_rt(REG_DATA_MODUL,
                       VAL_DATA_PACKET_MODE | VAL_DATA_MODUL_FSK
                           | VAL_MODUL_SHAPING_GAUSS_BT_1_0); // RegDataModul

    spi_write_rfm69_rt(REG_BITRATE_MSB,
                       VAL_BITRATE_250kbps_MSB); // RegBitrateMSB
    spi_write_rfm69_rt(REG_BITRATE_LSB,
                       VAL_BITRATE_250kbps_LSB); // RegbBitrateLSB
    spi_write_rfm69_rt(REG_FDEV_MSB, VAL_FDEV_MSB); // RegFdevMSB (0x05)
    spi_write_rfm69_rt(REG_FDEV_LSB, VAL_FDEV_LSB); // RegFdevLSB (0x06)

    spi_write_rfm69_rt(REG_RX_BW, 0xE0); // RegRxBw
    spi_write_rfm69_rt(REG_AFC_BW, 0xE0); // RegAfcBw

    spi_write_rfm69_rt(
        REG_PACKET_CONFIG_1,
        VAL_PACKET_VARIABLE_LENGTH | VAL_PACKET_WHITENING | VAL_PACKET_CRCON); // RegPacketConfig1

    spi_write_rfm69_rt(REG_PREAMBLE_MSB, 0x00); // RegPreambleMSB
    spi_write_rfm69_rt(REG_PREAMBLE_LSB, 0x04); // RegPreambleLSB

    spi_write_rfm69_rt(REG_PA_LEVEL,
                       VAL_PA_PA1_ON | VAL_PA_PA2_ON | VAL_PA_20dB); // RegPaLevel
}
