#include "Integrator.h"

#pragma GCC optimize ("O3")

namespace maneuvers{

Integrator::Integrator() : state(0), last_time(0), primed(false) { }

void Integrator::integrate(double dy){
    unsigned long current = micros();
    double difference = (current - last_time) / 1000000.0;
    
    if(primed){
        state += (dy*difference);
    }else{
        primed = true;
    }
    last_time = current;

}

double Integrator::getResult() const{
    return state;
}

void Integrator::set(double y){
    state = y;
}

GyroIntegrators gyroIntegrators;

#define GYRO_ZERO_ACCEL_TOLERANCE .05
#define GYRO_CUTOFFS 5

double adjustUsingAccel(double current, double target){
    double remainder = fmod(current - target, 360);
    if(current >= 0){
      remainder = fabs(remainder) > 180 ? -(360 - remainder) : remainder;
    }else{
      remainder = fabs(remainder) > 180 ? (360 + remainder) : remainder;
    }
    return current - remainder;
}

double getNeck(){
    return gyroIntegrators.neck.getResult();
}

double getChin(){
    return gyroIntegrators.chin.getResult();
}

double getSide(){
    return gyroIntegrators.side.getResult();
}

void updateGyroPositions(){
    Measurement mg = (*getGyroData)();
    
    if(fabs(mg.front_facing) < GYRO_CUTOFFS)
        mg.front_facing = 0;
    if(fabs(mg.right_facing) < GYRO_CUTOFFS)
        mg.right_facing = 0;
    if(fabs(mg.up_facing) < GYRO_CUTOFFS)
        mg.up_facing = 0;
        
    gyroIntegrators.side.integrate(mg.front_facing);
    gyroIntegrators.neck.integrate(mg.up_facing);
    gyroIntegrators.chin.integrate(mg.right_facing);
    
    Measurement ma = (*getAccelData)();
    
    static uint8_t zero_counter = 0;
    
    if(zero_counter > 10){
        if(fabs(sqrt(pow(ma.front_facing, 2) + pow(ma.up_facing, 2) + pow(ma.right_facing, 2)) - 1.0) < GYRO_ZERO_ACCEL_TOLERANCE){
            // If an axis reads near 1.0, that means we can zero
            // Remember that a positive G vector registers opposite on an axis (i.e. x axis is collinear with G, it registers negative)
            if(fabs(fabs(ma.front_facing) - 1.0) < GYRO_ZERO_ACCEL_TOLERANCE){
                if(ma.front_facing < 0){ //-G is in direction of front facing axis, so they are laying face down
                    //Serial.println("User is laying face down");
                    // Modify the right facing integrator to be -90
                    gyroIntegrators.neck.set(adjustUsingAccel(getNeck(), 0));
                    gyroIntegrators.chin.set(adjustUsingAccel(getChin(), -90));
                }else{ //-G is in opposite direction of front facing axis, so they are laying on their back
                    //Serial.println("User is laying on their back");
                    // Modify the right facing integrator to be 90
                    gyroIntegrators.neck.set(adjustUsingAccel(getNeck(), 0));
                    gyroIntegrators.chin.set(adjustUsingAccel(getChin(), 90));
                }
            }else if(fabs(fabs(ma.right_facing) - 1.0) < GYRO_ZERO_ACCEL_TOLERANCE){
                if(ma.right_facing < 0){ //-G is in direction of right facing axis, so they are laying on their right side
                    //Serial.println("User is laying on right side");
                    //90
                    gyroIntegrators.neck.set(adjustUsingAccel(getNeck(), 0));
                    gyroIntegrators.side.set(adjustUsingAccel(getSide(), 90));
                }else{ //-G is opposite to the right facing axis, so they are laying on their left side
                    //Serial.println("User is laying on left side");
                    //-90
                    gyroIntegrators.neck.set(adjustUsingAccel(getNeck(), 0));
                    gyroIntegrators.side.set(adjustUsingAccel(getSide(), -90));
                }
            }else if(fabs(fabs(ma.up_facing) - 1.0) < GYRO_ZERO_ACCEL_TOLERANCE){
                if(ma.up_facing < 0){ //User is literallly upside down, so -G is pointing in the direction of the front facing axis
                    //Serial.println("User is literally upside down");
                    gyroIntegrators.side.set(adjustUsingAccel(getSide(), 180));
                    gyroIntegrators.chin.set(adjustUsingAccel(getChin(), 180));
                }else{ //User is sitting up, so -G is pointing opposite of the up facing axis
                    //Serial.println("User is sitting up");
                    gyroIntegrators.side.set(adjustUsingAccel(getSide(), 0));
                    gyroIntegrators.chin.set(adjustUsingAccel(getChin(), 0));
                }
            }
        }
        
        zero_counter = 0;
    }else{
        zero_counter++;
    }
    
    
}

#undef GYRO_ZERO_ACCEL_TOLERANCE

}