#include "c_controller.h"
#include "control_algorithm.c"
#include "computation_graph.c"
double c_controller(int vrpn_id, double vrpn_ts, double set_x, double set_y, double set_z, double set_yaw,
        double cur_x, double cur_y, double cur_z,
        double cur_phi, double cur_theta, double cur_psi,
        double cur_phi_d, double cur_theta_d, double cur_psi_d,
        double *z_out, double *y_out, double *x_out, double *yaw_out,
        double* pid_y_out, double* pid_roll_out) {
    
    static log_t log_struct;
    static user_input_t user_input_struct;
    static sensor_t sensor_struct;
    static setpoint_t setpoint_struct;
    static parameter_t ps;
    static user_defined_t user_defined_struct;
    static actuator_command_t actuator_struct;
    static modular_structs_t structs;
    
    static int last_vrpn_id = -1;
    static int initialized = 0;
    if (!initialized) {
        control_algorithm_init(&ps);
        sensor_struct.currentQuadPosition.packetId = vrpn_id;
        initialized = 1;
    }
    
    // Check VRPN update
    if (vrpn_id != last_vrpn_id) {
        user_input_struct.locationFresh = 1;
        int vrpn_ts_packets = (vrpn_ts / 0.01) + 0.5; // + 0.5 to round
        sensor_struct.currentQuadPosition.packetId += vrpn_ts_packets;
        // VRPN values
        sensor_struct.currentQuadPosition.x_pos = cur_x;
        sensor_struct.currentQuadPosition.y_pos = cur_y;
        sensor_struct.currentQuadPosition.alt_pos = cur_z;
        sensor_struct.currentQuadPosition.pitch = cur_theta;
        sensor_struct.currentQuadPosition.roll = cur_phi;
        sensor_struct.currentQuadPosition.yaw = cur_psi;
        last_vrpn_id = vrpn_id;
    }
    
    // Setpoints
    /*
    setpoint_struct.desiredQuadPosition.x_pos = set_x;
    setpoint_struct.desiredQuadPosition.y_pos = set_y;
    setpoint_struct.desiredQuadPosition.alt_pos = set_z;
    setpoint_struct.desiredQuadPosition.yaw = set_yaw;
    */
    int i;
    
    // Sensors
    sensor_struct.pitch_angle_filtered = cur_theta;
    sensor_struct.roll_angle_filtered = cur_phi;
    sensor_struct.theta_dot = cur_theta_d;
    sensor_struct.phi_dot = cur_phi_d;
    sensor_struct.psi_dot = cur_psi_d;
    
    control_algorithm(&log_struct, &user_input_struct, &sensor_struct, &setpoint_struct, &ps, &user_defined_struct, &actuator_struct, NULL);
    
    *z_out = graph_get_output(ps.graph, ps.alt_pid, PID_CORRECTION);
    *y_out = graph_get_output(ps.graph, ps.roll_r_pid, PID_CORRECTION);
    *x_out = graph_get_output(ps.graph, ps.pitch_r_pid, PID_CORRECTION);
    *yaw_out = graph_get_output(ps.graph, ps.yaw_r_pid, PID_CORRECTION);
    
    *pid_y_out = graph_get_output(ps.graph, ps.y_pos_pid, PID_CORRECTION);
    *pid_roll_out = graph_get_output(ps.graph, ps.roll_pid, PID_CORRECTION);
    return 0;
}

int read_flap(int flap) { return AUTO_FLIGHT_MODE; }
float get_last_loop_time() { return 0.005; }