#include "adxl345.hpp"

ADXL345::ADXL345(bool channel):channel(channel){
    // SPI Mode 3 - CPOL = 1, CPHA = 1
    wiringPiSPISetupMode(channel, 500000, 3);
    // Power-up
    Write(registers::POWER_CTL, 0b00001000);
    // Maximal data output rate
    Write(registers::BW_RATE, 0b00001010);

    // Check connection by reading constant value of chip ID
    if(ID() != 0xe5){
        cout << "Cannot establish connection to accelerometer" << endl;
    }
}

uint8_t ADXL345::ID(){
    return Read(registers::DEVID);
}

array<int, 3> ADXL345::Trim_offsets(){
    // Zero offset
    Write(registers::OFSX, 0);
    Write(registers::OFSY, 0);
    Write(registers::OFSZ, 0);

    // Acquire current values
    usleep(100*1000);
    auto axis_values = Raw_values();

    // Calculate trims, Only acceleration of 1g should be on Z axis
    int X_trim = -(round(axis_values[0]/4.0));
    int Y_trim = -(round(axis_values[1]/4.0));
    int Z_trim = -(round((axis_values[2]-255)/4.0));

    // Write new trim values to registers
    Write(registers::OFSX, X_trim);
    Write(registers::OFSY, Y_trim);
    Write(registers::OFSZ, Z_trim);

    // Wait for propagation
    usleep(100*1000);

    return {X_trim, Y_trim, Z_trim};
}

void ADXL345::Trim_offsets(array<int8_t, 3> axis_offsets){
    Write(registers::OFSX, axis_offsets[0]);
    Write(registers::OFSY, axis_offsets[1]);
    Write(registers::OFSZ, axis_offsets[2]);

    // Wait for propagation
    usleep(100*1000);
}

uint8_t ADXL345::Read(registers address){
    // 0x80 is read command
    vector<uint8_t> transfer_data = {static_cast<uint8_t>(static_cast<uint8_t>(address) | 0x80), dummy_packet};
    wiringPiSPIDataRW(channel, transfer_data.data(), 2);
    return transfer_data[1];
}

vector<uint8_t> ADXL345::Read_sequence(registers address, short length){
    vector<uint8_t> transfer_data( length + 1, dummy_packet);
    // Insert address to first position in data transfer, rest if filled with dummy bytes
    transfer_data[0] = static_cast<uint8_t>(address) | 0xc0;
    wiringPiSPIDataRW(channel, transfer_data.data(), length + 1);
    // Remove first byte (address) from data
    transfer_data.erase(transfer_data.begin());
    return transfer_data;
}

void ADXL345::Write(registers address, uint8_t data){
    vector<uint8_t> transfer_data = {static_cast<uint8_t>(address), data};
    wiringPiSPIDataRW(channel, transfer_data.data(), 2);
}

array<int16_t, 3> ADXL345::Raw_values(){
    vector<uint8_t> register_values = Read_sequence(registers::DATAX0, 6);
    array<int16_t, 3> axis_values;
    // Lower register represents LSB of value
    axis_values[0] = register_values[0] | (register_values[1] << 8);
    axis_values[1] = register_values[2] | (register_values[3] << 8);
    axis_values[2] = register_values[4] | (register_values[5] << 8);
    return axis_values;
}

double ADXL345::Inclination(){
    auto axis_values = Raw_values();
    // Resulting value is angle of IC package to horizontal plane
    double angle = atan(static_cast<double>(axis_values[1])/axis_values[2]) * (180 / numbers::pi);
    // Calculate absolute tilt based on quadrants from sensor values
    if((axis_values[1] < 0) and (axis_values[2] >= 0)){
        angle *= -1;
    } else if ((axis_values[1] < 0) and (axis_values[2] < 0)){
        angle = 180 - angle;
    } else if ((axis_values[1] >= 0) and (axis_values[2] < 0)){
        angle = -angle + 180;
    } else if ((axis_values[1] > 0) and (axis_values[2] >= 0)){
        angle = 360 - angle;
    }
    // Calculate inverse angle to corespond with clockwise rotation
    return - angle + 360;
}
