#include <stdio.h>

#include "ev3.h"
#include "ev3_sensor.h"

#include "color_sensor.h"

#include "button.h"
#include "misc_const.h"

uint8_t sn_color;
int val;

const char* const color[] = {"?", "BLACK", "BLUE", "GREEN", "YELLOW", "RED", "WHITE", "BROWN"};
const int color_count = sizeof(color)/sizeof(color[0]);

int color_sensor_test () {
  if (ev3_search_sensor(LEGO_EV3_COLOR, &sn_color, 0)) {
    printf("COLOR sensor found\n");
    set_sensor_mode(sn_color, "COL-COLOR");
    while (!button_pressed()) {
      if (!get_sensor_value(0, sn_color, &val) ||
          val < 0 || val >= color_count) {
        val = 0;
      }
      printf("IR color: %s (%d)\n", color[val], val);
      fflush(stdout);
      nsleep(200);
    }
  } else {
    fprintf(stderr, "IR NOT found\n");
    return 1;
  }
  return 0;
}