//Code By A.Tedds.
//Using Adafruit Library Files. Thank you.

//Usage of tft.drawRect(start position width 0-240,Start position Height 0-320,size 0-240,size 0-320, Color)

#include <Adafruit_GFX_AS.h>    // Core graphics library
#include <Adafruit_ILI9341_8bit_AS.h> // Hardware-specific library

#define LCD_CS A3 // Chip Select goes to Analog 3
#define LCD_CD A2 // Command/Data goes to Analog 2
#define LCD_WR A1 // LCD Write goes to Analog 1
#define LCD_RD A0 // LCD Read goes to Analog 0
#define LCD_RESET A4 // Can alternately just connect to Arduino's reset pin


// Assign human-readable names to some common 16-bit color values:

// With Changes for inversion !!

#define	BLACK   0xFFFF
#define	BLUE    0xFFE0
#define	GREEN   0xF81F
#define CYAN    0xF800
#define GRAY1	0x8410  
#define	RED     0x07FF
#define GRAY2	0x4208
#define MAGENTA 0x07E0
#define YELLOW  0x001F
#define WHITE   0x0000


Adafruit_ILI9341_8bit_AS tft(LCD_CS, LCD_CD, LCD_WR, LCD_RD, LCD_RESET);

int SCREEN_WIDTH = 249;
int SCREEN_HEIGHT = 320;

float d = 3; // was 3
float px[] = { -d,  d,  d, -d, -d,  d,  d, -d };
float py[] = { -d, -d,  d,  d, -d, -d,  d,  d };
float pz[] = { -d, -d, -d, -d,  d,  d,  d,  d };

float p2x[] = {0,0,0,0,0,0,0,0};
float p2y[] = {0,0,0,0,0,0,0,0};

float r[] = {0,0,0};

#define SHAPE_SIZE 2000
// Define how fast the cube rotates. Smaller numbers are faster.
// This is the number of ms between draws.
#define ROTATION_SPEED 0

void setup()
{
  tft.reset();
  delay(10);
  tft.begin(0x9341);
  tft.fillScreen(BLACK); 
}

void loop()
{
  drawCube();
  //delay(ROTATION_SPEED);
}

void drawCube()
{
  r[0]=r[0]+PI/180.0; // Add a degree
  r[1]=r[1]+PI/180.0; // Add a degree
  r[2]=r[2]+PI/180.0; // Add a degree
  if (r[0] >= 360.0*PI/180.0) r[0] = 0;
  if (r[1] >= 360.0*PI/180.0) r[1] = 0;
  if (r[2] >= 360.0*PI/180.0) r[2] = 0;

  for (int i=0;i<8;i++)
  {
    float px2 = px[i];
    float py2 = cos(r[0])*py[i] - sin(r[0])*pz[i];
    float pz2 = sin(r[0])*py[i] + cos(r[0])*pz[i];

    float px3 = cos(r[1])*px2 + sin(r[1])*pz2;
    float py3 = py2;
    float pz3 = -sin(r[1])*px2 + cos(r[1])*pz2;

    float ax = cos(r[2])*px3 - sin(r[2])*py3;
    float ay = sin(r[2])*px3 + cos(r[2])*py3;
    float az = pz3-150;

    p2x[i] = SCREEN_WIDTH/2+ax*SHAPE_SIZE/az;
    p2y[i] = SCREEN_HEIGHT/2+ay*SHAPE_SIZE/az;
  }

  tft.fillScreen(BLACK);
  
  for (int i=0;i<3;i++) 
   {
    tft.drawLine(p2x[i],p2y[i],p2x[i+1],p2y[i+1],WHITE);
    tft.drawLine(p2x[i+4],p2y[i+4],p2x[i+5],p2y[i+5],WHITE);
    tft.drawLine(p2x[i],p2y[i],p2x[i+4],p2y[i+4],WHITE);
//    delay(10);
//    tft.drawLine(p2x[i],p2y[i],p2x[i+1],p2y[i+1],BLACK);
//    tft.drawLine(p2x[i+4],p2y[i+4],p2x[i+5],p2y[i+5],BLACK);
//    tft.drawLine(p2x[i],p2y[i],p2x[i+4],p2y[i+4],BLACK);
   }
  
  tft.drawLine(p2x[3],p2y[3],p2x[0],p2y[0],WHITE);
  tft.drawLine(p2x[7],p2y[7],p2x[4],p2y[4],WHITE);
  tft.drawLine(p2x[3],p2y[3],p2x[7],p2y[7],WHITE);
//  delay(10);
//  tft.drawLine(p2x[3],p2y[3],p2x[0],p2y[0],BLACK);
//  tft.drawLine(p2x[7],p2y[7],p2x[4],p2y[4],BLACK);
//  tft.drawLine(p2x[3],p2y[3],p2x[7],p2y[7],BLACK);
}