#include <stdio.h>
#include "hw_impl_zybo.h"
#include "quad_app.h"
#include "type_def.h"
#include "platform.h"

//#define RUN_TESTS

/**
 * Create the hardware drivers, and place them on the hardware struct.
 *
 * Things to keep in mind:
 * - The CommDriver requires a UART. Hence, make the a UARTDriver first
 *   and then give that instance to the CommDriver.
 * - Same for GPSDriver, except it needs a different hardware device.
 * - The I2C devices (IMU, LIDAR, Optical Flow), need a I2C bus. Hence
 *   make a I2CDriver first, then give the appropariate instance the
 *   device drivers.
 *   - Currently, we are putting the Lidar on its own bus. Apparently,
 *     when looking at an OScope with the Lidar on a bus with other
 *     devices, it does not look right. The SCL line would never
 *     go all the way back down to 0 volts
 */
int setup_hardware(hardware_t *hardware) {
  hardware->rc_receiver = create_zybo_rc_receiver();
  hardware->motors = create_zybo_motors();
  hardware->uart_0 = create_zybo_uart(0);
  hardware->uart_1 = create_zybo_uart(1);
  hardware->comm = create_zybo_comm(&hardware->uart_0);
  hardware->gps = create_zybo_gps(&hardware->uart_1);
  hardware->global_timer = create_zybo_global_timer();
  hardware->axi_timer = create_zybo_axi_timer();
  hardware->mio7_led = create_zybo_mio7_led();
  hardware->sys = create_zybo_system();
  hardware->i2c_0 = create_zybo_i2c(0);
  hardware->i2c_1 = create_zybo_i2c(1);
  hardware->imu = create_zybo_imu(&hardware->i2c_0);
  hardware->lidar = create_zybo_lidar(&hardware->i2c_1);
  hardware->of = create_zybo_optical_flow(&hardware->i2c_0);
  return 0;
}

int main()
{
  // Zynq initialization
  init_platform();

#ifdef RUN_TESTS
  //test_zybo_mio7_led_and_system();
  //test_zybo_i2c();
  //test_zybo_i2c_imu();
  //test_zybo_i2c_px4flow();
  //test_zybo_i2c_lidar();
  //test_zybo_i2c_all();
  //test_zybo_rc_receiver();
  //test_zybo_motors();
  //test_zybo_uart();
  //test_zybo_axi_timer();
  test_zybo_uart_comm();
  return 0;
#endif

  // Run the main quad application
  quad_main(setup_hardware);

  while(1) {
	  volatile int i = 0;
	  i++;
  }

  return 0;
}