#include <Wire.h>
#include <Adafruit_LIS3DH.h>

#define ACCEL_ADDR   0x18
#define TX_ADDR      0x20
#define WIDTH        16
#define HEIGHT       16
#define MAX_X (WIDTH  * 256 - 1)
#define MAX_Y (HEIGHT * 256 - 1)

int FPS = 90;
int N_GRAINS = 56; 
int ROW_LT = 8;
int ROW_CLK = 9;
int ROW_IN = 10;
int COL_LT = 7;
int COL_CLK = 6;  
int COL_IN = 5; 

struct Grain {
  int16_t  x,  y;
  int16_t vx, vy;
} grain[60];

bool frameUpdated;

Adafruit_LIS3DH accel      = Adafruit_LIS3DH();
uint32_t        prevTime   = 0;
uint8_t         img[WIDTH * HEIGHT];

void setup(void) {
  pinMode(13, OUTPUT); 
  pinMode(A0, INPUT); 
  pinMode(A1, INPUT); 
  pinMode(4, OUTPUT);
  pinMode(5, OUTPUT);
  digitalWrite(4, HIGH);
  digitalWrite(5, LOW);
  
  frameUpdated = false;
  uint8_t i, j, bytes;

  if (!accel.begin(ACCEL_ADDR)) {
    for (i = 1;; i++) {
      digitalWrite(13, i & 1);
      delay(250);
    }
  }
  
  accel.setRange(LIS3DH_RANGE_4_G);
  Wire.begin();
  Wire.setClock(400000);

  memset(img, 0, sizeof(img));
  
  for (i = 0; i < N_GRAINS; i++) {
    do {
      grain[i].x = random(WIDTH  * 256);
      grain[i].y = random(HEIGHT * 256);
      for (j = 0; (j < i) && (((grain[i].x / 256) != (grain[j].x / 256)) ||
                              ((grain[i].y / 256) != (grain[j].y / 256))); j++);
    } while (j < i); 
    img[(grain[i].y / 256) * WIDTH + (grain[i].x / 256)] = 255;
    grain[i].vx = grain[i].vy = 0;
  }
}

short rows[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
byte img_rows[32];

void loop() {
  FPS = (analogRead(A0) / 11) + 2;
  N_GRAINS = (analogRead(A1) / 18) + 1;
  
  if(frameUpdated){
    digitalWrite(13, HIGH);
  }
  else{
    digitalWrite(13, LOW);
  }
  frameUpdated = !frameUpdated;
  
  uint32_t t;
  while (((t = micros()) - prevTime) < (1000000L / FPS));
  prevTime = t;

  // Display frame

  for (int i = 0; i < N_GRAINS; i++) {
    int row_index = grain[i].y / 256;
    int col_index = grain[i].x / 256;
    rows[row_index] |= (1 << col_index);
  }

  // transmit image
  for (int row = 0; row <= 15; row++) {
    short currRow = rows[row];
    img_rows[row * 2] = (byte) (currRow >> 8);
    img_rows[(row * 2) + 1] = (byte) currRow;
  }

  Wire.beginTransmission(TX_ADDR);
  Wire.write(img_rows, 32);
  Wire.endTransmission();

  for (int row = 0; row <= 15; row++) {
    rows[row] = 0;
  }

  accel.read();
  int16_t ax = -accel.y / 256;  
  int16_t ay =  ((accel.x - 6) / 256) - 17;
  int16_t realZ = accel.z;
  int16_t az = abs(realZ) / 2048;

  az = (az >= 3) ? 1 : 4 - az; 
  ax -= az;                     
  ay -= az;
  int16_t az2 = az * 2 + 1;        

  int32_t v2;
  float   v;
  for (int i = 0; i < N_GRAINS; i++) {
    grain[i].vx += ax + random(az2); 
    grain[i].vy += ay + random(az2);
    v2 = (int32_t)grain[i].vx * grain[i].vx + (int32_t)grain[i].vy * grain[i].vy;
    if (v2 > 65536) {
      v = sqrt((float)v2);
      grain[i].vx = (int)(256.0 * (float)grain[i].vx / v);
      grain[i].vy = (int)(256.0 * (float)grain[i].vy / v);
    }
  }

  uint8_t        i, bytes, oldidx, newidx, delta;
  int16_t        newx, newy;

  for (i = 0; i < N_GRAINS; i++) {
    newx = grain[i].x + grain[i].vx;
    newy = grain[i].y + grain[i].vy;
    if (newx > MAX_X) {
      newx         = MAX_X;
      grain[i].vx /= -2;   
    } else if (newx < 0) {
      newx         = 0;
      grain[i].vx /= -2;
    }
    if (newy > MAX_Y) {
      newy         = MAX_Y;
      grain[i].vy /= -2;
    } else if (newy < 0) {
      newy         = 0;
      grain[i].vy /= -2;
    }

    oldidx = (grain[i].y / 256) * WIDTH + (grain[i].x / 256); 
    newidx = (newy      / 256) * WIDTH + (newx      / 256); 
    if ((oldidx != newidx) && 
        img[newidx]) {      
      delta = abs(newidx - oldidx);
      if (delta == 1) {          
        newx         = grain[i].x; 
        grain[i].vx /= -2;        
        newidx       = oldidx;    
      } else if (delta == WIDTH) {
        newy         = grain[i].y;  
        grain[i].vy /= -2;         
        newidx       = oldidx;     
      } else { 
        if ((abs(grain[i].vx) - abs(grain[i].vy)) >= 0) { 
          newidx = (grain[i].y / 256) * WIDTH + (newx / 256);
          if (!img[newidx]) { 
            newy         = grain[i].y; 
            grain[i].vy /= -2;        
          } else { 
            newidx = (newy / 256) * WIDTH + (grain[i].x / 256);
            if (!img[newidx]) { 
              newx         = grain[i].x; 
              grain[i].vx /= -2;        
            } else {
              newx         = grain[i].x; 
              newy         = grain[i].y;
              grain[i].vx /= -2;         
              grain[i].vy /= -2;
              newidx       = oldidx;  
            }
          }
        } else {
          newidx = (newy / 256) * WIDTH + (grain[i].x / 256);
          if (!img[newidx]) {
            newx         = grain[i].x; 
            grain[i].vy /= -2;       
          } else { 
            newidx = (grain[i].y / 256) * WIDTH + (newx / 256);
            if (!img[newidx]) {
              newy         = grain[i].y; 
              grain[i].vy /= -2;        
            } else { 
              newx         = grain[i].x; 
              newy         = grain[i].y;
              grain[i].vx /= -2;        
              grain[i].vy /= -2;
              newidx       = oldidx; 
            }
          }
        }
      }
    }
    grain[i].x  = newx;
    grain[i].y  = newy;
    img[oldidx] = 0;
    img[newidx] = 255;
  }
}