//Analog Clock Sketch created by Embedded Downloads LTD
//To view the full tutorial go to www.embeddeddownloads.com
#include <Adafruit_GFX_AS.h>    // Core graphics library
#include <Adafruit_ILI9341_8bit_AS.h>// Hardware-specific library
#include <TouchScreen.h>

// The control pins for the LCD can be assigned to any digital or
// analog pins...but we'll use the analog pins as this allows us to
// double up the pins with the touch screen (see the tft paint example).
#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

// When using the BREAKOUT BOARD only, use these 8 data lines to the LCD:
// For the Arduino Uno, Duemilanove, Diecimila, etc.:
//   D0 connects to digital pin 8  (Notice these are
//   D1 connects to digital pin 9   NOT in order!)
//   D2 connects to digital pin 2
//   D3 connects to digital pin 3
//   D4 connects to digital pin 4
//   D5 connects to digital pin 5
//   D6 connects to digital pin 6
//   D7 connects to digital pin 7
// For the Arduino Mega, use digital pins 22 through 29
// (on the 2-row header at the end of the board).

// These are the pins for the shield!
#define YP A1  // must be an analog pin, use "An" notation!
#define XM A2  // must be an analog pin, use "An" notation!
#define YM 7   // can be a digital pin
#define XP 6   // can be a digital pin

#define MINPRESSURE 10
#define MAXPRESSURE 1000

// For better pressure precision, we need to know the resistance
// between X+ and X- Use any multimeter to read it
// For the one we're using, its 300 ohms across the X plate
TouchScreen ts = TouchScreen(XP, YP, XM, YM, 300);

// Assign human-readable names to some common 16-bit color values:
#define	BLACK   0x0000
#define	BLUE    0x001F
#define	RED     0xF800
#define	GREEN   0x07E0
#define CYAN    0x07FF
#define MAGENTA 0xF81F
#define YELLOW  0xFFE0
#define WHITE   0xFFFF

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


void setup() {
  Serial.begin(9600);
  tft.reset();
  delay(10);
  tft.begin(0x9341);
  uint16_t identifier = tft.readID(); 
  
  Serial.println(identifier, HEX);
   tft.fillScreen(BLACK);
   
     Serial.println("tft LCD test");
  Serial.print("tft size is ");
  Serial.print(tft.width());
  Serial.print("x");
  Serial.println(tft.height());
  

}
typedef struct
    {
     int x,y;
    } point;
    

int key;
 float a=0.0,b=0.0;
//float ax,ay,az;
short lx[8]={1,1,1,1,-1,-1,-1,-1};
short ly[8]={1,1,-1,-1,1,1,-1,-1};
short lz[8]={1,-1,1,-1,1,-1,1,-1};
float lxn[8],lyn[8],lzn[8];
float xd[8],yd[8];

float c=2.0;
int ti=1;
float xt,yt,zt;
float mr[3][3];
float znm;


int face[6][4]={{4,0,1,5},{1,0,2,3},{5,1,3,7},{4,5,7,6},{0,4,6,2},{3,2,6,7}};
int facec[6]={10,20,30,20,30,10};

point points[10];


// Quick sort
int tri[12];
int zface[12];

void QuickSort(int deb, int fin)
{
  int   i=deb;
  int   j=fin;
  double milieu=256*zface[tri[(deb+fin)/2]];
  int   temp;

  while(i<=j)
  {
    while(256*zface[tri[i]]> milieu) i++;
    while(256*zface[tri[j]]< milieu) j--;

    if(i<=j)
    {
      temp=tri[i];
      tri[i]=tri[j];
      tri[j]=temp;
      i++; j--;
    }
  }

  if(i<fin) QuickSort(i,fin);
  if(deb<j) QuickSort(deb,j);
}

// ----------


void loop(){
   
    ti++;
    // ALGO Adapté depuis http://josh83.pagesperso-orange.fr/demoz/3dengfr.htm
    
    a=a-0.05;
    b=b+0.05;
    for(int n=0;n<8;n++)    {
      lxn[n]=lx[n]*cos(b)+ly[n]*sin(a)*sin(b)+lz[n]*cos(a)*sin(b);
      lyn[n]=ly[n]*cos(a)-lz[n]*sin(a);
      lzn[n]=-lx[n]*sin(b)+ly[n]*sin(a)*cos(b)+lz[n]*cos(a)*cos(b)+5;
      xd[n]=(240/2)+(240*lxn[n]*c)/(2*lzn[n]);
      yd[n]=(320/2)+(320*lyn[n]*c)/(2*lzn[n]);
    }


    int facev=0;
    for(int m=0;m<6;m++) {
      znm=(xd[face[m][2]]-xd[face[m][1]])*(yd[face[m][1]]-yd[face[m][3]])-(yd[face[m][2]]-yd[face[m][1]])*(xd[face[m][1]]-xd[face[m][3]]);
      //znm=2; only zsorting
      if(znm>0)     {
        zface[m]=lzn[face[m][0]]+lzn[face[m][1]]+lzn[face[m][2]]+lzn[face[m][3]]+lzn[face[m][4]];
        tri[facev]=m;
        facev++;
        }
    }
    QuickSort(0,facev-1);


    
    tft.fillScreen(BLACK);
    
    //for(int mb=0;mb<6;mb++) {
    for(int mb=0;mb<facev;mb++) {
      int color = WHITE;
      
      switch (tri[mb]) {
         case 0:
           color = WHITE;
           break;
         case 1:
           color = RED;
           break;
         case 2: 
           color = BLUE;
           break;
         case 3:
           color = YELLOW;
           break;
         case 4:
           color = GREEN;
           break;
         case 5:
           color = CYAN;
           break;
         default:
         break;
      } 
    
    points[0].x = xd[face[tri[mb]][0]];
    points[0].y = yd[face[tri[mb]][0]];
    points[1].x = xd[face[tri[mb]][1]];
    points[1].y = yd[face[tri[mb]][1]];
    points[2].x = xd[face[tri[mb]][2]];
    points[2].y = yd[face[tri[mb]][2]];
    points[3].x = xd[face[tri[mb]][3]];
    points[3].y = yd[face[tri[mb]][3]];
          // FACE 1
      tft.fillTriangle(
      points[0].x,    points[0].y,
      points[1].x,    points[1].y,
      points[2].x,    points[2].y,
      color);
      tft.fillTriangle(
      points[0].x,    points[0].y,
      points[2].x,    points[2].y,
      points[3].x,    points[3].y,
      color);
      
      //delay(500);
      //tft.fillTriangle(points[1].x,points[1].y,points[2].x,points[2].y,points[3].x,points[3].y,WHITE);
     // tft.fillTriangle(xd[3],yd[3],xd[4],yd[4],xd[1],yd[1], WHITE);
      
      //FillPoly(points,4,facec[mb]);
    }
  
}