#include <UTFT.h>

UTFT myGLCD(ITDB32S,38,39,40,41);


#define MINPRESSURE 10
#define MAXPRESSURE 1000


// 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

// Declare which fonts we will be using
extern uint8_t SmallFont[];


void setup() {
  Serial.begin(9600);
// Setup the LCD
  myGLCD.InitLCD();
  myGLCD.setFont(SmallFont);
  

}
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=1.0;
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);
  
 // draw();
}

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


//void loop(){
//   
//   
//    // ALGO Adapté depuis http://josh83.pagesperso-orange.fr/demoz/3dengfr.htm
//
//
//    
//  // a point object holds x y and z coordinates
//  TSPoint p = ts.getPoint();
//  
//  // we have some minimum pressure we consider 'valid'
//  // pressure of 0 means no pressing!
//  if (p.z > ts.pressureThreshhold) {
//     Serial.print("X = "); Serial.print(p.x);
//     Serial.print("\tY = "); Serial.print(p.y);
//     Serial.print("\tPressure = "); Serial.print(p.z);
//     
//     a = 0.05; //p.x / 1024.0;
//     b = 0.05; //p.y / 1024.0;
//     
//     Serial.print(" a = "); Serial.print(a);
//     Serial.print("\tb = "); Serial.println(b);
//     
//     draw();
//  }
//
//}
void loop() {
  //delay(100);
   //TSPoint p = ts.getPoint();
//  
  // we have some minimum pressure we consider 'valid'
  // pressure of 0 means no pressing!
  //if (p.z > ts.pressureThreshhold) {
        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]=(320/2)+(320*lxn[n]*c)/(2*lzn[n]);
      yd[n]=(240/2)+(240*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);

myGLCD.clrScr();
    
    //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
      myGLCD.setColor(color);
      myGLCD.drawLine(
      points[0].x,    points[0].y,
      points[1].x,    points[1].y);
      myGLCD.drawLine(
      points[1].x,    points[1].y,
      points[2].x,    points[2].y);
      myGLCD.drawLine(
      points[2].x,    points[2].y,
      points[3].x,    points[3].y);
      myGLCD.drawLine(
      points[3].x,    points[3].y,
      points[0].x,    points[0].y);
      
      //delay(500);
      //myGLCD.fillTriangle(points[1].x,points[1].y,points[2].x,points[2].y,points[3].x,points[3].y,WHITE);
     // myGLCD.fillTriangle(xd[3],yd[3],xd[4],yd[4],xd[1],yd[1], WHITE);
      
      //FillPoly(points,4,facec[mb]);
    }
//   }
}