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

extern int Xil_AssertWait;

int protection_loops()
{
	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
	stringBuilder_t * ack_packet = stringBuilder_create();
	while(1)
	{
		// --------------------------------------
		// Send request to ground station to start sending VRPN
		char buf[255] = {};
		int i;

		// Debug print statement
		//printf("function for yawset: %f\n", structs->setpoint_struct.desiredQuadPosition.yaw);

		// Send a reply to the ground station
		snprintf(buf, sizeof(buf), "The quad is ready to receive VRPN data.\r\n");
		unsigned char *responsePacket;

		metadata_t metadata =
		{
			BEGIN_CHAR,
			MessageTypes[4].ID,
			MessageTypes[4].subtypes[1].ID,
			0,
			(strlen(buf) + 1)
		};
		formatPacket(&metadata, buf, &responsePacket);

		// Send each byte of the packet individually
		for(i = 0; i < 8 + metadata.data_len; i++) {
			// Debug print statement for all of the bytes being sent
			printf("%d: 0x%x\n", i, responsePacket[i]);

			uart0_sendByte(responsePacket[i]);
		}

		usleep(10000);

		tryReceivePacket(ack_packet, 0);

		unsigned char * data;
		metadata_t md;
		parse_packet((unsigned char *) ack_packet->buf, &data, &md);

		if(md.msg_type == 0x04 && md.msg_subtype == 0x01)
			break;

		// --------------------------------------
	}

	// 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();

	// Use the stringbuilder to keep track of data received
	if(!(user_input_struct->sb = stringBuilder_create()))
		return -1;

	// Initialize the controller
	control_algorithm_init(parameter_struct);

	// Xilinx given initialization
	init_platform();

	//disable blocking asserts
	//Xil_AssertWait = FALSE;

	// Initialize UART0 (Bluetooth)
	if(!uart0_init(XPAR_PS7_UART_0_DEVICE_ID, 921600))
		return -1;

	uart0_clearFIFOs();

	//Enable blocking asserts
	//Xil_AssertWait = TRUE;

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

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

	// Initialize loop timers
	timer_init();

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

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

	return 0;
}