#include "MotionDetection.h"

MotionDetection::MotionDetection(){
    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(PWR_MGMT0);
    handler->transfer(0x1F);
    //busy Wait for startup 
    delayMicroseconds(250);
    //set Gyroconfig
    handler->transfer(0x20);
    handler->transfer(0x25);
    //set Gyro Filter
    handler->transfer(0x23);
    handler->transfer(0x37);
    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 | readRegister(ACCEL_DATA_X_LOW);
    result.y = readRegister(ACCEL_DATA_Y_HIGH)<<8 | readRegister(ACCEL_DATA_Y_LOW);
    result.z = readRegister(ACCEL_DATA_Z_HIGH)<<8 | 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 ,uint8_t axis){
    IMUResult measurment1;
    IMUResult measurment2;
    uint count = 0;
    for(uint i = 0;i<20;i++){
        measurment1 = this->getAcceleration();
        delayMicroseconds(10);
        measurment2 = this->getAcceleration();
        if(
            (((axis && xAxis) > 0) && (abs(abs(measurment1.x)-abs(measurment2.x))>threshold)) ||
            (((axis && yAxis) > 0) && (abs(abs(measurment1.y)-abs(measurment2.y))>threshold)) ||
            (((axis && zAxis) > 0) && (abs(abs(measurment1.z)-abs(measurment2.z))>threshold))){
                count++;
        }
        delayMicroseconds(15);
    }
    return (count > 6);
};

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{INT_MAX,INT_MAX};
    }

    //calculates the angle between the two axis, therefore value between 0-90
    int yAngle;
    int xAngle;
    if(reading.z != 0){
        yAngle = atan(float(reading.x)/reading.z)*180/3.1415+0.5;
        xAngle = atan(float(reading.y)/reading.z)*180/3.1415+0.5;
    } else {
        yAngle = 90*(reading.x > 0) - (reading.x < 0);
        xAngle = 90*(reading.y > 0) - (reading.y < 0);
    }

    //shift quadrants depending on signum
        //esp is facing down (normal position)
    if (reading.z > 0){
        if(reading.y < 0){
            xAngle = xAngle+180;
        } else{
            xAngle = -1*(180-xAngle);
        }

        if(reading.x < 0){
            yAngle = yAngle+180;
        } else{
            yAngle = -1*(180-yAngle);
        }
        //yAngle = -1*yAngle-90;
    }
          

    return Orientation{xAngle,yAngle};

};

Direction MotionDetection::getTiltDirection(uint tolerance){
    if (this->getAcceleration().z > 0){
        return Flipped;
    }
    Orientation Rot = this->getTilt();
    Serial.println(Rot.xRotation);
    Serial.println(Rot.xRotation == INT_MAX);
    if ((Rot.xRotation == INT_MAX)){
        return Error;
    }
    if(abs(abs(Rot.xRotation)-abs(Rot.yRotation))>tolerance){
        //determine which axis is more tiltet
        if (abs(Rot.xRotation)>abs(Rot.yRotation)){
            //test if dezibot is tilted left or right
            if (Rot.xRotation > 0){
                return Right;
            } else {
                return Left;
            }
        } else {
            //test if robot is tilted front or back
            if (Rot.yRotation > 0){
                return Front;
            } else {
                return Back;
            }
        }
    } else {
        //dezibot is (with tolerance) leveled
        return Neutral;
    }
};

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));
};

uint8_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;
};