#include <FastLED.h>
#include <borg.h>

#define NUM_LEDS 54
#define DATA_PIN 4

#define BLUE (CRGB) 0x0000FF
#define RED (CRGB) 0xFF0000
#define GREEN (CRGB) 0x00FF00
#define ORANGE (CRGB) 0xFF7000
#define WHITE (CRGB) 0xFFFFFF
#define YELLOW (CRGB) 0xFFFF00

#define DICE_COLOR (CRGB) 0x00FF00

#define GPLEFT 8
#define GPUP 9
#define GPRIGHT 10
#define GPDOWN 11
#define GPA 13
#define GPB 12

CRGB leds[NUM_LEDS];
CRGB solution[NUM_LEDS];
byte METAROTTODIR[6];
byte CONTROLDIR[6][6];

void rubixRot(byte side, bool dir);
void rubix_loop();

byte state = 0;

bool initialize = true;

void setup() {
  FastLED.addLeds<PL9823, DATA_PIN>(leds, NUM_LEDS);
  initMap();

  pinMode(GPLEFT, INPUT_PULLUP);
  pinMode(GPRIGHT, INPUT_PULLUP);
  pinMode(GPA, INPUT_PULLUP);
  pinMode(GPB, INPUT_PULLUP);
  
  METAROTTODIR[0]=CLOCKW;
  METAROTTODIR[1]=UP;
  METAROTTODIR[2]=ACLOCKW;
  METAROTTODIR[3]=DOWN;
  METAROTTODIR[4]=LEFT;
  METAROTTODIR[5]=RIGHT;

  
  CONTROLDIR[0][EAST] = 1;
  CONTROLDIR[0][WEST]= 4;
  
  CONTROLDIR[1][EAST] = 5;
  CONTROLDIR[1][WEST] = 0;

  CONTROLDIR[2][EAST] = 3;
  CONTROLDIR[2][WEST] = 5;

  CONTROLDIR[3][EAST] = 4;
  CONTROLDIR[3][WEST] = 2;

  CONTROLDIR[4][EAST] = 0;
  CONTROLDIR[4][WEST] = 3;

  CONTROLDIR[5][EAST] = 2;
  CONTROLDIR[5][WEST] = 1;

  Serial.begin(9600);
}

void loop() {
  bool statechange = digitalRead(GPLEFT) == LOW && digitalRead(GPRIGHT) == LOW && digitalRead(GPA) == LOW && digitalRead(GPB) == LOW;
  if (statechange) {
    initialize = true;
    state = ++state % 5;
    delay(500);
  }
  
  switch(state) {
  case 0:
    FastLED.clear();
    FastLED.show();
    break;
  case 1:
    rainbow_loop();
    break;
  case 2:
    chaos_loop();
    break;
  case 3:
    dice_loop();
    break;
  case 4:
    rubix_loop();
    break;
  }
}



void rainbow_loop()
{
  static uint8_t hue = 0;
  FastLED.showColor(CHSV(hue++, 255, 255)); 
  delay(10);
}

void chaos_loop()
{
  int chaos_select = random(0, NUM_LEDS);
  leds[chaos_select] = (CRGB) random(1, 0x1000000);
  FastLED.show();
}

unsigned long dice_updates = 150000;
void dice_loop()
{
  dice_updates--;
  if (dice_updates == 0) {
    roll(random(1,7), 0, leds);
    mirror(0, leds);
    FastLED.show();
    dice_updates = 150000;
  }
}

byte rubix_blinking = 0;
// SUPER UGLY HACK, NO GURANTEES THAT THIS WILL WORK ON ANYTHING AT ALL
unsigned long rubix_updates = 15000;
bool rubix_lightstate = true;

void rubix_loop() {
  if (initialize == true) {
    initialize = false;
    randomSeed(analogRead(0));
    FastLED.clear();
    FastLED.show();

    // Make the solution memory structure
    setColor({0, 255, 255}, ORANGE, solution);
    setColor({1, 255, 255}, BLUE, solution);
    setColor({2, 255, 255}, RED, solution);
    setColor({3, 255, 255}, GREEN, solution);
    setColor({4, 255, 255}, YELLOW, solution);
    setColor({5, 255, 255}, WHITE, solution);
  
    memcpy(leds, solution, sizeof(CRGB) * NUM_LEDS);
  
    FastLED.show();
    delay(300);
    int quotient = 3;
    
    for(int i = 0; i < quotient; i++) {
      rubixRot(random(0, 6), 0);
      FastLED.show();
    }
  }
  
  rubix_updates--;
  if (rubix_updates == 0) {
    Serial.write("blink\n");
    (rubix_lightstate) ? setColor({rubix_blinking, 1, 1}, (CRGB) 0x00, leds) : setColor({rubix_blinking, 1, 1}, solution[decodeLED({rubix_blinking, 1, 1})], leds);
    rubix_lightstate = (rubix_lightstate) ? false : true;
    rubix_updates = 15000;
    FastLED.show();
  }

  if (digitalRead(GPRIGHT) == LOW) {
    Serial.write("RIGHT\n");
    setColor({rubix_blinking, 1, 1}, solution[decodeLED({rubix_blinking, 1, 1})], leds);
    rubix_lightstate = true;
    rubix_blinking = CONTROLDIR[rubix_blinking][EAST];
    FastLED.show();
    delay(300);
  }
  else if (digitalRead(GPLEFT) == LOW) {
    Serial.write("LEFT\n");
    setColor({rubix_blinking, 1, 1}, solution[decodeLED({rubix_blinking, 1, 1})], leds);
    rubix_lightstate = true;
    rubix_blinking = CONTROLDIR[rubix_blinking][WEST];
    FastLED.show();
    delay(300);
  }
  else if (digitalRead(GPA) == LOW) {
    rubixRot(rubix_blinking, 0);
    FastLED.show();
    delay(300);
  }
  else if (digitalRead(GPB) == LOW) {
    rubixRot(rubix_blinking, 1);
    FastLED.show();
    delay(300);
  }
  
  
  if (memcmp(leds, solution, sizeof(CRGB) * NUM_LEDS) == 0) {
      FastLED.clear();
      FastLED.show();
      FastLED.delay(300);
      memcpy(leds, solution, sizeof(CRGB) * NUM_LEDS);
      FastLED.show();
      FastLED.delay(300);
  }
}





void roll(byte n, byte side, CRGB* leds) {
  setColor({side, 255, 255}, (CRGB) 0, leds);
  switch (n) {
    case 1:
      setColor({side,1,1}, DICE_COLOR, leds);
      break;
    case 2:
      setColor({side,2,0}, DICE_COLOR, leds);
      setColor({side,0,2}, DICE_COLOR, leds);
      break;
    case 3:
      setColor({side,2,0}, DICE_COLOR, leds);
      setColor({side,1,1}, DICE_COLOR, leds);
      setColor({side,0,2}, DICE_COLOR, leds);
      break;
    case 4:
      setColor({side,0,0}, DICE_COLOR, leds);
      setColor({side,2,0}, DICE_COLOR, leds);
      setColor({side,0,2}, DICE_COLOR, leds);
      setColor({side,2,2}, DICE_COLOR, leds);
      break;
    case 5:
      setColor({side,0,0}, DICE_COLOR, leds);
      setColor({side,2,0}, DICE_COLOR, leds);
      setColor({side,1,1}, DICE_COLOR, leds);
      setColor({side,0,2}, DICE_COLOR, leds);
      setColor({side,2,2}, DICE_COLOR, leds);
      break;
    case 6:
      setColor({side, 0, 255}, DICE_COLOR, leds);
      setColor({side, 2, 255}, DICE_COLOR, leds);
      break;
   default:
      printError((CRGB) 0xFF0000, leds);
      break;
  }
}



void rubixRot(byte side, bool dir)
{
  LEDSelect origin = {side, 0, 0};
  getNeighborLED(&origin, NORTH, &origin);

  if(dir == 0){
    new_rotaterot(origin, METAROTTODIR[side], 3 , leds);
    rotate(side, 0, 2, leds);
  }
  else{
    for(byte i = 0; i < 3; i++){
      rubixRot(side,0);
    }
  }
}