#include "ahrs.h"
#include "sensor_task.h"
#include "parameters.h"
#include <ch.h>

void ahrs_get_sensor_orientation(qf16 *result)
{
    v3d acc, mag;
    sensor_get_mag(&mag);
    sensor_get_acc(&acc);
    
    // First, get most of the orientation based on the
    // accelerometer readings.
    // We assume that horizontal acceleration is small.
    // Then the reading should be only normal force, which
    // in ENU is (0, 0, 1).
    {
        v3d normal_force = {0, 0, F16(1)};
        
        // http://stackoverflow.com/a/1171995/914716
        fix16_t dot = v3d_dot(&acc, &normal_force);
        v3d cross;
        v3d_cross(&cross, &acc, &normal_force);
        qf16_from_v3d(result, &cross, v3d_norm(&acc) + dot);
        
        qf16_normalize(result, result);
    }
        
    // Then get the azimuth from magnetometer readings.
    // Because magnetic inclination and field strength
    // is unknown, we only care about the horizontal
    // components of the reading.
    qf16_rotate(&mag, result, &mag);
    
    fix16_t angle = -fix16_atan2(mag.y, mag.x);
    angle += F16(1.57079); // 90 degrees (north in ENU is pos. y axis).
    
    {
        qf16 rotation;
        v3d axis = {0, 0, F16(1)};
        qf16_from_axis_angle(&rotation, &axis, angle);
        qf16_mul(result, &rotation, result);
    }
}

void ahrs_get_orientation(qf16 *result)
{
    ahrs_get_sensor_orientation(result);

#ifdef SPHERE_RX
    // 180° around Y axis
    qf16 base_to_sensor = {0, 0, F16(1), 0};
    qf16_mul(result, result, &base_to_sensor);
#endif
    
#ifdef SPHERE_TX
    // 90° around X axis
    qf16 base_to_sensor = {F16(0.70711), F16(0.70711), 0, 0}; 
    qf16_mul(result, result, &base_to_sensor);
#endif
}