#include "rotation.h"
#include "ahrs.h"
#include "speedsensor.h"
#include <ch.h>

static qf16 rot_per_10ms;
static qf16 old_to_enu;
static systime_t previous_time;

void rotation_get(qf16 *q)
{
    chSysLock();
    *q = rot_per_10ms;
    chSysUnlock();
}

void rotation_update()
{
    // Calculate the rotation of the platform since previous timestep.
    // We do this by taking the old_to_enu and new_to_enu rotations
    // and subtracting the latter from the former. The result has
    // rotation axis equivalent in old and new coordinates, which
    // we then convert to ENU.
    qf16 platform_rotation;
    {
        qf16 old_to_new, new_to_enu, enu_to_new;
        
        systime_t timenow = chTimeNow();
        ahrs_get_orientation(&new_to_enu);
        
        // Subtract the rotations
        qf16_conj(&enu_to_new, &new_to_enu);
        qf16_mul(&old_to_new, &enu_to_new, &old_to_enu);
        
        // Convert rotation axis to ENU
        v3d axis;
        qf16_to_v3d(&axis, &old_to_new);
        qf16_rotate(&axis, &new_to_enu, &axis);
        qf16_from_v3d(&old_to_new, &axis, old_to_new.a);
        
        // Scale by time difference
        systime_t ms = (timenow - previous_time) * 1000 / CH_FREQUENCY;
        qf16_pow(&platform_rotation, &old_to_new, fix16_div(10, ms));
        
        old_to_enu = new_to_enu;
        previous_time = timenow;
    }

    // Calculate the rotation around the motor axis.
    qf16 shell_rotation;
    {
        v3d motor_axis = {F16(1), 0, 0};
        qf16_rotate(&motor_axis, &old_to_enu, &motor_axis);
        
        fix16_t rot_per_10ms = -speedsensor_rad_per_sec / 100;
        qf16_from_axis_angle(&shell_rotation, &motor_axis, rot_per_10ms);
    }
    
    qf16 result;
    qf16_mul(&result, &shell_rotation, &platform_rotation);
    
    chSysLock();
    rot_per_10ms = result;
    chSysUnlock();
}