#include "TLE5012.hpp"
#include "stm32g0xx_hal.h"
#include "stm32g0xx_hal_gpio.h"
#include "stm32g0xx_hal_spi.h"
#include <stdint.h>

Tle5012::Tle5012(GPIO_TypeDef * csPort, uint16_t csPin,
                 SPI_HandleTypeDef * spiHandler) {
    this->csPort = csPort;
    this->csPin = csPin;
    this->spiHandler = spiHandler;
}
uint16_t Tle5012::getAngel() {
    // enable chip select
    HAL_GPIO_WritePin(this->csPort, this->csPin, GPIO_PIN_RESET);

    // uint16_t command = 0b1000000000100001;
    uint16_t command = RW | AVAL | SAFETY;

    uint8_t formatedCommand[6] = {0};
    formatedCommand[0] = (uint8_t)(command >> 8);
    formatedCommand[1] = (uint8_t)command;

    uint8_t angle[6] = {0};
    uint16_t safety;

    HAL_SPI_Transmit(this->spiHandler, formatedCommand, 2, 100);

    HAL_SPI_Receive(this->spiHandler, angle, 4, 0xFFFF);
    // HAL_SPI_TransmitReceive(this->spiHandler, formatedCommand, angle, 6, 1);
    // HAL_SPI_Receive(&hspi1, (uint8_t *)(&safety), 2, 0xFF);
    HAL_GPIO_WritePin(this->csPort, this->csPin, GPIO_PIN_SET);

    int16_t signedAngle = ((angle[0] << 8 | angle[1]) & 0x3FFF);
    bool bitSet = 0;
    if (angle[0] & 0b01000000) {
        signedAngle -= 16384;
        bitSet = 1;
    }
    else {
        bitSet = 0;
    }

    // normalize range
    uint16_t normalizedValue;
    if (signedAngle < 0) {
        normalizedValue = -signedAngle;
    }
    else {
        normalizedValue = 16383 + (16383 - signedAngle);
    }

    // double angleDeg = 360.0 / 32768.0 * signedAngle;
    // double angleDeg =  360.0 / 128.0 * ((angle[0] >> 8) & 0x7F);

    // disable chip select

    return normalizedValue;
}