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

}
double 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_Receive(&hspi1, (uint8_t *)(&safety), 2, 0xFF);

    int16_t signedAngle = ((angle[0] << 8 | angle[1]) & 0x3FFF);
    bool bitSet = 0;
    if (angle[0] & 0b01000000){
        signedAngle -= 16384;
        bitSet = 1;
    }
    else{
        bitSet = 0;
    }
    
    double angleDeg =  360.0 / 32768.0 * signedAngle;
    // double angleDeg =  360.0 / 128.0 * ((angle[0] >> 8) & 0x7F);

    
    // disable chip select
    HAL_GPIO_WritePin(this->csPort, this->csPin, GPIO_PIN_SET);

    return angleDeg;

}

