#include "Cube.h"

Cube::Cube(Ecran * _ecran)
{
  ecran = _ecran;
}

void Cube::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 Cube::draw()
{
  int nb = 0;

  while (nb < 200)
  {
    ecran->LcdClear();
    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] = (LCD_X / 2) 
            + (LCD_X * lxn[n] * c) / (2 * lzn[n]) * 2;
      yd[n] = (LCD_Y / 2) 
            + (LCD_Y * lyn[n] * c) / (2 * lzn[n]) * 2;
    }


    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);

    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
      ecran->drawFace(
        points[0].x,    points[0].y,
        points[1].x,    points[1].y,
        points[2].x,    points[2].y);
      ecran->drawFace(
        points[0].x,    points[0].y,
        points[2].x,    points[2].y,
        points[3].x,    points[3].y);

//      ecran->u8g2->drawLine(points[0].x, points[0].y, points[1].x, points[1].y);
//      ecran->u8g2->drawLine(points[1].x, points[1].y, points[2].x, points[2].y);
//      ecran->u8g2->drawLine(points[2].x, points[2].y, points[0].x, points[0].y);
//       
//      ecran->u8g2->drawLine(points[0].x, points[0].y, points[2].x, points[2].y);
//      ecran->u8g2->drawLine(points[2].x, points[2].y, points[3].x, points[3].y);
//      ecran->u8g2->drawLine(points[3].x, points[3].y, points[0].x, points[0].y);        
  
    }
    ecran->u8g2->sendBuffer();
    delay(50);
    nb++;
  }
}