/*
 * control_algorithm.c
 *
 *  Created on: Feb 20, 2016
 *      Author: ucart
 */

// This implemented modular quadrotor software implements a PID control algorithm

#include "control_algorithm.h"
#include "graph_blocks.h"
#include "PID.h"
#include "util.h"
#include "timer.h"

#define USE_LIDAR
#define USE_OF
#define USE_MAG_YAW
#define MAX_VALID_LIDAR (10.0) // Maximum valid distance to read from LiDAR to update

//10 degrees
#define ANGLE_CLAMP		(0.1745)

#define PX4FLOW_QUAL_MIN			(100)

#define ROLL_PITCH_MAX_ANGLE 0.35 // 20 degrees
#define PWM_DIFF_BOUNDS 20000
#define VRPN_REFRESH_TIME 0.01f // 10ms

void connect_autonomous(parameter_t* ps) {
	struct computation_graph* graph = ps->graph;
	//graph_set_source(graph, ps->pitch_pid, PID_SETPOINT, ps->x_pos_pid, PID_CORRECTION);
	//graph_set_source(graph, ps->roll_pid, PID_SETPOINT, ps->y_pos_pid, PID_CORRECTION);
	graph_set_source(graph, ps->pitch_pid, PID_SETPOINT, ps->yaw_correction, ROT_OUT_X);
	graph_set_source(graph, ps->roll_pid, PID_SETPOINT, ps->yaw_correction, ROT_OUT_Y);
	graph_set_source(graph, ps->yaw_r_pid, PID_SETPOINT, ps->yaw_pid, PID_CORRECTION);
#ifdef USE_OF
	//graph_set_source(graph, ps->mixer, MIXER_THROTTLE, ps->rc_throttle, ADD_SUM);
	graph_set_source(graph, ps->mixer, MIXER_THROTTLE, ps->throttle_trim_add, ADD_SUM);
#else
	graph_set_source(graph, ps->mixer, MIXER_THROTTLE, ps->throttle_trim_add, ADD_SUM);
#endif
}

void connect_manual(parameter_t* ps) {
	struct computation_graph* graph = ps->graph;
	graph_set_source(graph, ps->pitch_pid, PID_SETPOINT, ps->rc_pitch, CONST_VAL);
	graph_set_source(graph, ps->roll_pid, PID_SETPOINT, ps->rc_roll, CONST_VAL);
	graph_set_source(graph, ps->mixer, MIXER_THROTTLE, ps->rc_throttle, CONST_VAL);
	graph_set_source(graph, ps->yaw_r_pid, PID_SETPOINT, ps->rc_yaw, CONST_VAL);
}

int control_algorithm_init(parameter_t * ps)
{
    struct computation_graph* graph = create_graph();
    ps->graph = graph;

    // Create all the PID blocks
    ps->roll_pid = graph_add_defined_block(graph, BLOCK_PID, "Roll PID");
    ps->pitch_pid = graph_add_defined_block(graph, BLOCK_PID, "Pitch PID");
    ps->yaw_pid = graph_add_defined_block(graph, BLOCK_PID, "Yaw PID");
    ps->roll_r_pid = graph_add_defined_block(graph, BLOCK_PID, "Roll Rate PID");
    ps->pitch_r_pid = graph_add_defined_block(graph, BLOCK_PID, "Pitch Rate PID");
    ps->yaw_r_pid = graph_add_defined_block(graph, BLOCK_PID, "Yaw Rate PID");
    ps->x_pos_pid = graph_add_defined_block(graph, BLOCK_PID, "X pos PID");
    ps->y_pos_pid = graph_add_defined_block(graph, BLOCK_PID, "Y pos PID");
    ps->alt_pid = graph_add_defined_block(graph, BLOCK_PID, "Altitude PID");
    ps->x_set = graph_add_defined_block(graph, BLOCK_CONSTANT, "X Setpoint"); // ID 9
    ps->y_set = graph_add_defined_block(graph, BLOCK_CONSTANT, "Y Setpoint");
    ps->alt_set = graph_add_defined_block(graph, BLOCK_CONSTANT, "Alt Setpoint");
    ps->yaw_set = graph_add_defined_block(graph, BLOCK_CONSTANT, "Yaw Setpoint");
    ps->throttle_trim = graph_add_defined_block(graph, BLOCK_CONSTANT, "Throttle trim");
    ps->throttle_trim_add = graph_add_defined_block(graph, BLOCK_ADD, "T trim add");

    // Create blocks for sensor inputs
    ps->cur_pitch = graph_add_defined_block(graph, BLOCK_CONSTANT, "Pitch"); // ID 15
    ps->cur_roll = graph_add_defined_block(graph, BLOCK_CONSTANT, "Roll");
    ps->cur_yaw = graph_add_defined_block(graph, BLOCK_CONSTANT, "Yaw");
    ps->lidar = graph_add_defined_block(graph, BLOCK_CONSTANT, "Lidar");
    ps->flow_vel_x = graph_add_defined_block(graph, BLOCK_CONSTANT, "Flow Vel X");
    ps->flow_vel_y = graph_add_defined_block(graph, BLOCK_CONSTANT, "Flow Vel Y");
    ps->flow_quality = graph_add_defined_block(graph, BLOCK_CONSTANT, "Flow Quality");
    ps->flow_distance = graph_add_defined_block(graph, BLOCK_CONSTANT, "Flow Distance");
    // Sensor trims
    ps->pitch_trim = graph_add_defined_block(graph, BLOCK_CONSTANT, "Pitch trim");
    ps->pitch_trim_add = graph_add_defined_block(graph, BLOCK_ADD, "Pitch trim add");

	// Yaw angular velocity PID
    ps->gyro_y = graph_add_defined_block(graph, BLOCK_CONSTANT, "Gyro Y");
    ps->gyro_x = graph_add_defined_block(graph, BLOCK_CONSTANT, "Gyro X");
    ps->gyro_z = graph_add_defined_block(graph, BLOCK_CONSTANT, "Gyro Z");
    ps->clamp_d_pwmP = graph_add_defined_block(graph, BLOCK_BOUNDS, "P PWM Clamp");
    ps->clamp_d_pwmR = graph_add_defined_block(graph, BLOCK_BOUNDS, "R PWM Clamp");
    ps->clamp_d_pwmY = graph_add_defined_block(graph, BLOCK_BOUNDS, "Y PWM Clamp");

    // Create blocks for VRPN data
    ps->vrpn_x = graph_add_defined_block(graph, BLOCK_CONSTANT, "VRPN X");
    ps->vrpn_y = graph_add_defined_block(graph, BLOCK_CONSTANT, "VRPN Y");
    ps->vrpn_alt = graph_add_defined_block(graph, BLOCK_CONSTANT, "VRPN Alt");
    ps->vrpn_pitch = graph_add_defined_block(graph, BLOCK_CONSTANT, "VRPN Pitch");
    ps->vrpn_roll = graph_add_defined_block(graph, BLOCK_CONSTANT, "VRPN Roll");

    // Create blocks for RC controller
    ps->rc_pitch = graph_add_defined_block(graph, BLOCK_CONSTANT, "RC Pitch");
    ps->rc_roll = graph_add_defined_block(graph, BLOCK_CONSTANT, "RC Roll");
    ps->rc_yaw = graph_add_defined_block(graph, BLOCK_CONSTANT, "RC Yaw");
    ps->rc_throttle = graph_add_defined_block(graph, BLOCK_CONSTANT, "RC Throttle");

    // Create blocks for velocity controllers
    ps->x_vel_pid = graph_add_defined_block(graph, BLOCK_PID, "X Vel PID");
    ps->y_vel_pid = graph_add_defined_block(graph, BLOCK_PID, "Y Vel PID");
    ps->x_vel = graph_add_defined_block(graph, BLOCK_PID, "X Vel");
    ps->y_vel = graph_add_defined_block(graph, BLOCK_PID, "Y Vel");
    ps->x_vel_clamp = graph_add_defined_block(graph, BLOCK_BOUNDS, "X Vel Clamp");
    ps->y_vel_clamp = graph_add_defined_block(graph, BLOCK_BOUNDS, "Y vel Clamp");
        // Converts global X/Y to local X/Y
    ps->yaw_correction = graph_add_defined_block(graph, BLOCK_YAW_ROT, "Yaw Correction");

    // Optical Flow
    ps->of_angle_corr = graph_add_defined_block(graph, BLOCK_YAW_ROT, "OF Offset Angle");
    ps->of_integ_x = graph_add_defined_block(graph, BLOCK_INTEGRATOR, "OF Integrate X");
    ps->of_integ_y = graph_add_defined_block(graph, BLOCK_INTEGRATOR, "OF Integrate Y");
    ps->of_trim_x = graph_add_defined_block(graph, BLOCK_CONSTANT, "OF Trim X");
    ps->of_trim_y = graph_add_defined_block(graph, BLOCK_CONSTANT, "OF Trim Y");
    ps->of_trimmed_x = graph_add_defined_block(graph, BLOCK_ADD, "OF X Trim Add");
    ps->of_trimmed_y = graph_add_defined_block(graph, BLOCK_ADD, "OF Y Trim Add");

    // gyroscope z integrator
    ps->gyro_yaw = graph_add_defined_block(graph, BLOCK_INTEGRATOR, "Integrated gyro z");

    //Complementary yaw
    ps->mag_yaw = graph_add_defined_block(graph, BLOCK_CONSTANT, "Mag Yaw");

    ps->mixer = graph_add_defined_block(graph, BLOCK_MIXER, "Signal Mixer");

    ps->angle_time = graph_add_defined_block(graph, BLOCK_CONSTANT, "Ts_IMU");
    ps->pos_time = graph_add_defined_block(graph, BLOCK_CONSTANT, "Ts_VRPN");

    // Useful zero block
    int zero_block = graph_add_defined_block(graph, BLOCK_CONSTANT, "zero");
    graph_set_param_val(graph, zero_block, CONST_SET, 0);

    // Connect pitch PID chain
    // Trims
    graph_set_source(graph, ps->pitch_trim_add, ADD_SUMMAND1, ps->pitch_trim, CONST_VAL);
    graph_set_source(graph, ps->pitch_trim_add, ADD_SUMMAND2, ps->cur_pitch, CONST_VAL);
    // Controllers
    graph_set_source(graph, ps->pitch_r_pid, PID_SETPOINT, ps->pitch_pid, PID_CORRECTION);
    graph_set_source(graph, ps->pitch_r_pid, PID_CUR_POINT, ps->gyro_y, CONST_VAL);
    graph_set_source(graph, ps->pitch_r_pid, PID_DT, ps->angle_time, CONST_VAL);
    graph_set_source(graph, ps->pitch_pid, PID_CUR_POINT, ps->pitch_trim_add, ADD_SUM);
    //graph_set_source(graph, ps->pitch_pid, PID_CUR_POINT, ps->vrpn_pitch, CONST_VAL);
    graph_set_source(graph, ps->pitch_pid, PID_DT, ps->angle_time, CONST_VAL);

     // Connect roll PID chain
    graph_set_source(graph, ps->roll_r_pid, PID_SETPOINT, ps->roll_pid, PID_CORRECTION);
    graph_set_source(graph, ps->roll_r_pid, PID_CUR_POINT, ps->gyro_x, CONST_VAL);
    graph_set_source(graph, ps->roll_r_pid, PID_DT, ps->angle_time, CONST_VAL);
    graph_set_source(graph, ps->roll_pid, PID_CUR_POINT, ps->cur_roll, CONST_VAL);
    //graph_set_source(graph, ps->roll_pid, PID_CUR_POINT, ps->vrpn_roll, CONST_VAL);USE_FAKE_YAW
    graph_set_source(graph, ps->roll_pid, PID_DT, ps->angle_time, CONST_VAL);

    // Connect yaw PID chain
    graph_set_source(graph, ps->yaw_r_pid, PID_SETPOINT, ps->rc_yaw, PID_CORRECTION);
    graph_set_source(graph, ps->yaw_r_pid, PID_CUR_POINT, ps->gyro_z, CONST_VAL);
    graph_set_source(graph, ps->yaw_r_pid, PID_DT, ps->angle_time, CONST_VAL);

    graph_set_source(graph, ps->gyro_yaw, INTEGRATE_IN, ps->gyro_z, CONST_VAL);

#ifndef USE_OF
    // X velocity PID
        // Use a PID block to compute the derivative
    graph_set_param_val(graph, ps->x_vel, PID_KD, -1);
    graph_set_source(graph, ps->x_vel, PID_DT, ps->pos_time, CONST_VAL);
    graph_set_source(graph, ps->x_vel, PID_CUR_POINT, ps->vrpn_x, CONST_VAL);
    graph_set_source(graph, ps->x_vel, PID_SETPOINT, zero_block, CONST_VAL);
        // Connect velocity PID itself
    graph_set_source(graph, ps->x_vel_pid, PID_DT, ps->pos_time, CONST_VAL);
    graph_set_source(graph, ps->x_vel_pid, PID_CUR_POINT, ps->x_vel, PID_CORRECTION);
    //graph_set_source(graph, ps->x_vel_pid, PID_CUR_POINT, ps->of_angle_corr, ROT_OUT_X);
    graph_set_source(graph, ps->x_vel_pid, PID_SETPOINT, ps->x_vel_clamp, BOUNDS_OUT);
    graph_set_source(graph, ps->x_vel_clamp, BOUNDS_IN, ps->x_pos_pid, PID_CORRECTION);

    // X/Y global to local conversion
#ifdef USE_MAG_YAW
    graph_set_source(graph, ps->yaw_correction, ROT_YAW, ps->mag_yaw, CONST_VAL);
#else
    graph_set_source(graph, ps->yaw_correction, ROT_YAW, ps->cur_yaw, CONST_VAL);
#endif
    graph_set_source(graph, ps->yaw_correction, ROT_CUR_X, ps->x_vel_pid, PID_CORRECTION);
    graph_set_source(graph, ps->yaw_correction, ROT_CUR_Y, ps->y_vel_pid, PID_CORRECTION);
    

    // Y velocity PID
        // Use a PID block to compute the derivative
    graph_set_param_val(graph, ps->y_vel, PID_KD, -1);
    graph_set_source(graph, ps->y_vel, PID_DT, ps->pos_time, CONST_VAL);
    graph_set_source(graph, ps->y_vel, PID_CUR_POINT, ps->vrpn_y, CONST_VAL);
    graph_set_source(graph, ps->y_vel, PID_SETPOINT, zero_block, CONST_VAL);
        // Connect velocity PID itself
    graph_set_source(graph, ps->y_vel_pid, PID_DT, ps->pos_time, CONST_VAL);
    graph_set_source(graph, ps->y_vel_pid, PID_CUR_POINT, ps->y_vel, PID_CORRECTION);
    //graph_set_source(graph, ps->y_vel_pid, PID_CUR_POINT, ps->of_angle_corr, ROT_OUT_Y);
    graph_set_source(graph, ps->y_vel_pid, PID_SETPOINT, ps->y_vel_clamp, BOUNDS_OUT);
    graph_set_source(graph, ps->y_vel_clamp, BOUNDS_IN, ps->y_pos_pid, PID_CORRECTION);

    // X position
    graph_set_source(graph, ps->x_pos_pid, PID_DT, ps->pos_time, CONST_VAL);
    graph_set_source(graph, ps->x_pos_pid, PID_CUR_POINT, ps->vrpn_x, CONST_VAL);
    //graph_set_source(graph, ps->x_pos_pid, PID_CUR_POINT, ps->of_trimmed_x, CONST_VAL);
    graph_set_source(graph, ps->x_pos_pid, PID_SETPOINT, ps->x_set, CONST_VAL);
    // Y position
    graph_set_source(graph, ps->y_pos_pid, PID_DT, ps->pos_time, CONST_VAL);
    graph_set_source(graph, ps->y_pos_pid, PID_CUR_POINT, ps->vrpn_y, CONST_VAL);
    //graph_set_source(graph, ps->y_pos_pid, PID_CUR_POINT, ps->of_trimmed_y, CONST_VAL);
    graph_set_source(graph, ps->y_pos_pid, PID_SETPOINT, ps->y_set, CONST_VAL);
#endif

#ifdef USE_OF
    // X position
    graph_set_source(graph, ps->x_pos_pid, PID_DT, ps->angle_time, CONST_VAL);
    graph_set_source(graph, ps->x_pos_pid, PID_CUR_POINT, ps->of_trimmed_x, ADD_SUM);
    //graph_set_source(graph, ps->x_pos_pid, PID_CUR_POINT, ps->of_trimmed_x, CONST_VAL);
    graph_set_source(graph, ps->x_pos_pid, PID_SETPOINT, ps->x_set, CONST_VAL);
    // Y position
    graph_set_source(graph, ps->y_pos_pid, PID_DT, ps->angle_time, CONST_VAL);
    graph_set_source(graph, ps->y_pos_pid, PID_CUR_POINT, ps->of_trimmed_y, ADD_SUM);
    //graph_set_source(graph, ps->y_pos_pid, PID_CUR_POINT, ps->of_trimmed_y, CONST_VAL);
    graph_set_source(graph, ps->y_pos_pid, PID_SETPOINT, ps->y_set, CONST_VAL);

    graph_set_source(graph, ps->x_vel_clamp, BOUNDS_IN, ps->x_pos_pid, PID_CORRECTION);
    graph_set_source(graph, ps->y_vel_clamp, BOUNDS_IN, ps->y_pos_pid, PID_CORRECTION);


    // X/Y global to local conversion
#ifdef USE_MAG_YAW
    graph_set_source(graph, ps->yaw_correction, ROT_YAW, ps->mag_yaw, CONST_VAL);
#else
    graph_set_source(graph, ps->yaw_correction, ROT_YAW, ps->cur_yaw, CONST_VAL);
#endif
    graph_set_source(graph, ps->yaw_correction, ROT_CUR_X, ps->x_vel_clamp, PID_CORRECTION);
    graph_set_source(graph, ps->yaw_correction, ROT_CUR_Y, ps->y_vel_clamp, PID_CORRECTION);
#endif

    // Alt autonomous
#ifdef USE_LIDAR
    graph_set_source(graph, ps->alt_pid, PID_DT, ps->angle_time, CONST_VAL);
    graph_set_source(graph, ps->alt_pid, PID_CUR_POINT, ps->lidar, CONST_VAL);
#else
    graph_set_source(graph, ps->alt_pid, PID_DT, ps->pos_time, CONST_VAL);
    graph_set_source(graph, ps->alt_pid, PID_CUR_POINT, ps->vrpn_alt, CONST_VAL);
#endif
    graph_set_source(graph, ps->alt_pid, PID_SETPOINT, ps->alt_set, CONST_VAL);
    graph_set_source(graph, ps->throttle_trim_add, ADD_SUMMAND1, ps->alt_pid, PID_CORRECTION);
    graph_set_source(graph, ps->throttle_trim_add, ADD_SUMMAND2, ps->throttle_trim, CONST_VAL);
    // Yaw angle
#ifdef USE_MAG_YAW
    graph_set_source(graph, ps->yaw_pid, PID_DT, ps->angle_time, CONST_VAL);
    graph_set_source(graph, ps->yaw_pid, PID_CUR_POINT, ps->mag_yaw, CONST_VAL);
#else
    graph_set_source(graph, ps->yaw_pid, PID_DT, ps->pos_time, CONST_VAL);
    graph_set_source(graph, ps->yaw_pid, PID_CUR_POINT, ps->cur_yaw, CONST_VAL);
#endif
    graph_set_source(graph, ps->yaw_pid, PID_SETPOINT, ps->yaw_set, CONST_VAL);

    // Connect PWM clamping blocks
    graph_set_source(graph, ps->clamp_d_pwmP, BOUNDS_IN, ps->pitch_r_pid, PID_CORRECTION);
    graph_set_source(graph, ps->clamp_d_pwmR, BOUNDS_IN, ps->roll_r_pid, PID_CORRECTION);
    graph_set_source(graph, ps->clamp_d_pwmY, BOUNDS_IN, ps->yaw_r_pid, PID_CORRECTION);

    // Connect signal mixer
    //graph_set_source(graph, ps->mixer, MIXER_THROTTLE, ps->rc_throttle, CONST_VAL);
    graph_set_source(graph, ps->mixer, MIXER_PITCH, ps->clamp_d_pwmP, BOUNDS_OUT);
    graph_set_source(graph, ps->mixer, MIXER_ROLL, ps->clamp_d_pwmR, BOUNDS_OUT);
    graph_set_source(graph, ps->mixer, MIXER_YAW, ps->clamp_d_pwmY, BOUNDS_OUT);

    // Connect optical flow
#ifdef USE_MAG_YAW
    graph_set_source(graph, ps->of_angle_corr, ROT_YAW, ps->mag_yaw, ADD_SUM);
#else
    graph_set_source(graph, ps->of_angle_corr, ROT_YAW, ps->cur_yaw, ADD_SUM);
#endif
    graph_set_source(graph, ps->of_angle_corr, ROT_CUR_X, ps->flow_vel_x, CONST_VAL);
    graph_set_source(graph, ps->of_angle_corr, ROT_CUR_Y, ps->flow_vel_y, CONST_VAL);
        // Integration
    graph_set_source(graph, ps->of_integ_x, INTEGRATE_IN, ps->of_angle_corr, ROT_OUT_X);
    graph_set_source(graph, ps->of_integ_x, INTEGRATE_DT, ps->angle_time, CONST_VAL);
    graph_set_source(graph, ps->of_integ_y, INTEGRATE_IN, ps->of_angle_corr, ROT_OUT_Y);
    graph_set_source(graph, ps->of_integ_y, INTEGRATE_DT, ps->angle_time, CONST_VAL);
        // Trim
    graph_set_source(graph, ps->of_trimmed_x, ADD_SUMMAND1, ps->of_integ_x, INTEGRATED);
    graph_set_source(graph, ps->of_trimmed_x, ADD_SUMMAND2, ps->of_trim_x, CONST_VAL);
    graph_set_source(graph, ps->of_trimmed_y, ADD_SUMMAND1, ps->of_integ_y, INTEGRATED);
    graph_set_source(graph, ps->of_trimmed_y, ADD_SUMMAND2, ps->of_trim_y, CONST_VAL);


    // Set pitch PID constants
    graph_set_param_val(graph, ps->pitch_pid, PID_KP, PITCH_ANGLE_KP);
    graph_set_param_val(graph, ps->pitch_pid, PID_KI, PITCH_ANGLE_KI);
    graph_set_param_val(graph, ps->pitch_pid, PID_KD, PITCH_ANGLE_KD);
    graph_set_param_val(graph, ps->pitch_pid, PID_ALPHA, PITCH_ANGLE_ALPHA);
    graph_set_param_val(graph, ps->pitch_r_pid, PID_KP, PITCH_ANGULAR_VELOCITY_KP);
    graph_set_param_val(graph, ps->pitch_r_pid, PID_KI, PITCH_ANGULAR_VELOCITY_KI);
    graph_set_param_val(graph, ps->pitch_r_pid, PID_KD, PITCH_ANGULAR_VELOCITY_KD);
    graph_set_param_val(graph, ps->pitch_r_pid, PID_ALPHA, PITCH_ANGULAR_VELOCITY_ALPHA);

    // Set roll PID constants
    graph_set_param_val(graph, ps->roll_pid, PID_KP, ROLL_ANGLE_KP);
    graph_set_param_val(graph, ps->roll_pid, PID_KI, ROLL_ANGLE_KI);
    graph_set_param_val(graph, ps->roll_pid, PID_KD, ROLL_ANGLE_KD);
    graph_set_param_val(graph, ps->roll_pid, PID_ALPHA, ROLL_ANGLE_ALPHA);
    graph_set_param_val(graph, ps->roll_r_pid, PID_KP, ROLL_ANGULAR_VELOCITY_KP);
    graph_set_param_val(graph, ps->roll_r_pid, PID_KI, ROLL_ANGULAR_VELOCITY_KI);
    graph_set_param_val(graph, ps->roll_r_pid, PID_KD, ROLL_ANGULAR_VELOCITY_KD);
    graph_set_param_val(graph, ps->roll_r_pid, PID_ALPHA, ROLL_ANGULAR_VELOCITY_ALPHA);

    // Set yaw PID constants
    graph_set_param_val(graph, ps->yaw_pid, PID_KP, YAW_ANGLE_KP);
    graph_set_param_val(graph, ps->yaw_pid, PID_KI, YAW_ANGLE_KI);
    graph_set_param_val(graph, ps->yaw_pid, PID_KD, YAW_ANGLE_KD);
    graph_set_param_val(graph, ps->yaw_r_pid, PID_KP, YAW_ANGULAR_VELOCITY_KP);
    graph_set_param_val(graph, ps->yaw_r_pid, PID_KI, YAW_ANGULAR_VELOCITY_KI);
    graph_set_param_val(graph, ps->yaw_r_pid, PID_KD, YAW_ANGULAR_VELOCITY_KD);

    // Set velocity constants
    graph_set_param_val(graph, ps->x_vel_pid, PID_KP, XVEL_KP);
    graph_set_param_val(graph, ps->x_vel_pid, PID_KD, XVEL_KD);
    graph_set_param_val(graph, ps->y_vel_pid, PID_KP, YVEL_KP);
    graph_set_param_val(graph, ps->y_vel_pid, PID_KD, YVEL_KD);
    // Differentiators
    graph_set_param_val(graph, ps->x_vel, PID_ALPHA, XVEL_ALPHA);
    graph_set_param_val(graph, ps->y_vel, PID_ALPHA, YVEL_ALPHA);

    // Set X/Y/Alt constants
    graph_set_param_val(graph, ps->x_pos_pid, PID_KP, XPOS_KP);
    graph_set_param_val(graph, ps->x_pos_pid, PID_KI, XPOS_KI);
    graph_set_param_val(graph, ps->x_pos_pid, PID_KD, XPOS_KD);
    graph_set_param_val(graph, ps->x_pos_pid, PID_ALPHA, XPOS_ALPHA);
    graph_set_param_val(graph, ps->y_pos_pid, PID_KP, YPOS_KP);
    graph_set_param_val(graph, ps->y_pos_pid, PID_KI, YPOS_KI);
    graph_set_param_val(graph, ps->y_pos_pid, PID_KD, YPOS_KD);
    graph_set_param_val(graph, ps->y_pos_pid, PID_ALPHA, YPOS_ALPHA);
    graph_set_param_val(graph, ps->alt_pid, PID_KP, ALT_ZPOS_KP);
    graph_set_param_val(graph, ps->alt_pid, PID_KI, ALT_ZPOS_KI);
    graph_set_param_val(graph, ps->alt_pid, PID_KD, ALT_ZPOS_KD);
    graph_set_param_val(graph, ps->alt_pid, PID_ALPHA, ALT_ZPOS_ALPHA);

    // Set PWM difference clamping limits
    graph_set_param_val(graph, ps->clamp_d_pwmP, BOUNDS_MIN, -PWM_DIFF_BOUNDS);
    graph_set_param_val(graph, ps->clamp_d_pwmP, BOUNDS_MAX, PWM_DIFF_BOUNDS);
    graph_set_param_val(graph, ps->clamp_d_pwmR, BOUNDS_MIN, -PWM_DIFF_BOUNDS);
    graph_set_param_val(graph, ps->clamp_d_pwmR, BOUNDS_MAX, PWM_DIFF_BOUNDS);
    graph_set_param_val(graph, ps->clamp_d_pwmY, BOUNDS_MIN, -PWM_DIFF_BOUNDS);
    graph_set_param_val(graph, ps->clamp_d_pwmY, BOUNDS_MAX, PWM_DIFF_BOUNDS);

#ifdef USE_OF
    //Set angle clamping limits
    graph_set_param_val(graph, ps->x_vel_clamp, BOUNDS_MIN, -ANGLE_CLAMP);
    graph_set_param_val(graph, ps->x_vel_clamp, BOUNDS_MAX, ANGLE_CLAMP);
    graph_set_param_val(graph, ps->y_vel_clamp, BOUNDS_MIN, -ANGLE_CLAMP);
    graph_set_param_val(graph, ps->y_vel_clamp, BOUNDS_MAX, ANGLE_CLAMP);
#else
    // Set velocity clamping limits
    graph_set_param_val(graph, ps->x_vel_clamp, BOUNDS_MIN, -VEL_CLAMP);
    graph_set_param_val(graph, ps->x_vel_clamp, BOUNDS_MAX, VEL_CLAMP);
    graph_set_param_val(graph, ps->y_vel_clamp, BOUNDS_MIN, -VEL_CLAMP);
    graph_set_param_val(graph, ps->y_vel_clamp, BOUNDS_MAX, VEL_CLAMP);
#endif

    // Set trims
    graph_set_param_val(graph, ps->pitch_trim, CONST_SET, PITCH_TRIM);

    // Initial value for sampling periods
    graph_set_param_val(graph, ps->pos_time, CONST_SET, 0.04);
    graph_set_param_val(graph, ps->angle_time, CONST_SET, 0.005);
    // Set initial mode
    connect_manual(ps);

	return 0;
 }

 int control_algorithm(log_t* log_struct, user_input_t * user_input_struct, sensor_t* sensor_struct, setpoint_t* setpoint_struct, parameter_t* ps, user_defined_t* user_defined_struct, actuator_command_t* actuator_struct, modular_structs_t* structs)
 {
    struct computation_graph* graph = ps->graph;
	// use the 'flap' switch as the flight mode selector
	int cur_fm_switch = read_flap(user_input_struct->rc_commands[FLAP]);
	static int last_fm_switch = MANUAL_FLIGHT_MODE;

	// reset flight_mode to MANUAL right away if the flap switch is in manual position
	// to engage AUTO mode the code waits for a new packet after the flap is switched to auto
	// before actually engaging AUTO mode
	if(cur_fm_switch == MANUAL_FLIGHT_MODE) {
		if (last_fm_switch == AUTO_FLIGHT_MODE) {
			connect_manual(ps);
		}
		user_defined_struct->flight_mode = MANUAL_FLIGHT_MODE;
	}

	// flap switch was just toggled to auto flight mode
	if((last_fm_switch != cur_fm_switch) && (cur_fm_switch == AUTO_FLIGHT_MODE))
	{
		user_defined_struct->engaging_auto = 1;

		graph_set_param_val(graph, ps->throttle_trim, CONST_SET, user_input_struct->rc_commands[THROTTLE]);
	}

	if(user_input_struct->locationFresh && user_defined_struct->engaging_auto == 1)
			user_defined_struct->engaging_auto = 2;


	// The last packet ID from VRPN.
	static int last_vrpn_id = 0;
	// if the flap switch was toggled to AUTO_FLIGHT_MODE and we've received a new packet
	// then record the current position as the desired position
	// also reset the previous error and accumulated error from the position PIDs
	if((cur_fm_switch == AUTO_FLIGHT_MODE) && (user_defined_struct->engaging_auto == 2))
	{
#ifdef USE_OF
		graph_set_param_val(graph, ps->x_set, CONST_SET, graph_get_output(graph, ps->of_trimmed_x, ADD_SUM));
		graph_set_param_val(graph, ps->y_set, CONST_SET, graph_get_output(graph, ps->of_trimmed_y, ADD_SUM));
		//graph_set_param_val(graph, ps->alt_set, CONST_SET, sensor_struct->currentQuadPosition.alt_pos);
		graph_set_param_val(graph, ps->yaw_set, CONST_SET, sensor_struct->currentQuadPosition.yaw);
#else
		graph_set_param_val(graph, ps->x_set, CONST_SET, sensor_struct->currentQuadPosition.x_pos);
		graph_set_param_val(graph, ps->y_set, CONST_SET, sensor_struct->currentQuadPosition.y_pos);
		graph_set_param_val(graph, ps->alt_set, CONST_SET, sensor_struct->currentQuadPosition.alt_pos);
		graph_set_param_val(graph, ps->yaw_set, CONST_SET, sensor_struct->currentQuadPosition.yaw);
#endif

		// reset the flag that engages auto mode
		user_defined_struct->engaging_auto = 0;
		// finally engage the AUTO_FLIGHT_MODE
		// this ensures that we've gotten a new update packet right after the switch was set to auto mode
		user_defined_struct->flight_mode = AUTO_FLIGHT_MODE;
		connect_autonomous(ps);
		// Reset this when autonomous is engaged, so there is not a large difference at the start of autonomous
		last_vrpn_id = sensor_struct->currentQuadPosition.packetId - 1;
	}

	//PIDS///////////////////////////////////////////////////////////////////////

	/* 					Position loop
	 * Reads current position, and outputs
	 * a pitch or roll for the angle loop PIDs
	 */

	if(user_input_struct->locationFresh)
	{
	    // VRPN values
	    graph_set_param_val(graph, ps->vrpn_x, CONST_SET, sensor_struct->currentQuadPosition.x_pos);
	    graph_set_param_val(graph, ps->vrpn_y, CONST_SET, sensor_struct->currentQuadPosition.y_pos);
	    graph_set_param_val(graph, ps->vrpn_alt, CONST_SET, sensor_struct->currentQuadPosition.alt_pos);
	    graph_set_param_val(graph, ps->vrpn_pitch, CONST_SET, sensor_struct->currentQuadPosition.pitch);
	    graph_set_param_val(graph, ps->vrpn_roll, CONST_SET, sensor_struct->currentQuadPosition.roll);
	    graph_set_param_val(graph, ps->cur_yaw, CONST_SET, sensor_struct->currentQuadPosition.yaw);

	    // Use the difference in IDs to compute the sampling time for the position PIDs
	    int current_vrpn_id = sensor_struct->currentQuadPosition.packetId;
	    graph_set_param_val(graph, ps->pos_time, CONST_SET, VRPN_REFRESH_TIME * (current_vrpn_id - last_vrpn_id));
	    last_vrpn_id = current_vrpn_id;
	}
    

	// Loop time
    graph_set_param_val(graph, ps->angle_time, CONST_SET, get_last_loop_time());

	// Sensor values
    graph_set_param_val(graph, ps->cur_pitch, CONST_SET, sensor_struct->pitch_angle_filtered);
    graph_set_param_val(graph, ps->cur_roll, CONST_SET, sensor_struct->roll_angle_filtered);
    graph_set_param_val(graph, ps->mag_yaw, CONST_SET, sensor_struct->yaw_angle_filtered);
    graph_set_param_val(graph, ps->gyro_y, CONST_SET, sensor_struct->gyr_y);
    graph_set_param_val(graph, ps->gyro_x, CONST_SET, sensor_struct->gyr_x);
    graph_set_param_val(graph, ps->gyro_z, CONST_SET, sensor_struct->gyr_z);
    //if (fabs(sensor_struct->lidar_altitude) <= MAX_VALID_LIDAR) {
        graph_set_param_val(graph, ps->lidar, CONST_SET, sensor_struct->lidar_altitude);
    //}
    graph_set_param_val(graph, ps->flow_quality, CONST_SET, sensor_struct->optical_flow.quality);
    //As per documentation, disregard frames with low quality, as they contain unreliable data
    //NOTE: As per the Wehrneyton(R) Method(TM), we will be exponentially decaying the
    //optical flow velocities when the quality is below the threshold
    //if (sensor_struct->optical_flow.quality >= PX4FLOW_QUAL_MIN) {
        graph_set_param_val(graph, ps->flow_vel_x, CONST_SET, sensor_struct->optical_flow.xVel);
        graph_set_param_val(graph, ps->flow_vel_y, CONST_SET, sensor_struct->optical_flow.yVel);
    //}
    graph_set_param_val(graph, ps->flow_distance, CONST_SET, sensor_struct->optical_flow.distance);

    // RC values
    graph_set_param_val(graph, ps->rc_pitch, CONST_SET, user_input_struct->pitch_angle_manual_setpoint);
    graph_set_param_val(graph, ps->rc_roll, CONST_SET, user_input_struct->roll_angle_manual_setpoint);
    graph_set_param_val(graph, ps->rc_yaw, CONST_SET, user_input_struct->yaw_manual_setpoint);
    graph_set_param_val(graph, ps->rc_throttle, CONST_SET, user_input_struct->rc_commands[THROTTLE]);

    // Compute VRPN blocks so they can be logged
    int outputs[3] = {ps->mixer, ps->of_integ_x, ps->of_integ_y};
    graph_compute_nodes(graph, outputs, 3);

	 // here for now so in case any flight command is not PID controlled, it will default to rc_command value:
	//memcpy(raw_actuator_struct->controller_corrected_motor_commands, user_input_struct->rc_commands, sizeof(int) * 6);

	// don't use the PID corrections if the throttle is less than about 10% of its range
    // unless we are in autonomous
	if((user_input_struct->rc_commands[THROTTLE] > 0.18000) ||
        user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)
	{
			//THROTTLE

			actuator_struct->motor_magnitudes[0] = graph_get_output(graph, ps->mixer, MIXER_MOTOR0);
			actuator_struct->motor_magnitudes[1] = graph_get_output(graph, ps->mixer, MIXER_MOTOR1);
			actuator_struct->motor_magnitudes[2] = graph_get_output(graph, ps->mixer, MIXER_MOTOR2);
			actuator_struct->motor_magnitudes[3] = graph_get_output(graph, ps->mixer, MIXER_MOTOR3);
	}
	else
	{
		actuator_struct->motor_magnitudes[0] = user_input_struct->rc_commands[THROTTLE];
		actuator_struct->motor_magnitudes[1] = user_input_struct->rc_commands[THROTTLE];
		actuator_struct->motor_magnitudes[2] = user_input_struct->rc_commands[THROTTLE];
		actuator_struct->motor_magnitudes[3] = user_input_struct->rc_commands[THROTTLE];
	}

	last_fm_switch = cur_fm_switch;

	// Make location stale now
	user_input_struct->locationFresh = 0;

    return 0;
 }

 void setPIDCoeff(PID_t* p, float pValue, float iValue, float dValue) {

 	p->Kp = pValue;
 	p->Ki = iValue;
 	p->Kd = dValue;

 }