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

//#define BENCH_TEST

extern int Xil_AssertWait;

int protection_loops(modular_structs_t *structs)
{
	u32 rc_commands[6]; // 6 "receiver_input" elements: Throttle, Pitch, Roll, Yaw, Gear, and Flap
	
	struct PWMInputDriver *pwm_inputs = &structs->hardware_struct.pwm_inputs;
	read_rec_all(pwm_inputs, rc_commands);

	// wait for RC receiver to connect to transmitter
	while(rc_commands[THROTTLE] < 75000)
	  read_rec_all(pwm_inputs, 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])) {
		process_received(structs);
		read_rec_all(pwm_inputs, rc_commands);
	}

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

	return 0;
}

int init_structs(modular_structs_t *structs) {
  struct LEDDriver *mio7_led = &structs->hardware_struct.mio7_led;
  struct SystemDriver *sys = &structs->hardware_struct.sys;
  if (mio7_led->reset(mio7_led)) return -1;
  mio7_init_globals(mio7_led, sys);
  // Turn off LED 7 to let observers know that the quad is not yet active

  mio7_led->turn_off(mio7_led);

  // Initialize the controller
  control_algorithm_init(&structs->parameter_struct);

	// Initialize the logging
	initialize_logging(&structs->log_struct, &structs->parameter_struct);

  // Initialize loop timers
  struct TimerDriver *global_timer = &structs->hardware_struct.global_timer;
  struct TimerDriver *axi_timer = &structs->hardware_struct.axi_timer;
  if (global_timer->reset(global_timer)) {
    return -1;
  }
  if (axi_timer->reset(axi_timer)) {
    return -1;
  }
  timer_init_globals(global_timer, axi_timer);

  // Initialize UART0
  struct UARTDriver *uart = &structs->hardware_struct.uart;
  if (uart->reset(uart)) {
    return -1;
  }

  // Initialize I2C controller
  struct I2CDriver *i2c = &structs->hardware_struct.i2c;
  if (i2c->reset(i2c)) {
    return -1;
  }
  iic_set_globals(i2c, sys);

  // Initialize PWM Recorders and Motor outputs
  struct PWMInputDriver *pwm_inputs = &structs->hardware_struct.pwm_inputs;
  if (pwm_inputs->reset(pwm_inputs)) return -1;
  struct PWMOutputDriver *pwm_outputs = &structs->hardware_struct.pwm_outputs;
  if (pwm_outputs->reset(pwm_outputs)) return -1;

  // Set motor outputs to off
  int i;
  for (i = 0; i < 4; i += 1) {
    pwm_outputs->write(pwm_outputs, i, MOTOR_MIN);
  }

  // Initialize sensors

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

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

  return 0;
}