#include "MotionDetection.h"
#include <cmath.h>
#include <limits.h>
MotionDetection::MotionDetection(){//:handler(FSPI){
    handler = new SPIClass(FSPI);
};

void MotionDetection::begin(void){
    pinMode(34,OUTPUT);
    digitalWrite(34,HIGH);
    handler->begin(36,37,35,34);
     
    handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0)); 
    digitalWrite(34,LOW);
    // set Accel and Gyroscop to Low Noise
    handler->transfer(0x1F);
    handler->transfer(0x0F);
    //busy Wait for startup 
    delayMicroseconds(200);
    digitalWrite(34,HIGH);
    handler->endTransaction();
};
void MotionDetection::end(void){
    handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0));
    digitalWrite(34,LOW);
    handler->transfer(cmdWrite(PWR_MGMT0));
    //turn Accel and Gyroscope off
    handler->transfer(0x00);
    digitalWrite(34,HIGH);
    handler->end();
};
IMUResult MotionDetection::getAcceleration(){
    IMUResult result;
    result.x = readRegister(ACCEL_DATA_X_HIGH)<<8;
    result.x |= readRegister(ACCEL_DATA_X_LOW);
    result.y = readRegister(ACCEL_DATA_Y_HIGH)<<8;
    result.y |= readRegister(ACCEL_DATA_Y_LOW);
    result.z = readRegister(ACCEL_DATA_Z_HIGH)<<8;
    result.z |= readRegister(ACCEL_DATA_Z_LOW);
    return result;
};
IMUResult MotionDetection::getRotation(){
    IMUResult result;
    result.x = readRegister(GYRO_DATA_X_HIGH) <<8;
    result.x |= readRegister(GYRO_DATA_X_LOW);
    result.y = readRegister(GYRO_DATA_Y_HIGH)<<8;
    result.y |= readRegister(GYRO_DATA_Y_LOW);
    result.z = readRegister(GYRO_DATA_Z_HIGH)<<8;
    result.z |= readRegister(GYRO_DATA_Z_LOW);
    return result;
};
float MotionDetection::getTemperature(){
    int16_t raw_temperatur = readRegister(REG_TEMP_HIGH)<<8;
    raw_temperatur |= readRegister(REG_TEMP_LOW); 
    return raw_temperatur/128 +25;
};

int8_t MotionDetection::getWhoAmI(){
    return readRegister(WHO_AM_I);    
};

bool MotionDetection::isShaken(uint32_t threshold = defaultShakeThreshold,uint8_t axis = xAxis|yAxis|zAxis){

};

void MotionDetection::calibrateZAxis(uint gforceValue){
    this->gForceCalib = gforceValue;
};

Orientation MotionDetection::getTilt(){
    uint tolerance = 200;
    IMUResult reading = this->getAcceleration();
    bool flipped = reading.z < 0;
    float accelrationStrenght = sqrt(reading.x*reading.x+reading.y*reading.y+reading.z*reading.z);
    //check if reading is valid
    if (abs(accelrationStrenght-this->gForceCalib) > tolerance ){
        //total accelration is not gravitational force, error
        return Orientation{xRotation = UINT_MAX,yRotation = UINT_MAX};
    }

    //calculates the angle between the two axis, therefore value between 0-90
    uint yAngle;
    uint xAngle;
    if(result.z != 0){
        yAngle = atan((result.x)/result.z)+0.5;
        xAngle = atan(float(result.y)/result.z)+0.5;
    } else {
        yAngle = 90;
        xAngle = 90;
    }


    //shift quadrants depending on signum

};

uint8_t MotionDetection::cmdRead(uint8_t reg){
    return (CMD_READ | (reg & ADDR_MASK));
};
uint8_t MotionDetection::cmdWrite(uint8_t reg){
    return (CMD_WRITE | (reg & ADDR_MASK));
};

int8_t MotionDetection::readRegister(uint8_t reg){
    handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0));
    digitalWrite(34,LOW);
    uint8_t result;
    result = handler->transfer(cmdRead(reg));
    result = handler->transfer(0x00);
     digitalWrite(34,HIGH);
    handler->endTransaction(); 
    return result;
};