#ifdef DEBUG
#include <TinyDebug.h>
#endif

#define DS 2
#define STCP 3
#define SHCP 4
int zOff = 150;
int xOff = 0;
int yOff = 0;
int cSize = 50;
int view_plane = 12;//64;
float angle = PI/60;
#define vres 16
#define hres 16
const int ledAmount = 16 * 16;

float cube3d[8][3] = {
  {xOff - cSize,yOff + cSize,zOff - cSize},
  {xOff + cSize,yOff + cSize,zOff - cSize},
  {xOff - cSize,yOff - cSize,zOff - cSize},
  {xOff + cSize,yOff - cSize,zOff - cSize},
  {xOff - cSize,yOff + cSize,zOff + cSize},
  {xOff + cSize,yOff + cSize,zOff + cSize},
  {xOff - cSize,yOff - cSize,zOff + cSize},
  {xOff + cSize,yOff - cSize,zOff + cSize}
};
unsigned char cube2d[8][2];
bool buf[vres][hres];

void line(int x0, int y0, int x1, int y1) {
  bool steep = false; 
  if (x0<0){x0=0;}
  if (x1<0){x1=0;}
  if (y0<0){y0=0;}
  if (y1<0){y1=0;}


  if (x0>(hres-1)){x0=(hres-1);}
  if (x1>(hres-1)){x1=(hres-1);}
  if (y0>(hres-1)){y0=(hres-1);}
  if (y1>(hres-1)){y1=(hres-1);}


  if (x0>(vres-1)){x0=(vres-1);}
  if (x1>(vres-1)){x1=(vres-1);}
  if (y0>(vres-1)){y0=(vres-1);}
  if (y1>(vres-1)){y1=(vres-1);}
#ifdef DEBUG
  Debug.print(x0);
  Debug.print(F(" "));
  Debug.print(y0);
  Debug.print(F(" "));
  Debug.print(x1);
  Debug.print(F(" "));
  Debug.println(y1);
  #endif

    if (abs(x0-x1)<abs(y0-y1)) { 
        int t = x0;
        x0=y0;
        y0=t;
        t = x1;
        x1=y1;
        y1=t;
        
        steep = true; 
    } 
    if (x0>x1) { 
        int t = x0;
        x0=x1;
        x1=t;
        t=y0;
        y0=y1;
        y1=t;
         
    } 
    int dx = x1-x0; 
    int dy = y1-y0; 
    float derror = abs(dy/float(dx)); 
    float error = 0; 
    int y = y0; 
    for (int x=x0; x<=x1; x++) { 
        if (steep) { 
            buf[y][x]=1;
            
        } else { 
            buf[x][y]=1;
        } 
        error += derror; 
        if (error>.5) { 
            y += (y1>y0?1:-1); 
            error -= 1.; 
        } 
    } 

}

void setup() {
  // put your setup code here, to run once:
  #ifdef DEBUG
  Debug.begin();
  #endif
  pinMode(DS, OUTPUT);
  pinMode(STCP, OUTPUT);
  pinMode(SHCP, OUTPUT);

}

void loop() {
  // put your main code here, to run repeatedly:
  //for (int i = 0; i < 70; i++) {
  //        yrotate(angle);
  //}
  //      printcube();
  //      while(10)delay(10);
  //      return;
  int rsteps = random(10,60);
  switch(random(6)) {
    case 0:
      for (int i = 0; i < rsteps; i++) {
        zrotate(angle);
        printcube();
      }
      break;
    case 1:
      for (int i = 0; i < rsteps; i++) {
        zrotate(2*PI - angle);
        printcube();
      }
      break;
    case 2:
      for (int i = 0; i < rsteps; i++) {
        xrotate(angle);
        printcube();
      }
      break;
    case 3:
      for (int i = 0; i < rsteps; i++) {
        xrotate(2*PI - angle);
        printcube();
      }
      break;
    case 4:
      for (int i = 0; i < rsteps; i++) {
        yrotate(angle);
        printcube();
      }
      break;
    case 5:
      for (int i = 0; i < rsteps; i++) {
        yrotate(2*PI - angle);
        printcube();
      }
      break;
  }
}


void printcube() {
  //calculate 2d points
  for(byte i = 0; i < 8; i++) {
    cube2d[i][0] = (unsigned char)((cube3d[i][0] * view_plane / cube3d[i][2]) + (hres/2));
    cube2d[i][1] = (unsigned char)((cube3d[i][1] * view_plane / cube3d[i][2]) + (vres/2));
  }
  draw_cube();
}

void zrotate(float q) {
  float tx,ty,temp;
  for(byte i = 0; i < 8; i++) {
    tx = cube3d[i][0] - xOff;
    ty = cube3d[i][1] - yOff;
    temp = tx * cos(q) - ty * sin(q);
    ty = tx * sin(q) + ty * cos(q);
    tx = temp;
    cube3d[i][0] = tx + xOff;
    cube3d[i][1] = ty + yOff;
  }
}

void yrotate(float q) {
  float tx,tz,temp;
  for(byte i = 0; i < 8; i++) {
    tx = cube3d[i][0] - xOff;
    tz = cube3d[i][2] - zOff;
    temp = tz * cos(q) - tx * sin(q);
    tx = tz * sin(q) + tx * cos(q);
    tz = temp;
    cube3d[i][0] = tx + xOff;
    cube3d[i][2] = tz + zOff;
  }
}

void xrotate(float q) {
  float ty,tz,temp;
  for(byte i = 0; i < 8; i++) {
    ty = cube3d[i][1] - yOff;
    tz = cube3d[i][2] - zOff;
    temp = ty * cos(q) - tz * sin(q);
    tz = ty * sin(q) + tz * cos(q);
    ty = temp;
    cube3d[i][1] = ty + yOff;
    cube3d[i][2] = tz + zOff;
  }
}
void showLeds() {
  digitalWrite(STCP, LOW);
  for (int x = 0;x<ledAmount;x++) {
    digitalWrite(SHCP, LOW);
    digitalWrite(DS, buf[(vres - 1)- (x/vres)][(vres - 1)- (x%hres)] ? HIGH : LOW);
    digitalWrite(SHCP, HIGH);
  }
  digitalWrite(STCP, HIGH);
}

void draw_cube() {
  memset(buf, 0, sizeof buf);
  line(cube2d[0][0],cube2d[0][1],cube2d[1][0],cube2d[1][1]);
  line(cube2d[0][0],cube2d[0][1],cube2d[2][0],cube2d[2][1]);
  line(cube2d[0][0],cube2d[0][1],cube2d[4][0],cube2d[4][1]);
  line(cube2d[1][0],cube2d[1][1],cube2d[5][0],cube2d[5][1]);
  line(cube2d[1][0],cube2d[1][1],cube2d[3][0],cube2d[3][1]);
  line(cube2d[2][0],cube2d[2][1],cube2d[6][0],cube2d[6][1]);
  line(cube2d[2][0],cube2d[2][1],cube2d[3][0],cube2d[3][1]);
  line(cube2d[4][0],cube2d[4][1],cube2d[6][0],cube2d[6][1]);
  line(cube2d[4][0],cube2d[4][1],cube2d[5][0],cube2d[5][1]);
  line(cube2d[7][0],cube2d[7][1],cube2d[6][0],cube2d[6][1]);
  line(cube2d[7][0],cube2d[7][1],cube2d[3][0],cube2d[3][1]);
  line(cube2d[7][0],cube2d[7][1],cube2d[5][0],cube2d[5][1]);
  showLeds();
}