/*
 * initialize_components.c
 *
 *  Created on: Feb 20, 2016
 *      Author: ucart
 */
 
#include "initialize_components.h"
#include "communication.h"

//#define BENCH_TEST

extern int Xil_AssertWait;

int protection_loops(modular_structs_t *structs)
{
	int rc_commands[6]; // 6 "receiver_input" elements: Throttle, Pitch, Roll, Yaw, Gear, and Flap

	read_rec_all(rc_commands);

	// wait for RC receiver to connect to transmitter
	while(rc_commands[THROTTLE] < 75000)
		read_rec_all(rc_commands);

	// wait until throttle is low and the gear switch is engaged (so you don't immediately break out of the main loop below)
	// also wait for the flight mode to be set to manual
	while(rc_commands[THROTTLE] > 125000 || read_kill(rc_commands[GEAR]) || !read_flap(rc_commands[FLAP]))
		read_rec_all(rc_commands);

	// wait until the ground station has connected to the quad and acknowledged that its ready to start
	char buf[255] = {};
	int length = snprintf(buf, sizeof(buf), "The quad is ready to receive VRPN data.\r\n");
	length = length >= sizeof(buf) ? 255 : length;

	while (!structs->user_input_struct.receivedBeginUpdate) {
		send_data(MessageTypes[4].ID, MessageTypes[4].subtypes[1].ID, 0, buf, length);
		process_received(structs);
		usleep(10000);
	}

	// let the pilot/observers know that the quad is now active
	MIO7_led_on();

	return 0;
}

int initializeAllComponents(user_input_t * user_input_struct, log_t * log_struct, raw_sensor_t * raw_sensor_struct,
		sensor_t * sensor_struct, setpoint_t * setpoint_struct, parameter_t * parameter_struct, user_defined_t *user_defined_struct,
		raw_actuator_t * raw_actuator_struct, actuator_command_t * actuator_command_struct)
{
    // Turn off LED 7 to let observers know that the quad is not yet active
	MIO7_led_off();

	// Initialize the controller
	control_algorithm_init(parameter_struct);

	// Xilinx given initialization
	init_platform();


	// Initialize loop timers
	if (timer_init()) {
		return -1;
	}

	// Initialize UART0 (Bluetooth)
	if (initUartComms()) {
		return -1;
	}

	// Initialize I2C controller and start the sensor board
	if (initI2C0() == -1) {
		return -1;
	}

	// Initialize PWM Recorders and Motor outputs
	pwm_init();

	//manual flight mode
	user_defined_struct->flight_mode = MANUAL_FLIGHT_MODE;

#ifndef BENCH_TEST
	// Get the first loop data from accelerometer for the gyroscope to use
	if(sensor_init(raw_sensor_struct, sensor_struct) == -1)
		return -1;
#endif

	return 0;
}