#include "AS5040.h"

AS5040Sensor::AS5040Sensor(uint16_t cs, uint16_t mosi, uint16_t miso, uint16_t sck) {
    cs_ = cs;
    mosi_ = mosi;
    miso_ = miso;
    sck_ = sck;
}
void AS5040Sensor::init(SPIClass *_spi) {
    spi = _spi;
    // 1MHz clock (AMS should be able to accept up to 10MHz)
    settings = SPISettings(1000000, MSBFIRST, SPI_MODE0);
    //setup pins
    pinMode(cs_, OUTPUT);
    pinMode(miso_, INPUT);
    pinMode(mosi_, OUTPUT);
    pinMode(sck_, OUTPUT);
    spi->setMISO(miso_);
    spi->setMOSI(mosi_);
    spi->setSCLK(sck_);
    spi->begin();
    this->Sensor::init(); // call base class init
}

float AS5040Sensor::getSensorAngle() {
    //SPI - begin transaction
    spi->beginTransaction(settings);
    //Send the command
    digitalWrite(cs_, LOW);
    uint16_t data = spi->transfer16(0x3FFF);
    digitalWrite(cs_,HIGH);
//SPI - end transaction
    spi->endTransaction();
    data = (data) >> 5;
    return (float)data * _2PI / cpr;
}