#include <SPI.h>
#include <Adafruit_FT6206.h>
#include <Adafruit_ILI9341.h>
#include <Adafruit_GFX.h>

/* Defines */
#define TFT_CS 10
#define TFT_DC 9

Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC);

extern uint8_t SmallFont[];
extern uint8_t BigFont[];
/*

short pageId = 0;

int buttons[MAX_BUTTONS][5];
short bCount = 0;
short startGameButton = -1;
*/

/* Global Vars */
float last = 0;
const float Pi = 3.14159;
const int gameSpeed = 25;

enum sides {
  left,
  right
};

/* Player Vars */
bool ingame[4] {false,false,false,false};
float x[4];
float y[4];
int omega[4] = {180,180,180,180};
int theta[4] = {180,180,180,180};


void setup() {
  Serial.begin(9600);
  tft.begin();
  tft.fillScreen(ILI9341_BLACK);
  last = millis();

  ingame[0] = true;
  x[0] = 50.0;
  y[0] = 50.0;
}

void loop() {
    float mod = millis() - last;
    last = millis();
    mod /= 1000;
    for(byte i=0;i <4;i++) if(ingame[i] == true) RenderPlayer(mod,0);
   if (Serial.available() > 0) {

          // read the incoming byte:
          byte incomingByte = Serial.read();
          if(incomingByte == 'a'){
            MovePlayer(left,mod,0);

          }
          else if(incomingByte == 'd') {
            MovePlayer(right,mod,0);
          }
  }
  delay(1000/gameSpeed);
}

void MovePlayer(sides side,float mod,int id) {
  if(side == left)
    theta[id] -= omega[id]*mod;
  else
    theta[id] += omega[id]*mod;
}

void RenderPlayer(float mod,byte id) {
    tft.fillRect(x[id],y[id],2,2,0xffff);

    x[id] += gameSpeed*cos(Pi*theta[id]/omega[id])*mod;
    y[id] += gameSpeed*sin(Pi*theta[id]/omega[id])*mod;
    Serial.println(mod);
    Serial.println(theta[id]);
}

void PrintText(int x,int y) {
  tft.setCursor(x,y);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(3);
  tft.println("Blue Wins");
}

/*

void drawHomeScreen() {
  pageId = 1;
  myGLCD.setBackColor(0,0,0);
  PrintText("CatchMe",CENTER, 10,255,255,255,BigFont);
  startGameButton = CreateButton("Start Game",25,25,100,50, 255,255,255 , 0,0,0);
}

void PrintText(text[], x,y,r = 255,g = 255,b = 255, font[] = SmallFont) {
  myGLCD.setColor(r,g,b);
  myGLCD.setFont(font);
  myGLCD.print(text,x,y);
}

short CreateButton(text[], x,y, width,height, boxr = 255, boxg = 255,boxb = 255, r = 255, g = 255, b = 255) {
  myGLCD.setColor(boxr, boxg, boxb);
  myGLCD.fillRoundRect (x, y, width, height);
  myGLCD.setFont(BigFont);
  myGLCD.setBackColor(r, g, b);
  myGLCD.print(text, x, y);
  buttons[bCount][0] = x;
  buttons[bCount][1] = y;
  buttons[bCount][2] = width;
  buttons[bCount][3] = height;
  buttons[bCount][4] = pageId;
  return bCount++;
}

void loop() {
  if (myTouch.dataAvailable()) {
      myTouch.read();
      x=myTouch.getX(); // X coordinate where the screen has been pressed
      y=myTouch.getY(); // Y coordinates where the screen has been pressed

      short pressedButton = -1;
      for(short i = 0;i < MAX_BUTTONS;i++) {
        if(buttons[i][4] != pageId)
          continue;

        if(x >= buttons[i][0] && x <= (buttons[i][0] + buttons[i][2]) && y >= buttons[i][1] && y <= (buttons[i][1] + buttons[i][3])) {
          pressedButton = i;
          break;
        }
      }

      switch(pressedButton) {
        case startGameButton: {
          // Start Game
        }
      }
  }
}*/