#include "pico/stdlib.h"
#include "pico.h"
#include "hardware/uart.h"

#include "tmc2209.hpp"

TMC2209_UART::TMC2209_UART(
    uart_inst_t *uart_inst, 
    uint8_t node_address, 
    uint baudrate, 
    uint tx_pin, 
    uint rx_pin
) {
    this->uart_inst = uart_inst;
    this->node_address = node_address & 0b11;
    this->baudrate = baudrate;
    this->tx_pin = tx_pin;
    this->rx_pin = rx_pin;
}

void TMC2209_UART::init() {
    gpio_set_function(this->tx_pin, UART_FUNCSEL_NUM(this->uart_inst, this->tx_pin));
    gpio_set_function(this->rx_pin, UART_FUNCSEL_NUM(this->uart_inst, this->rx_pin));
    
    // UART format: 8 data bits, 1 stop bit, no parity
    // -> https://www.analog.com/media/en/technical-documentation/data-sheets/TMC2209_datasheet_rev1.09.pdf
    uart_set_format(this->uart_inst, 8, 1, UART_PARITY_NONE);

    // Clear to Send (CTS) and Request to Send (RTS) are not used
    // (This is for automaticlly telling the other side that it is ready to receive/send data)
    uart_set_hw_flow(this->uart_inst, false, false);

    uart_set_fifo_enabled(this->uart_inst, true);

    uart_init(this->uart_inst, this->baudrate);
}

uint8_t TMC2209_UART::calc_crc8_atm(uint8_t *data, uint8_t size) {
    int i,j;
    uint8_t crc = 0; // CRC located in last byte of message
    uint8_t currentByte;

    for (i=0; i<size; i++) {    // Execute for all bytes of a message
        currentByte = data[i];  // Retrieve a byte to be sent from Array

        for (j=0; j<8; j++) {
            if ((crc >> 7) ^ (currentByte & 0x01)) { // update CRC based result of XOR operation
                crc = (crc << 1) ^ 0x07;
            } else {
                crc = (crc << 1);
            }
            currentByte = currentByte >> 1;
        } // for CRC bit
    } // for message byte

    return crc;
}

uint32_t TMC2209_UART::read(uint8_t address) {
    uint8_t data[4] = {
        0b01010101, // Sync byte
        this->node_address, // Node address
        (address & (uint8_t)(0x7F)), // Register address, last bit is 0 for read
        0
    };

    data[3] = this->calc_crc8_atm(data, 3);

    uart_write_blocking(this->uart_inst, data, 4);

    uint8_t response[12];
    uart_read_blocking(this->uart_inst, response, 12);

    if(response[11] != calc_crc8_atm(&response[4], 7)) {
        this->crc_error_flag = true;
        return 0;
    } else {
        return *((uint32_t*) &response[7]);
    }
}