#include <FastLED.h>
#define LED_PIN  5
#define NUM_ROWS 20
#define NUM_COLS 20
#define NUM_LEDS NUM_ROWS * NUM_COLS
#define MAX_AMOUNT_OF_SNAKES 8
#define MAX_AMOUNT_OF_APPLES 8
struct Coordinate{int x, y;};
CRGB leds[NUM_LEDS];

int textHue = 0;

//rechts,onder,links,boven
static int dx[4] = { 1, 0, -1, 0 };
static int dy[4] = { 0, -1, 0, 1 };

int amountOccupiedCoordinates;
Coordinate occupiedCoordinates[NUM_ROWS* NUM_COLS];
bool amountOfConflicts;

void printCor(Coordinate cor){
  Serial.print(cor.x);
  Serial.print(" ");
  Serial.println(cor.y);
}

unsigned long textStartTime;
char textToDisplay[] = " Press Yellow To Start!!  ";
int highScore = 0;


inline bool operator==(const Coordinate& lhs, const Coordinate& rhs)
{
    return lhs.x == rhs.x &&
           lhs.y     == rhs.y;
}


int uC(int x, int d){//updateCoordinate
  return((x+20+d)%20);
}

static uint8_t const font_A00[96][5] = {
	{ 0x00, 0x00, 0x00, 0x00, 0x00 },  // 20  32
	{ 0x00, 0x00, 0x4F, 0x00, 0x00 },  // 21  33  !
	{ 0x00, 0x07, 0x00, 0x07, 0x00 },  // 22  34  "
	{ 0x14, 0x7F, 0x14, 0x7F, 0x14 },  // 23  35  #
	{ 0x24, 0x2A, 0x7F, 0x2A, 0x12 },  // 24  36  $
	{ 0x23, 0x13, 0x08, 0x64, 0x62 },  // 25  37  %
	{ 0x36, 0x49, 0x55, 0x22, 0x50 },  // 26  38  &
	{ 0x00, 0x05, 0x03, 0x00, 0x00 },  // 27  39  '
	{ 0x00, 0x1C, 0x22, 0x41, 0x00 },  // 28  40  (
	{ 0x00, 0x41, 0x22, 0x1C, 0x00 },  // 29  41  )
	{ 0x14, 0x08, 0x3E, 0x08, 0x14 },  // 2A  42  *
	{ 0x08, 0x08, 0x3E, 0x08, 0x08 },  // 2B  43  +
	{ 0x00, 0x50, 0x30, 0x00, 0x00 },  // 2C  44  ,
	{ 0x08, 0x08, 0x08, 0x08, 0x08 },  // 2D  45  -
	{ 0x00, 0x60, 0x60, 0x00, 0x00 },  // 2E  46  .
	{ 0x20, 0x10, 0x08, 0x04, 0x02 },  // 2F  47  /
	{ 0x3E, 0x51, 0x49, 0x45, 0x3E },  // 30  48  0
	{ 0x00, 0x42, 0x7F, 0x40, 0x00 },  // 31  49  1
	{ 0x42, 0x61, 0x51, 0x49, 0x46 },  // 32  50  2
	{ 0x21, 0x41, 0x45, 0x4B, 0x31 },  // 33  51  3
	{ 0x18, 0x14, 0x12, 0x7F, 0x10 },  // 34  52  4
	{ 0x27, 0x45, 0x45, 0x45, 0x39 },  // 35  53  5
	{ 0x3C, 0x4A, 0x49, 0x49, 0x30 },  // 36  54  6
	{ 0x03, 0x01, 0x71, 0x09, 0x07 },  // 37  55  7
	{ 0x36, 0x49, 0x49, 0x49, 0x36 },  // 38  56  8
	{ 0x06, 0x49, 0x49, 0x29, 0x1E },  // 39  57  9
	{ 0x00, 0x36, 0x36, 0x00, 0x00 },  // 3A  58  :
	{ 0x00, 0x56, 0x36, 0x00, 0x00 },  // 3B  59  ;
	{ 0x08, 0x14, 0x22, 0x41, 0x00 },  // 3C  60  <
	{ 0x14, 0x14, 0x14, 0x14, 0x14 },  // 3D  61  =
	{ 0x00, 0x41, 0x22, 0x14, 0x08 },  // 3E  62  >
	{ 0x02, 0x01, 0x51, 0x09, 0x06 },  // 3F  63  ?
	{ 0x32, 0x49, 0x79, 0x41, 0x3E },  // 40  64  @
	{ 0x7E, 0x11, 0x11, 0x11, 0x7E },  // 41  65  A
	{ 0x7F, 0x49, 0x49, 0x49, 0x36 },  // 42  66  B
	{ 0x3E, 0x41, 0x41, 0x41, 0x22 },  // 43  67  C
	{ 0x7F, 0x41, 0x41, 0x22, 0x1C },  // 44  68  D
	{ 0x7F, 0x49, 0x49, 0x49, 0x41 },  // 45  69  E
	{ 0x7F, 0x09, 0x09, 0x09, 0x01 },  // 46  70  F
	{ 0x3E, 0x41, 0x49, 0x49, 0x7A },  // 47  71  G
	{ 0x7F, 0x08, 0x08, 0x08, 0x7F },  // 48  72  H
	{ 0x00, 0x41, 0x7F, 0x41, 0x00 },  // 49  73  I
	{ 0x20, 0x40, 0x41, 0x3F, 0x01 },  // 4A  74  J
	{ 0x7F, 0x08, 0x14, 0x22, 0x41 },  // 4B  75  K
	{ 0x7F, 0x40, 0x40, 0x40, 0x40 },  // 4C  76  L
	{ 0x7F, 0x02, 0x0C, 0x02, 0x7F },  // 4D  77  M
	{ 0x7F, 0x04, 0x08, 0x10, 0x7F },  // 4E  78  N
	{ 0x3E, 0x41, 0x41, 0x41, 0x3E },  // 4F  79  O
	{ 0x7F, 0x09, 0x09, 0x09, 0x06 },  // 50  80  P
	{ 0x3E, 0x41, 0x51, 0x21, 0x5E },  // 51  81  Q
	{ 0x7F, 0x09, 0x19, 0x29, 0x46 },  // 52  82  R
	{ 0x46, 0x49, 0x49, 0x49, 0x31 },  // 53  83  S
	{ 0x01, 0x01, 0x7F, 0x01, 0x01 },  // 54  84  T
	{ 0x3F, 0x40, 0x40, 0x40, 0x3F },  // 55  85  U
	{ 0x1F, 0x20, 0x40, 0x20, 0x1F },  // 56  86  V
	{ 0x3F, 0x40, 0x38, 0x40, 0x3F },  // 57  87  W
	{ 0x63, 0x14, 0x08, 0x14, 0x63 },  // 58  88  X
	{ 0x07, 0x08, 0x70, 0x08, 0x07 },  // 59  89  Y
	{ 0x61, 0x51, 0x49, 0x45, 0x43 },  // 5A  90  Z
	{ 0x7F, 0x41, 0x41, 0x00, 0x00 },  // 5B  91  [
	{ 0x15, 0x16, 0x7C, 0x16, 0x15 },  // 5C  92  '\'
	{ 0x00, 0x41, 0x41, 0x7F, 0x00 },  // 5D  93  ]
	{ 0x04, 0x02, 0x01, 0x02, 0x04 },  // 5E  94  ^
	{ 0x40, 0x40, 0x40, 0x40, 0x40 },  // 5F  95  _
	{ 0x00, 0x01, 0x02, 0x04, 0x00 },  // 60  96  `
	{ 0x20, 0x54, 0x54, 0x54, 0x78 },  // 61  97  a
	{ 0x7F, 0x48, 0x44, 0x44, 0x38 },  // 62  98  b
	{ 0x38, 0x44, 0x44, 0x44, 0x20 },  // 63  99  c
	{ 0x38, 0x44, 0x44, 0x48, 0x7F },  // 64 100  d
	{ 0x38, 0x54, 0x54, 0x54, 0x18 },  // 65 101  e
	{ 0x08, 0x7E, 0x09, 0x01, 0x02 },  // 66 102  f
	{ 0x0C, 0x52, 0x52, 0x52, 0x3E },  // 67 103  g
	{ 0x7F, 0x08, 0x04, 0x04, 0x78 },  // 68 104  h
	{ 0x00, 0x44, 0x7D, 0x40, 0x00 },  // 69 105  i
	{ 0x20, 0x40, 0x44, 0x3D, 0x00 },  // 6A 106  j
	{ 0x7F, 0x10, 0x28, 0x44, 0x00 },  // 6B 107  k
	{ 0x00, 0x41, 0x7F, 0x40, 0x00 },  // 6C 108  l
	{ 0x7C, 0x04, 0x18, 0x04, 0x78 },  // 6D 109  m
	{ 0x7C, 0x08, 0x04, 0x04, 0x78 },  // 6E 110  n
	{ 0x38, 0x44, 0x44, 0x44, 0x38 },  // 6F 111  o
	{ 0x7C, 0x14, 0x14, 0x14, 0x08 },  // 70 112  p
	{ 0x08, 0x14, 0x14, 0x18, 0x7C },  // 71 113  q
	{ 0x7C, 0x08, 0x04, 0x04, 0x08 },  // 72 114  r
	{ 0x48, 0x54, 0x54, 0x54, 0x20 },  // 73 115  s
	{ 0x04, 0x3F, 0x44, 0x40, 0x20 },  // 74 116  t
	{ 0x3C, 0x40, 0x40, 0x20, 0x7C },  // 75 117  u
	{ 0x1C, 0x20, 0x40, 0x20, 0x1C },  // 76 118  v
	{ 0x3C, 0x40, 0x38, 0x40, 0x3C },  // 77 119  w
	{ 0x44, 0x28, 0x10, 0x28, 0x44 },  // 78 120  x
	{ 0x0C, 0x50, 0x50, 0x50, 0x3C },  // 79 121  y
	{ 0x44, 0x64, 0x54, 0x4C, 0x44 },  // 7A 122  z
	{ 0x00, 0x08, 0x36, 0x41, 0x00 },  // 7B 123  {
	{ 0x00, 0x00, 0x7F, 0x00, 0x00 },  // 7C 124  |
	{ 0x00, 0x41, 0x36, 0x08, 0x00 },  // 7D 125  }
	{ 0x08, 0x08, 0x2A, 0x1C, 0x08 },  // 7E 126  ~
	{ 0x08, 0x1C, 0x2A, 0x08, 0x08 }   // 7F 127
};

//showtext(5, textToDisplay, sizeof(textToDisplay)/sizeof(*textToDisplay) )

void showtext(int yOff, char text[], size_t text_length, int xOff = 0, int white = 255){
  textHue++;
  int shift = 0;
  if (text_length > 4){ shift = (((millis() - textStartTime)/50)%(6*(text_length-4)));}
  for (int character = 0; character < text_length; character++){
    for (int i = 0; i < 7; i++){
      for (int j = 0; j < 5; j++){
        int x = character*6 + j - shift + xOff;
        int y = i + yOff;
        if (x >= 0 and x< 20 and y >= 0 and y< 20){
          leds[y * 20 + x] = CHSV (textHue,white,((font_A00[text[character]-' '][j] & ( 1 << i )) >> i)*255);//sets pixel
        }
      }
    }
  }
  return;
}

class Button{
  int pin;
  bool lastState;
  bool currentState;
  public:
    Button(int pin): pin(pin), lastState(false), currentState (false){
      pinMode(pin, INPUT);
    }
    void updateState(){
      lastState = currentState;
      currentState = digitalRead(pin);
    }
    bool isFalling(){
      return (lastState && !currentState);
    }
};
class Joystick{
  int xpin, ypin, direction ;
  public:
    Joystick(int xpin, int ypin): xpin(xpin), ypin(ypin){
      pinMode(xpin, INPUT);
      pinMode(ypin, INPUT);
      int direction = 4;
    }
    void updateDirection(){
      int  x = analogRead(xpin);
      int y = analogRead(ypin);
      Serial.println(direction);
      if (x < 200){direction = 0;}
      if (x > 800){direction = 2;}
      if (y > 800){direction = 1;}
      if (y < 200){direction = 3;}
    }
    int getDirection(){return direction;}
};
class Snake{
  bool active,alive;
  int direction,color;
  int length;
  Coordinate body[NUM_ROWS];//NUM_ROWS * NUM_COLS
  public:
    Snake(int X, int Y, int d, int c, bool a): direction(d), color(c), active(a), alive(a) {
      length = 1;
      body[0].x = X;
      body[0].y = Y;
      //extend();
      //extend();
    }
    void displayBody(){
      for(int bodyPart = 0; bodyPart < length; bodyPart++){
        leds[body[bodyPart].y * 20 + body[bodyPart].x] = CHSV( color, (bodyPart + 1 == length)? 255 : 70, 255);
      }
    }
    void extend(){
      body[length].x = uC(body[length-1].x,dx[direction]);
      body[length].y = uC(body[length-1].y,dy[direction]);
      length++;
    }
    void shorten(){
      for(int bodyPart = 0; bodyPart < length - 1; bodyPart++){
        body[bodyPart] = body[bodyPart + 1];
      }
      length--;
    }
    int getColor(){return color;}
    int getLength(){return length;}
    Coordinate getCoordinate(int index){return (body[index]);}
    void setDirection(int _direction){
      if (_direction == 4){return;}
      if (direction != 4 && ((_direction + direction)%2 != 0)){
        direction = _direction;
      }
    }
    bool isActive(){return active;}
    bool isAlive(){return alive;}
    void die(){alive = false;}
    void reset(int X, int Y, int d, int c, bool a){
      direction = d;
      color = c;
      active = a;
      alive = a;
      length = 1;
      body[0].x = X;
      body[0].y = Y;
    }
};

class Apple{
  Coordinate location;
  bool alive;
  public:
    Apple(){
      location.x = 0; 
      location.y = 0; 
      alive = false;
    }
    void spawnApple(int X, int Y){
      location.x = X;
      location.y = Y;
      alive = true;
    }
    void die(){alive = false;}
    bool isAlive(){return alive;}
    void showApple(){leds[location.y * 20 + location.x] = CRGB::White;}
    Coordinate getCoordinate(){return (location);}
};

Snake snakes[8] = {
  Snake(8,19,1,0, false), 
  Snake(13,19,1,32, true), 
  Snake(19,11,2,64, false), 
  Snake(19,6,2,96, false), 
  Snake(11,0,3,128, false), 
  Snake(6,0,3,160, false), 
  Snake(0,8,0,192, true), 
  Snake(0,13,0,224, true)
};

Apple apples[8];
Joystick joystick(A1, A0);
Snake snake1(8,8,3,212, 1);
Button pause_button(6);
Button select_button(7);
//Apple apple;
void setup() {
  randomSeed(1);
  Serial.begin(115200);
  FastLED.addLeds<NEOPIXEL, LED_PIN>(leds, NUM_LEDS);
}

int amountOfAliveSnakes(){
  int counter = 0;
  for (int snake = 0; snake < MAX_AMOUNT_OF_SNAKES; snake ++){
    if(snakes[snake].isAlive()){counter++;}
  }
  return counter;
}
void startSequence(){
  char drie[] = " 3 ";
  char twee[] = " 2 ";
  char een[] = " 1 ";
  showtext(6, drie, sizeof(drie)/sizeof(*drie),1);
  FastLED.show();
  delay(1000);
  showtext(6, twee, sizeof(twee)/sizeof(*twee),1);
  FastLED.show();
  delay(1000);
  showtext(6, een, sizeof(een)/sizeof(*een),1);
  FastLED.show();
  delay(1000);
}
void loop() {
  snakes[0].reset(8,19,1,0, true);
  snakes[1].reset(13,19,1,32, true);
  snakes[2].reset(19,11,2,64, true); 
  snakes[3].reset(19,6,2,96, true);
  snakes[4].reset(11,0,3,128, true); 
  snakes[5].reset(6,0,3,160, true); 
  snakes[6].reset(0,8,0,192, true); 
  snakes[7].reset(0,13,0,224, true);
  bool ended = false;
  bool paused = false;
  int ticks = 0;
  int nextMove = 0;
  Serial.println("new game");
  textStartTime = millis();
  while(!(select_button.isFalling())){
    updateInputs();
    FastLED.clear();
    showtext(6, textToDisplay, sizeof(textToDisplay)/sizeof(*textToDisplay));
    FastLED.show();
  }
  FastLED.clear();
  while(!ended){
    if(!ended && !paused){
      Serial.println("start");
      startSequence();
      updateFrame();
    }
    while(!ended && !paused){
      //Serial.println(ticks);
      ticks += 1;
      updateInputs();
      if(pause_button.isFalling() || select_button.isFalling()){
        Serial.println("pause");
        paused = true;
        break;
      }
      if (nextMove <= ticks){
        nextMove += amountOfAliveSnakes() * 2;
        updateGame();
      }
      
      updateFrame();
      if (amountOfAliveSnakes() < 2){
        ended = true;
      }
    }
    if(!ended){
      updateInputs();
      if(select_button.isFalling()){
        paused = false;
      }
      if(pause_button.isFalling()){
        ended = true;
      }
    }
  }
  delay(1000);
  int score = 0;
  for (int snake = 0; snake < MAX_AMOUNT_OF_SNAKES; snake ++){ //8
    if (snakes[snake].isActive() && snakes[snake].isAlive()){
      score = snakes[snake].getLength();
      textHue = snakes[snake].getColor();
    }
  }
  if (score > highScore){
    highScore = score;
  }
  char textScore[3] = "000";
  char textHighScore[3] = "000";
  textScore[0] = (score / 100)+48;
  textScore[1] = ((score % 100)/10)+48;
  textScore[2] = (score % 10)+48;
  textHighScore[0] = (score / 100)+48;
  textHighScore[1] = ((score % 100)/10)+48;
  textHighScore[2] = (score % 10)+48;
  FastLED.clear();
  showtext(2, textScore, sizeof(textScore)/sizeof(*textScore),1);
  showtext(11, textHighScore, sizeof(textHighScore)/sizeof(*textHighScore),1,0);
  FastLED.show();
  Serial.println("game over");
  while (!pause_button.isFalling() && !select_button.isFalling()){
    updateInputs();
  }
}

void updateInputs(){
  pause_button.updateState();
  select_button.updateState();
  joystick.updateDirection();
}

void updateGame(){
  snakes[1].setDirection(joystick.getDirection());

  amountOccupiedCoordinates = 0;
  for (int snake = 0; snake < MAX_AMOUNT_OF_SNAKES; snake ++){ //8
    if (snakes[snake].isActive() && snakes[snake].isAlive()){
      snakes[snake].extend();
      for (int i = 0; i < snakes[snake].getLength(); i++){
        occupiedCoordinates[amountOccupiedCoordinates] = snakes[snake].getCoordinate(i);
        amountOccupiedCoordinates++;
      }
    }
  }
  //snakes now all moved one forward. all coordinates that contain snake are stored.

  for (int snake = 0; snake < MAX_AMOUNT_OF_SNAKES; snake ++){ //8
    if (snakes[snake].isActive() && snakes[snake].isAlive()){
      Coordinate head = snakes[snake].getCoordinate(snakes[snake].getLength() - 1);
      amountOfConflicts = false;
      for (int i = 0; i < amountOccupiedCoordinates; i++){
        if(head == occupiedCoordinates[i]){
          if (amountOfConflicts){
            snakes[snake].die();
            snakes[snake].shorten();
            break;
          }
          amountOfConflicts = true;     
        }
      }
    }
  }
  // kill all snakes that should die
  
  for (int snake = 0; snake < MAX_AMOUNT_OF_SNAKES; snake ++){ //8
    if (snakes[snake].isActive() && snakes[snake].isAlive()){
      bool ateApple = false;
      Coordinate head = snakes[snake].getCoordinate(snakes[snake].getLength() - 1);
      for (int apple = 0; apple < MAX_AMOUNT_OF_APPLES; apple ++){
        if(head == apples[apple].getCoordinate() && apples[apple].isAlive()){
          ateApple = true;
          apples[apple].die();
          break;
        }
      }
      if (!ateApple){
        snakes[snake].shorten();
      }
    }
  }
  //shorten snakes again, unless they ate apple

  for (int apple = 0; apple < MAX_AMOUNT_OF_APPLES; apple ++){
    if (!(apples[apple].isAlive()) && random(100) < 2){
      bool freeCoordinate = false;
      Coordinate appleLocation;
      while(!freeCoordinate){
        appleLocation.x = random(20);
        appleLocation.y = random(20);
        freeCoordinate = true;
        for (int i = 0; i < amountOccupiedCoordinates; i++){
          if (appleLocation == occupiedCoordinates[i]){
            freeCoordinate = false;
            break;
          }
        }
      }
      apples[apple].spawnApple(appleLocation.x,appleLocation.y);
    }
  }
  //spawn apples
}



void updateFrame(){
  FastLED.clear();
  for (int apple = 0; apple < MAX_AMOUNT_OF_APPLES; apple ++){
    if (apples[apple].isAlive()){
      apples[apple].showApple();
    }
  }

  for (int snake = 0; snake < MAX_AMOUNT_OF_SNAKES; snake ++){ //8
    if (snakes[snake].isActive() && snakes[snake].isAlive()){
      snakes[snake].displayBody();
    }
  }
  FastLED.show();
}