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

//#define BENCH_TEST

extern int Xil_AssertWait;

int start_condition_is_met(float *rc_commands) {
  return rc_commands[THROTTLE] < 0.125 &&
    gear_is_engaged(rc_commands[GEAR]) &&
    flap_is_engaged(rc_commands[FLAP]);
}

int protection_loops(modular_structs_t *structs)
{
  float rc_commands[6];
  struct RCReceiverDriver *rc_receiver = &structs->hardware_struct.rc_receiver;
  read_rec_all(rc_receiver, rc_commands);

  while(!start_condition_is_met(rc_commands)) {
    process_received(structs);
    read_rec_all(rc_receiver, 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_0;
  if (uart->reset(uart)) {
    return -1;
  }

  // Initialize I2C_0 controller
  struct I2CDriver *i2c_0 = &structs->hardware_struct.i2c_0;
  if (i2c_0->reset(i2c_0)) {
    return -1;
  }
  // Initialize I2C controller
  struct I2CDriver *i2c_1 = &structs->hardware_struct.i2c_1;
  if (i2c_1->reset(i2c_1)) {
	  return -1;
  }

  // Initialize PWM Recorders and Motor outputs
  struct RCReceiverDriver *rc_receiver = &structs->hardware_struct.rc_receiver;
  if (rc_receiver->reset(rc_receiver)) return -1;
  struct MotorDriver *motors = &structs->hardware_struct.motors;
  if (motors->reset(motors)) return -1;

  // Set motor outputs to off
  int i;
  for (i = 0; i < 4; i += 1) {
    motors->write(motors, 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->hardware_struct,
                 &structs->raw_sensor_struct,
                 &structs->sensor_struct)) {
    return -1;
  }

  sensor_processing_init(&structs->sensor_struct);

  return 0;
}