/*
 * main.c
 *
 *  Created on: Nov 11, 2015
 *      Author: ucart
 */
#include <stdio.h>
#include "timer.h"
#include "log_data.h"
#include "initialize_components.h"
#include "user_input.h"
#include "sensor.h"
#include "sensor_processing.h"
#include "control_algorithm.h"
#include "actuator_command_processing.h"
#include "send_actuator_commands.h"
#include "update_gui.h"
#include "communication.h"

//#define BENCH_TEST
//#define UART_BENCHMARK

int main()
{
	// Structures to be used throughout
	modular_structs_t structs = { };

	// Wire up hardware
	setup_hardware(&structs.hardware_struct);

	// Initialize all required components and structs:
	// Uart, PWM receiver/generator, I2C, Sensor Board
	// Xilinx Platform, Loop Timer, Control Algorithm
	int init_error = init_structs(&(structs));

	if (init_error != 0) {
		return -1;
	}

#ifndef BENCH_TEST
	// Loops to make sure the quad is responding correctly before starting the control loop
	protection_loops(&structs);
#endif

	int last_kill_condition = kill_condition(&(structs.user_input_struct));

	// Main control loop
	while (1)
	{
		int this_kill_condition = kill_condition(&(structs.user_input_struct));

		// Processing of loop timer at the beginning of the control loop
		timer_start_loop();

		// Process all received data

#ifdef UART_BENCHMARK
		usleep(500000);
		u32 start_time = timer_get_count();
#endif

		process_received(&structs);

#ifdef UART_BENCHMARK
		u32 end_time = timer_get_count();
		u32 duration = end_time - start_time;
		u32 packets_processed = uart_buff_packets_processed();
		u32 data[2];
		data[0] = duration;
		data[1] = packets_processed;
		send_data(0, 0, 0, (char *) &data, 8);
#endif

#ifndef BENCH_TEST
		// Get the user input and put it into user_input_struct
		get_user_input(&(structs.hardware_struct), &(structs.log_struct), &(structs.user_input_struct));

		// Get data from the sensors and put it into raw_sensor_struct
		get_sensors(&(structs.log_struct), &(structs.user_input_struct), &(structs.raw_sensor_struct));

		// Process the sensor data and put it into sensor_struct
		sensor_processing(&(structs.log_struct), &(structs.user_input_struct), &(structs.raw_sensor_struct), &(structs.sensor_struct));

		if (!this_kill_condition) {
			// Run the control algorithm
			control_algorithm(&(structs.log_struct), &(structs.user_input_struct), &(structs.sensor_struct), &(structs.setpoint_struct),
					&(structs.parameter_struct), &(structs.user_defined_struct), &(structs.actuator_command_struct), &structs);

			// send the actuator commands
			send_actuator_commands(&(structs.hardware_struct.pwm_outputs), &(structs.log_struct), &(structs.actuator_command_struct));
		}
		else {
			pwm_kill();
		}
		// update the GUI
		update_GUI(&(structs.log_struct));
#endif
		// Processing of loop timer at the end of the control loop
		timer_end_loop(&(structs.log_struct));

		if (!this_kill_condition) {
			if(structs.user_defined_struct.flight_mode == AUTO_FLIGHT_MODE)
			{
				// Log the data collected in this loop
				log_data(&(structs.log_struct), &(structs.parameter_struct));

				static int loop_counter = 0;
				loop_counter++;

				// toggle the MIO7 on and off to show that the quad is in AUTO_FLIGHT_MODE
				if(loop_counter == 10)
				{
					MIO7_led_off();
				}
				else if(loop_counter >= 20)
				{

					MIO7_led_on();
					loop_counter = 0;
				}

			}
			if(structs.user_defined_struct.flight_mode == MANUAL_FLIGHT_MODE) {
				MIO7_led_on();
			}
		}

		if (this_kill_condition == 1 && last_kill_condition == 0) {
		  // Just disabled
		  printLogging(&structs.hardware_struct, &(structs.log_struct), &(structs.parameter_struct));
		  resetLogging();
		  MIO7_led_off();
		}

		last_kill_condition = this_kill_condition;
	}

	kill_motors();

	flash_MIO_7_led(10, 100);

	return 0;
}