/*
 * user_input.c
 *
 *  Created on: Feb 20, 2016
 *      Author: ucart
 */
 
#include "user_input.h"
#include "uart.h"
 
int get_user_input(log_t* log_struct, user_input_t* user_input_struct)
{
	// Read in values from RC Receiver
	read_rec_all(user_input_struct->rc_commands);

	log_struct->commands.pitch    = user_input_struct->rc_commands[PITCH];
	log_struct->commands.roll     = user_input_struct->rc_commands[ROLL];
	log_struct->commands.throttle = user_input_struct->rc_commands[THROTTLE];
	log_struct->commands.yaw      = user_input_struct->rc_commands[YAW];

	//create setpoints for manual flight
	// currently in units of radians
	user_input_struct->yaw_manual_setpoint = convert_from_receiver_cmd(user_input_struct->rc_commands[YAW], YAW_MAX, YAW_CENTER, YAW_MIN, YAW_RAD_TARGET, -(YAW_RAD_TARGET));
	user_input_struct->roll_angle_manual_setpoint = convert_from_receiver_cmd(user_input_struct->rc_commands[ROLL], ROLL_MAX, ROLL_CENTER, ROLL_MIN, ROLL_RAD_TARGET, -(ROLL_RAD_TARGET));
	user_input_struct->pitch_angle_manual_setpoint = convert_from_receiver_cmd(user_input_struct->rc_commands[PITCH], PITCH_MAX, PITCH_CENTER, PITCH_MIN, PITCH_RAD_TARGET, -(PITCH_RAD_TARGET));

	// Listen on bluetooth and if there's a packet,
	// then receive the packet and set hasPacket for later processing
	// "update packet" type processing is done in sensor.c
	// "command packet" type processing is done in control_algorithm.c
	//user_input_struct->hasPacket = tryReceivePacket(user_input_struct->sb, 0);

    return 0;
}

int kill_condition(user_input_t* user_input_struct)
{
	return read_kill(user_input_struct->rc_commands[GEAR]);
}

/*
 * Converts an RC receiver command to whatever units that max_target and min_target are in. This function first centers the receiver command at 0.
 * It creates a windowed linear function, based on the sign of the centered receiver command.
 *
 * 										       -
 * 									          / (x - center_receiver_cmd) * (min_target / (min_receiver_cmd - center_receiver_cmd)) when x <=  0
 * convert_receiver_cmd(x = receiver_cmd) =  <
 * 										      \ (x - center_receiver_cmd) * (max_target / (max_receiver_cmd - center_receiver_cmd) when x > 0
 * 										       -
 *
 */
float convert_from_receiver_cmd(int receiver_cmd, int max_receiver_cmd, int center_receiver_cmd, int min_receiver_cmd, float max_target, float min_target)
{
	// centers the receiver command by subtracting the given center value. This means that if receiver_cmd == center then receiver_cmd_centered should be 0.
	int receiver_cmd_centered = receiver_cmd - center_receiver_cmd;

	if(receiver_cmd_centered <= 0) {
		float ret = ((float)(receiver_cmd_centered * min_target)) / ((float) (min_receiver_cmd - center_receiver_cmd));

		if(ret < min_target)
			ret = min_target;

		return ret;
	}

	else {
		float ret = ((float)(receiver_cmd_centered * max_target)) / ((float) (max_receiver_cmd - center_receiver_cmd));

		if(ret > max_target)
			ret = max_target;

		return ret;
	}

	return 0.0;
}