#include "AS5045.h"


MagneticSensorAS5045::MagneticSensorAS5045(uint16_t as5040_cs, uint16_t as5040_mosi, uint16_t as5040_miso,
                                           uint16_t as5040_sclk): AS5040_CS_(as5040_cs),
                                                                  AS5040_MOSI_(as5040_mosi),
                                                                  AS5040_MISO_(as5040_miso),
                                                                  AS5040_SCLK_(as5040_sclk),
                                                                  spi(nullptr),
                                                                  settings(AS5145SSISettings) {
}

MagneticSensorAS5045::~MagneticSensorAS5045() = default;

auto MagneticSensorAS5045::init(SPIClass *_spi) -> void {
    this->spi = _spi;
    settings = AS5145SSISettings;
    pinMode(AS5040_CS_, OUTPUT);
    pinMode(AS5040_MISO_, INPUT);
    pinMode(AS5040_MOSI_, OUTPUT);
    pinMode(AS5040_SCLK_, OUTPUT);

    spi->setMISO(AS5040_MISO_);
    spi->setMOSI(AS5040_MOSI_);
    spi->setSCLK(AS5040_SCLK_);
    spi->begin();
    this->Sensor::init();
}

float MagneticSensorAS5045::getSensorAngle() {
    float angle_data = readRawAngleSSI();
    angle_data = (static_cast<float>(angle_data) / AS5045_CPR) * _2PI;
    return angle_data;
}

uint16_t MagneticSensorAS5045::readRawAngleSSI() const {
    spi->beginTransaction(settings);
    digitalWrite(AS5040_CS_, LOW);
    uint16_t value = spi->transfer16(0x0000);
    digitalWrite(AS5040_CS_, HIGH);
    spi->endTransaction();
    delayMicroseconds(30);
    return (value >> 3) & 0x1FFF;  // TODO(vanyabeat): Add error checking MAGNETIC OWERFLOW and etc.
}