#include <string.h>

#include "../shared/consts.h"
#include "io.h"
#include "modes.h"
#include "orangutan_shim.h"

w2_s_io_all g_w2_io;

void w2_io_main() {
	g_w2_io.button[0].pressed = get_single_debounced_button_press(BUTTON_A);
	g_w2_io.button[1].pressed = get_single_debounced_button_press(BUTTON_B);
	g_w2_io.button[2].pressed = get_single_debounced_button_press(BUTTON_C);
	g_w2_io.button[3].pressed = get_single_debounced_button_press(TOP_BUTTON);
	g_w2_io.button[4].pressed = get_single_debounced_button_press(BOTTOM_BUTTON);

	unsigned int sensor_values[5];
	qtr_read(sensor_values, QTR_EMITTERS_ON);
	for (int i = 0; i < 5; i++) g_w2_io.qtr[i].range = sensor_values[i];

	// TODO average voltage over mutiple samples sensor
	g_w2_io.battery.charge_level	 = analog_read(W2_BATTERY_PIN);
	g_w2_io.front_distance.detection = analog_read(W2_FRONT_SENSOR_PIN);
	g_w2_io.side_distance.detection	 = analog_read(W2_SIDE_SENSOR_PIN);

	set_motors(g_w2_io.motor_left.speed, g_w2_io.motor_right.speed);
	red_led(g_w2_io.led_red.on);
	green_led(g_w2_io.led_green.on);

	// TODO don't do this every cycle
	char text_copy[17];
	memcpy((char *)text_copy, g_w2_io.lcd.text, 16);
	text_copy[16] = 0x00;
	print(text_copy);
};