/*
* M034.h - Library for M034
* Created by brucker 22.03.2023
*/

#include "M034.h"                       // classes and functions for M034

/*
* Nanotec absolut position encoder
* SSI format length: 51 bit
* 16 startbits, all always zero
* 16 multiturn bits, MSB first
* 17 singleturn bits, MSB first
* 1 bit "D1", always zero
* 1 bit "D2", Error if 0
* SPI read 7 bytes = 56 bit
* convert read data:
* shift relevant bits to the right position, mask not relevant bits and link bytes
*/

NanotecAbsolutEncoder::NanotecAbsolutEncoder(SPIClass *SPIPort){   // constructor
  _SPI = SPIPort;
}

void NanotecAbsolutEncoder::readPosition(){ // read position from encoder

    noInterrupts();
    for(uint8_t i=0; i<7; i++){             // read 7 bytes
        _readBytes[i] = _SPI->transfer (0); // store in array
    }
    interrupts();
    delayMicroseconds(10);                   // time out of encoder is typ. 7µs
}

uint16_t NanotecAbsolutEncoder::getMultiturn(){
    _multiturn  = (uint16_t) _readBytes[2] << 9;    // convert and return multiturn
    _multiturn |= (uint16_t) _readBytes[3] << 1;
    _multiturn |= (uint16_t) _readBytes[4] >> 7;
    return _multiturn;
}

uint32_t NanotecAbsolutEncoder::getSingleturn(){    // convert and return singelturn
    _singleturn = (uint32_t) (_readBytes[4] & 0x7F) << 10;
    _singleturn |= (uint32_t) _readBytes[5] << 2;
    _singleturn |= (uint32_t) _readBytes[6] >> 6;
    return _singleturn;
}

uint8_t NanotecAbsolutEncoder::getStatus(){         // check if encoder or transmission error exists
    _startbits  = (uint16_t) _readBytes[0] << 9;    // convert startbits
    _startbits |= (uint16_t) _readBytes[1] << 1;
    _startbits |= (uint16_t) _readBytes[2] >> 7;
    _status = (_readBytes[6] >> 4) & 0x03;          // convert status
    if(_status == 1 & _startbits == 0){
        return 0;                                   // return without error
    }
    else return 1;                                  // return with error
}
