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

#include "ev3.h"
#include "ev3_light.h" // ~/robot/ev3dev-c/source/ev3/ev3_light.h

#include "color.h"

/* ITERATE ALL */
void cycle_colors (unsigned int pause_sec) {
  for (uint8_t lit_loc = LIT_LEFT; lit_loc < LIT__LOC__; lit_loc++) {
    for (uint8_t col = LIT_OFF; col < LIT__COL__; col++) {
      set_light(lit_loc, col);
      sleep(pause_sec);
    }
  }
}

/* ABSOLUTE */
void color_left_right(uint8_t l, uint8_t r) {
  set_light(LIT_LEFT,  l);
  set_light(LIT_RIGHT, r);
}

/* SAME */
void color_same (uint8_t col) {
  color_left_right(col, col);
}
void color_all_amber() { color_same(LIT_AMBER); }
void color_all_green() { color_same(LIT_GREEN); }
void color_all_red()   { color_same(LIT_RED);   }
void color_all_off()   { color_same(LIT_OFF);   }

/* ONE ON, ONE OFF */
// left
void color_only_left_amber() { color_left_right(LIT_AMBER, LIT_OFF); }
void color_only_left_red()   { color_left_right(LIT_RED,   LIT_OFF); }
void color_only_left_green() { color_left_right(LIT_GREEN, LIT_OFF); }
// right
void color_only_right_amber() { color_left_right(LIT_OFF, LIT_AMBER); }
void color_only_right_red()   { color_left_right(LIT_OFF, LIT_RED);   }
void color_obly_right_green() { color_left_right(LIT_OFF, LIT_GREEN); }

/* COMBINATIONS */
void color_amber_green() { color_left_right(LIT_AMBER, LIT_GREEN); }
void color_amber_red()   { color_left_right(LIT_AMBER, LIT_RED);   }
void color_green_amber() { color_left_right(LIT_GREEN, LIT_AMBER); }
void color_green_red()   { color_left_right(LIT_GREEN, LIT_RED);   }
void color_red_amber()   { color_left_right(LIT_RED,   LIT_AMBER); }
void color_red_green()   { color_left_right(LIT_RED,   LIT_RED);   }

/* RELATIVE */
// left
void color_left_amber()  { set_light(LIT_LEFT, LIT_AMBER); }
void color_left_green()  { set_light(LIT_LEFT, LIT_GREEN); }
void color_left_red()    { set_light(LIT_LEFT, LIT_RED);   }
void color_left_off()    { set_light(LIT_LEFT, LIT_OFF);   }

// right
void color_right_amber() { set_light(LIT_RIGHT, LIT_AMBER); }
void color_right_green() { set_light(LIT_RIGHT, LIT_GREEN); }
void color_right_red()   { set_light(LIT_RIGHT, LIT_RED);   }
void color_right_off()   { set_light(LIT_RIGHT, LIT_OFF);   }

/* TEST */
void color_test_all () {
  unsigned int pause_sec = 1;

  color_all_amber();
  sleep(pause_sec);

  color_all_green();
  sleep(pause_sec);

  color_all_red();
  sleep(pause_sec);

  color_all_off();
  sleep(pause_sec);

  for (uint8_t loc = 0; loc < LIT__LOC__; loc++) {
    set_light(loc, LIT_GREEN);
    sleep(pause_sec);

    set_light(loc, LIT_AMBER);
    sleep(pause_sec);

    set_light(loc, LIT_RED);
    sleep(pause_sec);
  }

  color_amber_green();
  sleep(pause_sec);

  color_amber_red();
  sleep(pause_sec);

  color_green_amber();
  sleep(pause_sec);

  color_green_red();
  sleep(pause_sec);

  color_red_amber();
  sleep(pause_sec);

  color_red_green();
  sleep(pause_sec);

  cycle_colors(pause_sec);

  color_all_amber();
}