#include "util.h"
#include "../hardware/hw_impl_zybo.h"

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <xgpiops.h>
#include "PID.h"
#include "log_data.h"
#include <sleep.h>
#include "controllers.h"
#include "xparameters.h"

extern int motor0_bias, motor1_bias, motor2_bias, motor3_bias;

void setup_hardware(hardware_t *hardware) {
  hardware->i2c = create_zybo_i2c();
  hardware->pwm_inputs = create_zybo_pwm_inputs();
  hardware->pwm_outputs = create_zybo_pwm_outputs();
  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();
}

/**
 * Reads all 6 receiver channels at once
 */
void read_rec_all(struct PWMInputDriver *pwm_input, u32 *mixer){
  int i;
  for(i = 0; i < 6; i++){
    pwm_input->read(pwm_input, i, &mixer[i]);
  }
}

int hexStrToInt(char *buf, int startIdx, int endIdx) {
	int result = 0;
	int i;
	int power = 0;
	for (i=endIdx; i >= startIdx; i--) {
		int value = buf[i];
		if ('0' <= value && value <= '9') {
			value -= '0';
		} else if ('a' <= value && value <= 'f') {
			value -= 'a';
			value += 10;
		} else if ('A' <= value && value <= 'F') {
			value -= 'A';
			value += 10;
		}

		result += (2 << (4 * power)) * value;
		power++;
	}

	return result;
}

/**
 * Argument is the reading from the pwm_recorder4 which is connected to the gear pwm
 * If the message from the receiver is 0 - gear, kill the system by sending a 1
 * Otherwise, do nothing
 */
int read_kill(int gear){
	if(gear > 115000 && gear < 125000)
		return 1;
	return 0;
}

int read_flap(int flap)
{
	// flap '0' is 108,000 CC (Up)
	// flap '1' is 192,000 CC (Down)
	// here we say if the reading is greater than 150,000 than its '1'; '0' otherwise
	if(flap > 150000)
		return 1;
	return 0;
}

/**
 * Turns off the motors
 */
void kill_motors(struct PWMOutputDriver *pwm_outputs) {
  pwm_outputs->write(pwm_outputs, 0, 0);
  pwm_outputs->write(pwm_outputs, 1, 0);
  pwm_outputs->write(pwm_outputs, 2, 0);
  pwm_outputs->write(pwm_outputs, 3, 0);
}

int build_int(u8 *buff) {
  return  buff[3] << 24
    | buff[2] << 16
    | buff[1] << 8
    | buff[0];
}

float build_float(u8 *buff) {
  union {
    float f;
    int i;
  } x;

  x.i =  buff[3] << 24
    | buff[2] << 16
    | buff[1] << 8
    | buff[0];
  return x.f;
}