#include "control.h"
#include "rf_task.h"
#include "ahrs_task.h"
#include "motor.h"
#include <ch.h>
#include <stdlib.h>

// Return a + n * b where n is selected so that -b/2 <= a < b/2
static fix16_t centered_mod(fix16_t a, fix16_t b)
{
    a = a % b;
    
    if (a < -b / 2)
        a += b;
    
    if (a >= b / 2)
        a -= b;
    
    return a;
}

// Perform one PID controller loop.
// 'measured' is the output from the process that is to be controlled
// 'goal' is the value that 'measured' should ideally have.
// 'timestep' is the time passed after previous call in seconds.
static fix16_t pid_controller(pid_controller_t *data, fix16_t measured, fix16_t goal, fix16_t timestep)
{
    // Compute error (proportional)
    fix16_t error = measured - goal;
    
    // If dealing with mod-360° angles, fix any wrapovers
    if (data->modulus)
    {
        error = centered_mod(error, data->modulus);
        
        fix16_t raw_delta = measured - data->previous_filtered;
        if (raw_delta > data->modulus / 2)
            data->previous_filtered += data->modulus;
        
        if (raw_delta < -data->modulus / 2)
            data->previous_filtered -= data->modulus;
    }
    
    // Compute derivative of measured value
    fix16_t filtered = fix16_mul(measured, fix16_one - data->lowpass) +
                       fix16_mul(data->previous_filtered, data->lowpass);
    fix16_t delta = filtered - data->previous_filtered;
    data->previous_filtered = filtered;
    
    // Compute output
    fix16_t output = fix16_mul(data->P, error)
                    + fix16_mul(data->I, data->current_integral)
                    + fix16_mul(data->D, fix16_div(delta, timestep));
    
    // Avoid integral windup by not updating the integral when output
    // is outside range.
    if (output < data->min_output)
        output = data->min_output;
    else if (output > data->max_output)
        output = data->max_output;
    else
        data->current_integral += fix16_mul(error, timestep);
    
    return output;
}

pid_controller_t pitch_ctrl = {
    200, 200, 30,
    (fix16_t)(65536 * 0.9), (fix16_t)(65536 * 360),
    -65536,
    65536,
    0,
    0
};

pid_controller_t heading_ctrl = {
    800, 0, 0,
    (fix16_t)(65536 * 0.8), (fix16_t)(65536 * 360),
    -65536,
    65536,
    0,
    0
};


static fix16_t deg2rad(fix16_t degs)
{
    return fix16_mul(degs, fix16_from_float(3.141596 / 180.0));
}

volatile bool manual_ctrl = false;
int32_t CONTROL_ZERO_PITCH = -14;
static systime_t previous_time_on = 0;
static systime_t previous_time = 0;

void run_control()
{
    if (manual_ctrl)
    {
        previous_time = 0;
        return;
    }
    
    systime_t time_now = chTimeNow();
    if (previous_time == 0)
        previous_time = previous_time_on = time_now;
    
    fix16_t timestep = fix16_div(time_now - previous_time, 1000);
    previous_time = time_now;
    
    // 1) The servo position control
    fix16_t target_heading = fix16_from_int(360) * rf_heading / 63;
    fix16_t new_servo_position;
    
    fix16_t current_heading = ahrs_heading;
    
    if (rf_speed == 0)
    {
        // Stop
        new_servo_position = 0;
    }
    else if (rf_speed == 1)
    {
        // Hold current speed & direction
        new_servo_position = servo_position;
    }
    else
    {
        // Control speed & direction
        new_servo_position = pid_controller(&heading_ctrl, current_heading, target_heading, timestep);
    }
    
    set_servo_position(new_servo_position);
    
    if (rf_speed != 0)
    {
        previous_time_on = chTimeNow();
        set_servo_power(true);
    }
    else if (chTimeNow() - previous_time_on > MS2ST(1000))
    {
        set_servo_power(false);
    }
    
    // 2) The motor speed control
    
    fix16_t target_pitch = fix16_from_int(90) * rf_speed / 63;
    fix16_t current_pitch = ahrs_pitch - fix16_from_int(CONTROL_ZERO_PITCH);
    fix16_t new_motor_speed;
    
    fix16_t roll_limit = fix16_from_int(70);
    if (ahrs_roll > roll_limit || ahrs_roll < -roll_limit || rf_speed == 0)
    {
        new_motor_speed = 0;
        pitch_ctrl.current_integral = 0;
    }
    else
    {
        // Don't let the integral term spin us over when the ball suddenly
        // stops.
        if (current_pitch > fix16_from_int(120)
            && pitch_ctrl.current_integral < 0)
            pitch_ctrl.current_integral = 0;
        
        // This is a combination of open-loop non-linear estimate based on
        // the desired angle and closed-loop PID control.
        new_motor_speed = (fix16_one - fix16_cos(deg2rad(target_pitch))) / 4;
        new_motor_speed -= pid_controller(&pitch_ctrl, current_pitch, target_pitch, timestep);
    }
    
    set_motor_speed(new_motor_speed);
}