#include <TFT_eSPI.h>
#include <SPI.h>       // this is needed for display
#include <Wire.h>      // this is needed for FT6206
#include <Adafruit_FT6206.h>
#include <QList.h>
#include <ArxSmartPtr.h>
float PIE = 3.14;
// The FT6206 uses hardware I2C (SCL/SDA)
Adafruit_FT6206 ctp = Adafruit_FT6206();

// The display also uses hardware SPI, plus #9 & #10
#define TFT_CS 15
#define TFT_DC 2
#define TFT_MOSI 23
#define TFT_SCLK 18
//Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC);
#define ILI9341_RED TFT_RED
#define ILI9341_YELLOW TFT_YELLOW
#define ILI9341_GREEN TFT_GREEN
#define ILI9341_CYAN TFT_CYAN
#define ILI9341_BLUE TFT_BLUE
#define ILI9341_MAGENTA TFT_MAGENTA

TFT_eSPI tft = TFT_eSPI();       // Invoke custom library
int period = 1000;
unsigned long time_now = 0;

int randomColor() {
  switch (random(0, 14)) {
    case 0:
      return TFT_RED;
      break;
    case 1:
      return TFT_NAVY;
      break;
    case 2:
      return TFT_DARKGREEN;
      break;
    case 3:
      return TFT_DARKCYAN;
      break;
    case 4:
      return TFT_MAROON;
      break;
    case 5:
      return TFT_PURPLE;
      break;
    case 6:
      return TFT_PINK;
      break;
    case 7:
      return TFT_LIGHTGREY;
      break;
    case 8:
      return TFT_YELLOW;
      break;
    case 9:
      return TFT_GREEN;
      break;
    case 10:
      return TFT_CYAN;
      break;
    case 11:
      return TFT_MAGENTA;
      break;
    case 12:
      return TFT_WHITE;
      break;
    case 13:
      return TFT_BLUE;
      break;
  }
}

class shape {
  public:
    float xloc; //defined but not initialised
    float yloc;
    int size;
    int color;
    float speed;
    float direction;
    ~shape() {
    }
    shape() {
      xloc = 160; //initialised!
      yloc = 120;
      size = 5;
      color = TFT_WHITE;
      speed = 1;
      direction = 0;
    }
    shape(float xloc, float yloc, int size, int color, float speed, float direction) {
      this->xloc = xloc; //initialised to the input
      this->yloc = yloc;
      this->size = size;
      this->color = color;
      this->speed = speed;
      this->direction = direction;
    }
    void move() {
      xloc = xloc + (cos(direction * PIE / 180) * speed);
      yloc = yloc + (sin(direction * PIE / 180) * speed);
      if (xloc < size) {
        direction = (180 - direction);
      }
      if (xloc > (480 - size)) {
        direction = (180 - direction);
      }
      if (yloc < size) {
        direction = -direction;
      }
      if (yloc > (320 - size)) {
        direction = -direction;
      }
    }
    virtual void draw() = 0;
    void run() {
      move();
      draw();
    }
    float returnX() {
      return xloc;
    }
    float returnY() {
      return yloc;
    }
    float returnR() {
      return size;
    }
    void bounceAgainst(shape & i) {
      direction = ((atan2(i.returnY() - yloc, i.returnX() - xloc) * 180 / 3.1415) - direction) * 2 + direction + 180;
    }
    void checkCollision(QList<std::shared_ptr<shape>> & tempShapeList, int self) {
      for (int i = 0; i < tempShapeList.size(); i++) {
        if (i != self) {
          if ((xloc - tempShapeList.at(i)->returnX()) * (xloc - tempShapeList.at(i)->returnX()) + (yloc - tempShapeList.at(i)->returnY()) * (yloc - tempShapeList.at(i)->returnY()) <= (size + tempShapeList.at(i)->returnR()) * (size + tempShapeList.at(i)->returnR())) {
            //bounceAgainst(tempShapeList.at(i));
            tempShapeList.at(i)->bounceAgainst(*this); //note we are sending 'this asteroid
          }
        }
      }
    }
};

class square : public shape {
  public:
    square(float xloc, float yloc, int size, int color, float speed, float direction): shape(xloc, yloc, size, color, speed, direction) {}
    void draw()override {
      tft.drawRect(xloc - size - 2, yloc - size - 2, size * 2 + 4, size * 2 + 4, TFT_BLACK);
      tft.drawRect(xloc - size - 1, yloc - size - 1, size * 2 + 2, size * 2 + 2, TFT_BLACK);
      tft.fillRect(xloc - size, yloc - size, size * 2, size * 2, color);
    }
};

class circle : public shape {
  public:
    circle(float xloc, float yloc, int size, int color, float speed, float direction): shape(xloc, yloc, size, color, speed, direction) {}
    void draw()override {
     tft.drawCircle(xloc, yloc, size + 1, TFT_BLACK);
     tft.drawCircle(xloc, yloc, size + 2, TFT_BLACK);
     tft.fillCircle(xloc, yloc, size + 2, TFT_BLACK);
     tft.fillCircle(xloc, yloc, size, color);
    }
};
class triangle: public shape {
  public:
    triangle(float xloc, float yloc, int size, int color, float speed, float direction): shape(xloc, yloc, size, color, speed, direction) {}
    void draw() override {
     tft.drawTriangle(xloc + (size +1) * cos((direction * 3.1415 / 180) + (2 * 3.1415 / 3)),  yloc + (size+1) * sin((direction * 3.1415 / 180) + (2 * 3.1415 / 3)),
                       xloc + (size +1) * cos((direction * 3.1415 / 180) + (4 * 3.1415 / 3)), yloc + (size+1) * sin((direction * 3.1415 / 180) + (4 * 3.1415 / 3)),
                       xloc + (size +1) * cos((direction * 3.1415 / 180)),                    yloc + (size+1) * sin((direction * 3.1415 / 180)),
                       TFT_BLACK);
     tft.drawTriangle(xloc + (size +2) * cos((direction * 3.1415 / 180) + (2 * 3.1415 / 3)),  yloc + (size+2) * sin((direction * 3.1415 / 180) + (2 * 3.1415 / 3)),
                      xloc + (size +2) * cos((direction * 3.1415 / 180) + (4 * 3.1415 / 3)),  yloc + (size+2) * sin((direction * 3.1415 / 180) + (4 * 3.1415 / 3)),
                      xloc + (size +2) * cos((direction * 3.1415 / 180)),                     yloc + (size+2) * sin((direction * 3.1415 / 180)),
                      TFT_BLACK);
     tft.drawTriangle(xloc + (size +3) * cos((direction * 3.1415 / 180) + (2 * 3.1415 / 3)),yloc + (size+3) * sin((direction * 3.1415 / 180) + (2 * 3.1415 / 3)),
                      xloc + (size +3) * cos((direction * 3.1415 / 180) + (4 * 3.1415 / 3)),yloc + (size+3) * sin((direction * 3.1415 / 180) + (4 * 3.1415 / 3)),
                      xloc + (size +3) * cos((direction * 3.1415 / 180)),                   yloc + (size+3) * sin((direction * 3.1415 / 180)),
                      TFT_BLACK);   
     tft.fillTriangle(xloc + (size +3) * cos((direction * 3.1415 / 180) + (2 * 3.1415 / 3)),yloc + (size+3) * sin((direction * 3.1415 / 180) + (2 * 3.1415 / 3)),
                      xloc + (size +3) * cos((direction * 3.1415 / 180) + (4 * 3.1415 / 3)),yloc + (size+3) * sin((direction * 3.1415 / 180) + (4 * 3.1415 / 3)),
                      xloc + (size +3) * cos((direction * 3.1415 / 180)),                   yloc + (size+3) * sin((direction * 3.1415 / 180)),
                      TFT_BLACK); 
     tft.fillCircle(xloc, yloc, size + 3, TFT_BLACK);
     tft.drawTriangle(xloc + (size) * cos((direction * 3.1415 / 180) + (2 * 3.1415 / 3)), yloc + (size) * sin((direction * 3.1415 / 180) + (2 * 3.1415 / 3)),
                      xloc + (size) * cos((direction * 3.1415 / 180) + (4 * 3.1415 / 3)), yloc + (size) * sin((direction * 3.1415 / 180) + (4 * 3.1415 / 3)),
                      xloc + (size) * cos((direction * 3.1415 / 180)),                    yloc + (size) * sin((direction * 3.1415 / 180)),
                      color);             
     tft.fillTriangle(xloc + (size) * cos((direction * 3.1415 / 180) + (2 * 3.1415 / 3)), yloc + (size) * sin((direction * 3.1415 / 180) + (2 * 3.1415 / 3)),
                      xloc + (size) * cos((direction * 3.1415 / 180) + (4 * 3.1415 / 3)), yloc + (size) * sin((direction * 3.1415 / 180) + (4 * 3.1415 / 3)),
                      xloc + (size) * cos((direction * 3.1415 / 180)),                    yloc + (size) * sin((direction * 3.1415 / 180)),
                      color);
    }
};
QList<std::shared_ptr<shape>> shapeList;
void setup() {
  pinMode(27, INPUT);
  tft.init();
  tft.fillScreen(TFT_BLACK);
  tft.setRotation(1);
  for (int i = 0; i < 5; i++) {
    std::shared_ptr<shape> newPointerObject(new triangle(random(5, 315), random(5, 235), 10, randomColor(), 1, random(0, 360)));
    shapeList.push_back(newPointerObject);
  }
  for (int i = 0; i < 3; i++) {
    std::shared_ptr<shape> newPointerObject(new square(random(5, 315), random(5, 235), 10, randomColor(), 1, random(0, 360)));
    shapeList.push_back(newPointerObject);
  }
  for (int i = 0; i < 3; i++) {
    std::shared_ptr<shape> newPointerObject(new circle(random(5, 315), random(5, 235), 10, randomColor(), 1, random(0, 360)));
    shapeList.push_back(newPointerObject);
  }
}
int fps = 0;
void loop() {
  fps++;
  if (millis() >= time_now + period) {
    time_now += period;
    tft.drawString("fps: " + String(fps) + " ", 0, 0, 2);
    fps = 0;
  }
  if (digitalRead(27) == HIGH) {
    //shapeList.push_back(b);
  }
  for (int i = 0; i < shapeList.size(); i++) {
    shapeList.at(i)->run();
    shapeList.at(i)->checkCollision(shapeList, i); //i indicates which shape on the list is its self
  }
}