#include <ch.h>
#include <hal.h>
#include <stdio.h>
#include <stdlib.h>
#include <fix16.h>
#include "control_task.h"
#include "sensor_task.h"
#include "rotation.h"
#include "speedsensor.h"
#include "pid.h"
#include "parameters.h"
#include "motor.h"
#include "ahrs.h"

static Thread *controlThread = 0;
static WORKING_AREA(controlThread_wa, 3500);
volatile unsigned control_updates;
volatile unsigned control_start_time;

volatile static bool manual_ctrl;
volatile static v2d target_velocity;
volatile static qf16 smoothed_rotation;

void control_get_rotation(qf16 *rotation)
{
    chSysLock();
    *rotation = smoothed_rotation;
    chSysUnlock();
}

void control_get_ground_velocity(v2d *velocity)
{
    qf16 rot;
    control_get_rotation(&rot);
    
    // Point at the bottom of the sphere is in contact with ground.
    // By calculating its rotation, we get the speed of sphere relative
    // to ground. This is only accurate for small rotations.
    //
    // Sphere radius is 5 cm.
    v3d point = {0, 0, F16(-5)};
    qf16_rotate(&point, &rot, &point);
    
    // cm/10ms equals m/s.
    velocity->x = -point.x;
    velocity->y = -point.y;
}

void control_set_target(v2d *velocity)
{
    chSysLock();
    target_velocity = *velocity;
    chSysUnlock();
}

void control_set_manual(bool manual)
{
    manual_ctrl = manual;
}

static void update_motor()
{
    static systime_t last_time;
    static pid_state_t state;
    
    v2d current_vel;
    control_get_ground_velocity(&current_vel);
    
    chSysLock();
    v2d target_vel = target_velocity;
    chSysUnlock();
    
    if (target_vel.x == 0 && target_vel.y == 0)
    {
        set_motor_speed(0);
        return;
    }
    
    // Target for motor is dot product
    // between target ground velocity and
    // Y-axis direction vector.
    fix16_t motor_target, motor_current;
    {
        v3d y = {0,F16(1),0};
        qf16 r;
        ahrs_get_orientation(&r);
        qf16_rotate(&y, &r, &y);
        
        v2d y2 = {y.x, y.y};
        v2d_normalize(&y2, &y2);
        
        motor_target = v2d_dot(&target_vel, &y2);
        motor_current = v2d_dot(&current_vel, &y2);
    }
    
    if (last_time == 0)
    {
        last_time = chTimeNow();
        state.prev_measured = motor_current;
    }
    
    pid_config_t config = {
        params.motor_pid_p, params.motor_pid_i, params.motor_pid_d,
        F16(-1.0), F16(1.0),
        params.motor_pid_lowpass
    };
    
    systime_t newtime = chTimeNow();
    systime_t step = newtime - last_time;
    last_time = newtime;
    
    fix16_t output = -pid_step(&state, &config, motor_current, motor_target, fix16_div(step, MS2ST(1000)));
    set_motor_speed(output);
}


static void update_servo()
{
    static systime_t last_time;
    static pid_state_t state;
    static fix16_t filtered_output;
    
    v2d current_vel;
    control_get_ground_velocity(&current_vel);
    
    chSysLock();
    v2d target_vel = target_velocity;
    chSysUnlock();
    
    if (target_vel.x == 0 && target_vel.y == 0)
    {
        set_servo_position(0);
        
        if (chTimeNow() - last_time > MS2ST(500))
            set_servo_power(false);
        
        return;
    }
    
    set_servo_power(true);
    
    // Target for servo is dot product
    // between target ground velocity and
    // X-axis direction vector.
    fix16_t servo_target, servo_current;
    {
        qf16 r;
        ahrs_get_orientation(&r);
        
        // To resolve right-angle situations, slightly
        // prefer positive Y-axis direction
        v3d y = {params.servo_y_prefer, 0, 0};
        qf16_rotate(&y, &r, &y);
        current_vel.x += y.x;
        current_vel.y += y.y;
        
        v3d x = {F16(1),0,0};
        qf16_rotate(&x, &r, &x);
        
        v2d x2 = {x.x, x.y};
        v2d_normalize(&x2, &x2);
        
        servo_target = v2d_dot(&target_vel, &x2);
        servo_current = v2d_dot(&current_vel, &x2);
    }
    
    if (last_time == 0)
    {
        last_time = chTimeNow();
        state.prev_measured = servo_current;
    }
    
    pid_config_t config = {
        params.servo_pid_p, params.servo_pid_i, params.servo_pid_d,
        F16(-1.0), F16(1.0),
        params.servo_pid_lowpass
    };
    
    systime_t newtime = chTimeNow();
    systime_t step = newtime - last_time;
    last_time = newtime;
    
    fix16_t output = pid_step(&state, &config, servo_current, servo_target, fix16_div(step, MS2ST(1000)));
    
    if (fix16_abs(output) < params.servo_output_deadband)
        output = 0;
    
    filtered_output = fix16_mul(output, F16(1) - params.servo_output_lowpass) + fix16_mul(filtered_output, params.servo_output_lowpass);
    set_servo_position(filtered_output);
}

static msg_t control_task(void *arg)
{
    static EventListener el;    
    chEvtRegister(&sensor_data_event, &el, 0);
    
    chRegSetThreadName("CTRL");
    
    while (!chThdShouldTerminate())
    {
        chEvtWaitAny(ALL_EVENTS); // Wait for new sensor data
        
        rotation_update();
        
        // Simple 1:20 smoothing
        qf16 new;
        rotation_get(&new);
        qf16_avg(&new, (qf16*)&smoothed_rotation, &new, F16(0.95));
        
        chSysLock();
        smoothed_rotation = new;
        chSysUnlock();
        
        if (!manual_ctrl)
        {
            update_motor();
            update_servo();
        }
        
        control_updates++;
    }
    
    return 0;
}

void control_start()
{
    control_updates = 0;
    control_start_time = chTimeNow();
    controlThread = chThdCreateStatic(controlThread_wa, sizeof(controlThread_wa), NORMALPRIO - 10, control_task, NULL);
}

void control_stop()
{
    chThdTerminate(controlThread);
}