/*
 * sensor_processing.c
 *
 *  Created on: Feb 20, 2016
 *      Author: ucart
 */
 
#include <stdio.h>
#include "log_data.h"
#include "sensor.h"
#include "conversion.h"
#include "quadposition.h"
#include "sensor_processing.h"
#include "timer.h"
#include <math.h>

#define ALPHA 0.99

int sensor_processing_init(sensor_t* sensor_struct) {
	float a0 = 0.0200833310260;
	float a1 = 0.0401666620520;
	float a2 = 0.0200833310260;
	float b1 = -1.561015391253;
	float b2 = 0.6413487153577;
	sensor_struct->accel_x_filt = filter_make_state(a0, a1, a2, b1, b2);
	sensor_struct->accel_y_filt = filter_make_state(a0, a1, a2, b1, b2);
	sensor_struct->accel_z_filt = filter_make_state(a0, a1, a2, b1, b2);
	return 0;
}

int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_sensor_t* raw_sensor_struct, sensor_t* sensor_struct)
{
	// Filter accelerometer values
	gam_t* gam = &(raw_sensor_struct->gam);
	float accel_x = biquad_execute(&sensor_struct->accel_x_filt, gam->accel_x);
	float accel_y = biquad_execute(&sensor_struct->accel_y_filt, gam->accel_y);
	float accel_z = biquad_execute(&sensor_struct->accel_z_filt, gam->accel_z);
	//Get X and Y angles
	// javey: this assigns accel_(pitch/roll) in units of radians
	float accel_pitch = atan(accel_x / sqrt(accel_y*accel_y + accel_z*accel_z));
	float accel_roll = -atan(accel_y / sqrt(accel_x*accel_x + accel_z*accel_z)); // negative because sensor board is upside down


	// copy currentQuadPosition and trimmedRCValues from raw_sensor_struct to sensor_struct
	deep_copy_Qpos(&(sensor_struct->currentQuadPosition), &(raw_sensor_struct->currentQuadPosition));

	// Calculate Euler angles and velocities using Gimbal Equations below
	/////////////////////////////////////////////////////////////////////////
	// | Phi_d   |   |  1  sin(Phi)tan(theta)    cos(Phi)tan(theta) |  | p |
	// | theta_d | = |  0  cos(Phi)              -sin(Phi)		    |  | q |
	// | Psi_d   |   |  0  sin(Phi)sec(theta)    cos(Phi)sec(theat) |  | r |
	//
	// Phi_dot = p + q sin(Phi) tan(theta) + r cos(Phi) tan(theta)
	// theta_dot = q cos(Phi) - r sin(Phi)
	// Psi_dot = q sin(Phi) sec(theta) + r cos(Phi) sec(theta)
	///////////////////////////////////////////////////////////////////////////

	// javey:
	//
	// The gimbal equations are defined in the book "Flight Simulation" by Rolfe and Staples.
	// Find on page 46, equation 3.6

	// these are calculated to be used in the gimbal equations below
	// the variable roll(pitch)_angle_filtered is phi(theta)
	double sin_phi = sin(sensor_struct->roll_angle_filtered);
	double cos_phi = cos(sensor_struct->roll_angle_filtered);
	double tan_theta = tan(sensor_struct->pitch_angle_filtered);
	double sec_theta = 1/cos(sensor_struct->pitch_angle_filtered);

//	Gryo "p" is the angular velocity rotation about the x-axis (defined as var gyro_xVel_p in gam struct)
//	Gyro "q" is the angular velocity rotation about the y-axis (defined as var gyro_xVel_q in gam struct)
//	Gyro "r" is the angular velocity rotation about the z-axis (defined as var gyro_xVel_r in gam struct)

	// phi is the conventional symbol used for roll angle, so phi_dot is the roll velocity
	sensor_struct->phi_dot = raw_sensor_struct->gam.gyro_xVel_p + (raw_sensor_struct->gam.gyro_yVel_q*sin_phi*tan_theta)
			+ (raw_sensor_struct->gam.gyro_zVel_r*cos_phi*tan_theta);

	// theta is the conventional symbol used for pitch angle, so theta_dot is the pitch velocity
	sensor_struct->theta_dot = (raw_sensor_struct->gam.gyro_yVel_q*cos_phi)
			- (raw_sensor_struct->gam.gyro_zVel_r*sin_phi);

	// psi is the conventional symbol used for yaw angle, so psi_dot is the yaw velocity
	sensor_struct->psi_dot = (raw_sensor_struct->gam.gyro_yVel_q*sin_phi*sec_theta)
			+ (raw_sensor_struct->gam.gyro_zVel_r*cos_phi*sec_theta);

	// Copy in raw gyroscope values
	sensor_struct->gyr_x = raw_sensor_struct->gam.gyro_xVel_p;
	sensor_struct->gyr_y = raw_sensor_struct->gam.gyro_yVel_q;
	sensor_struct->gyr_z = raw_sensor_struct->gam.gyro_zVel_r;

	// Complementary Filter Calculations
	sensor_struct->pitch_angle_filtered = ALPHA * (sensor_struct->pitch_angle_filtered + sensor_struct->theta_dot * get_last_loop_time())
			+ (1. - ALPHA) * accel_pitch;

	sensor_struct->roll_angle_filtered = ALPHA * (sensor_struct->roll_angle_filtered + sensor_struct->phi_dot* get_last_loop_time())
			+ (1. - ALPHA) * accel_roll;


	// Z-axis points upward, so negate distance
	sensor_struct->lidar_altitude = -raw_sensor_struct->lidar_distance_m;

	// Simply copy optical flow data
	sensor_struct->optical_flow = raw_sensor_struct->optical_flow;
	return 0;
}

void set_pitch_angle_filtered(sensor_t * sensor_struct, float accel_roll)
{
	sensor_struct->pitch_angle_filtered = accel_roll;
}
void set_roll_angle_filtered(sensor_t * sensor_struct, float accel_pitch)
{
	sensor_struct->roll_angle_filtered = accel_pitch;
}

void deep_copy_Qpos(quadPosition_t * dest, quadPosition_t * src)
{
	dest->packetId = src->packetId;
	dest->y_pos = src->y_pos;
	dest->x_pos = src->x_pos;
	dest->alt_pos = src->alt_pos;
	dest->roll = src->roll;
	dest->pitch = src->pitch;
	dest->yaw = src->yaw;

}