#include "LedCube.h"
//#include "TimerOne.h"
#include "TimerOne.h"


LedCube myCube(DEFAULT_CUBESIZE, DEFAULT_SHADES, CubeRegisterPortB);
myCube.framerate(DEFAULT_FRAMERATE);


LedCube::LedCube( const unsigned char size,
  const unsigned char shades,
   CubeRegister output = CubeRegister())
:_cubesize(size), _shades(shades), myRegister(output){
  _framerate = DEFAULT_FRAMERATE;

//  unsigned char pixels[5][5][5];
  pixels = new unsigned char**[_cubesize];
  for (unsigned char x=0;x<_cubesize;x++){
    pixels[x] = new unsigned char*[_cubesize];
    for (unsigned char y=0;y<_cubesize;y++){
      pixels[x][y] = new unsigned char[_cubesize];
    }
  }

  reset();
};

LedCube::~LedCube(void){
  for (unsigned char x=0;x<_cubesize;x++){
    for (unsigned char y=0;y<_cubesize;y++){
        delete [] pixels[x][y];
    }
    delete [] pixels[x];
  }
  delete [] pixels;

}

void LedCube::toSerial(void){
  if (Serial){
    Serial.println("LedCube "+ String(_cubesize) + "x"+ String(_cubesize) +"x"+ String(_cubesize) +" @" + String(_framerate)+ "Hz and " + String(_shades) + " shades of grey\n");
  }
}


void LedCube::start(void){
  reset();
  myRegister.reset();
  myRegister.output_enable();
  if (Serial){ Serial.println("[LedCube] startTimer1(" + String(1000000 / (_framerate * _shades * _cubesize)) + ");"); }
  delay(500);

  //startTimer1(1000000 / (_framerate * _shades * _cubesize));

  unsigned long timer_interval = 1000000 / (_framerate * _shades * _cubesize);
  Timer1.initialize(timer_interval);
  Timer1.attachInterrupt(myCube.draw_layer);


  delay(500);
  if (Serial){ Serial.println("LedCube::start :-)"); }
}

void LedCube::reset(void){
  Timer1.detachInterrupt();
  //resetTimer1();
  myRegister.reset();
  layer = 0;
  level = 0;

}

void LedCube::framerate(unsigned char framerate){
  _framerate = framerate;
}

unsigned char LedCube::size(void){
  return _cubesize;
}

unsigned char LedCube::shades(void){
  return _shades;
}

unsigned char LedCube::pixel(unsigned char x, unsigned char y, unsigned char z){
  return pixels[x][y][z];
}

void LedCube::pixel(unsigned char x, unsigned char y, unsigned char z, unsigned char value){
  pixels[x][y][z] = constrain(value, 0, _shades);
}

 void LedCube::draw_layer(void){

  layer = layer++ % _cubesize;
  if (layer == 0){
    level = level++ % _shades;
  };

  for (unsigned char z = 0; z < _cubesize; z++) {
    myRegister.shift_bit(z == layer);
  }
  for (unsigned char x = 0; x < _cubesize; x++) {
    for (unsigned char y = 0; y < _cubesize; y++) {
      myRegister.shift_bit((pixels[x][y][layer] > level));
    }
  }
  myRegister.register_to_output();
}

void LedCube::fill(unsigned char brightness){

  unsigned char value = min(brightness, _shades);
  for (unsigned char x = 0; x < _cubesize; x++) {
      for (unsigned char y = 0; y < _cubesize; y++) {
        for (unsigned char z = 0; z < _cubesize; z++) {
          pixels[x][y][z] = value;
        }
      }
    }
}

void LedCube::shade(char diff = -1){

  char value = constrain(diff, -_shades, _shades);

  for (unsigned char x = 0; x < _cubesize; x++) {
      for (unsigned char y = 0; y < _cubesize; y++) {
        for (unsigned char z = 0; z < _cubesize; z++) {
          pixels[x][y][z] = constrain(pixels[x][y][z] + value, 0, _shades);
        }
      }
    }
}

void LedCube::rotate_cover(int steps, unsigned int frame_delay = 100) {
  unsigned char x = 0;
  unsigned char y = 0;
  unsigned char z = 0;
  unsigned char backup;

  for (int s = 0; s < steps; s++) {
    for (z = 0; z < _cubesize; z++) {
      backup = pixels[0][0][z];

      // 0,0 -> 0,4 : v
      x = 0;
      for (y = 1; y < _cubesize; y++) {
        pixels[x][y - 1][z] = pixels[x][y][z];
      };

      y = _cubesize - 1;
      for (x = 1; x < _cubesize; x++) {
        pixels[x - 1][y][z] = pixels[x][y][z];
      }

      x = _cubesize - 1;
      for (y = _cubesize - 2; y < _cubesize; --y) {
        pixels[x][y + 1][z] = pixels[x][y][z];
      }

      y = 0;
      for (x = _cubesize - 2; x < _cubesize; --x) {
        pixels[x + 1][y][z] = pixels[x][y][z];
      }
      pixels[1][0][z] = backup;
    }
    delay(frame_delay);
  }
};

void LedCube::rotate(int steps, unsigned int frame_delay = 100) {
  unsigned char x = 0;
  unsigned char y = 0;
  unsigned char z = 0;
  unsigned char backup;


  for (int s = 0; s < steps; s++) {
    for (z = 0; z < _cubesize; z++) {
      backup = pixels[0][0][z];

      // 0,0 -> 0,4 : v
      x = 0;
      for (y = 1; y < _cubesize; y++) {
        pixels[x][y - 1][z] = pixels[x][y][z];
      };

      y = _cubesize - 1;
      for (x = 1; x < _cubesize; x++) {
        pixels[x - 1][y][z] = pixels[x][y][z];
      }

      x = _cubesize - 1;
      for (y = _cubesize - 2; y < _cubesize; --y) {
        pixels[x][y + 1][z] = pixels[x][y][z];
      }

      y = 0;
      for (x = _cubesize - 2; x < _cubesize; --x) {
        pixels[x + 1][y][z] = pixels[x][y][z];
      }
      pixels[1][0][z] = backup;
    }
    delay(frame_delay);
  }
};
