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

//#define RUN_TESTS

int setup_hardware(hardware_t *hardware) {
  hardware->i2c = create_zybo_i2c();
  hardware->rc_receiver = create_zybo_rc_receiver();
  hardware->motors = create_zybo_motors();
  hardware->uart = create_zybo_uart();
  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();
  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_all();
  //test_zybo_rc_receiver();
  test_zybo_motors();
  //test_zybo_uart();
  //test_zybo_axi_timer();
  //test_zybo_uart();
  return 0;
#endif

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

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

  return 0;
}