#include <stdio.h>
#include <stdbool.h>
#include <unistd.h>

#include "ev3.h"
#include "ev3_sensor.h" // e.g.: ~/robot/ev3dev-c/eg/sensor/sensor.c

#include "color.h"

uint8_t button;

bool button_pressed() {
	int val;
	if (button == SENSOR__NONE_) {
     return (ev3_read_keys((uint8_t*) &val) && (val & EV3_KEY_UP));
	}
   else {
     return (get_sensor_value(0, button, &val) && (val != 0 ));
   }
}

void button_init() {
  ev3_sensor_init();
}

int button_test() {
  printf("button infinite loop test commenced\n");
  color_all_green();

  if (ev3_search_sensor(LEGO_EV3_TOUCH, &button, 0)) {
    printf("button found\n");
  } else {
    fprintf(stderr, "button NOT found\n");
    return 1;
  }

  color_all_red();
  while (!button_pressed()) {
    sleep(1);
  }
  return 0;
}