import processing.video.*;

Capture video;

int pixelSize = 20;

color Black = color(0);
color White = color(255);
color Red = color(234, 0, 0);
color Green = color(7, 149, 0);
color Blue = color(0, 63, 245);
color Yellow = color(255, 230, 0);
color Orange = color(255, 166, 0);
color Brown = color(162, 89, 0);
color LightGreen = color(148, 234, 0);

color[] colors = new color[9];

lego[] myLegos = new lego[0];

void setup() {
  size(640, 480);
  strokeWeight(0.1);
  stroke(180);
  
  initLego();
  initColors();
  
  video = new Capture(this, width, height);
  video.start();
}

void draw() {
  
  if (video.available()) {
    video.read();
    video.loadPixels();
    
    background(0);
    image(video, 0, 0, width, height);
    //loadPixels();
    
    for(int x = 0; x < width; x += pixelSize) {
      for(int y = 0; y < height; y += pixelSize) {
        int thisPixel = x + y * video.width;
        int thisLego = x / pixelSize + y / pixelSize * video.width / pixelSize;
        color colorFromVideo = video.pixels[thisPixel];
        float pixelR = red(colorFromVideo);
        float pixelG = green(colorFromVideo);
        float pixelB = blue(colorFromVideo);
        
        myLegos[thisLego].pieceColor = colors[colorInLegoIndex(pixelR, pixelG, pixelB)];
        myLegos[thisLego].drawPiece();
//        fill(colorFromVideo);
//        rect(x, y, pixelSize, pixelSize);
      }
    }
    //updatePixels();
  }
}

void initLego() {
  for(int i = 0; i < height / pixelSize; i++) {
    for(int j = 0; j < width / pixelSize; j++) {
      myLegos = (lego[])append(myLegos, new lego(j * pixelSize, i * pixelSize));
    }
  }
}

void initColors() {
  colors = (color[])append(colors, color(0));
  colors = (color[])append(colors, color(255));
  colors = (color[])append(colors, color(234, 0, 0));
  colors = (color[])append(colors, color(7, 149, 0));
  colors = (color[])append(colors, color(0, 63, 245));
  colors = (color[])append(colors, color(255, 230, 0));
  colors = (color[])append(colors, color(255, 166, 0));
  colors = (color[])append(colors, color(162, 89, 0));
  colors = (color[])append(colors, color(148, 234, 0));
}

int colorInLegoIndex(float R, float G, float B) {
  float[] colorDistances = new float[0];
  colorDistances = append(colorDistances, dist(R, G, B, 0, 0, 0));
  colorDistances = append(colorDistances, dist(R, G, B, 255, 255, 255));
  colorDistances = append(colorDistances, dist(R, G, B, 234, 0, 0));
  colorDistances = append(colorDistances, dist(R, G, B, 7, 149, 0));
  colorDistances = append(colorDistances, dist(R, G, B, 0, 63, 245));
  colorDistances = append(colorDistances, dist(R, G, B, 255, 230, 0));
  colorDistances = append(colorDistances, dist(R, G, B, 255, 166, 0));
  colorDistances = append(colorDistances, dist(R, G, B, 162, 89, 0));
  colorDistances = append(colorDistances, dist(R, G, B, 148, 234, 0));
  
  float minDistance = min(colorDistances);
  
  return findMinIndex(colorDistances);
  
//  for(int i = 0; i < colorDistances.length; i++) {
//    if(colorDistances[i] == minDistance) {
//      switch(i) {
//        case 0: return Black;
//        case 1: return White;
//        case 2: return Red;
//        case 3: return Green;
//        case 4: return Blue;
//        case 5: return Yellow;
//        case 6: return Orange;
//        case 7: return Brown;
//        case 8: return LightGreen;
//      }
//    }
//  }
}

int findMinIndex(float[] distArray) {
  float minDist = distArray[0];
  int index = 0;
  for(int i = 1; i < distArray.length; i++) {
    if(minDist > distArray[i]) {
      index = i;
      minDist = distArray[i];
    }
  }
  return index;
}

class lego {
  color pieceColor;
  int pieceX;
  int pieceY;
  
  lego(int x, int y) {
    pieceColor = Black;
    pieceX = x;
    pieceY = y;
  }
  
  void drawPiece() {
    fill(pieceColor);
    rect(pieceX, pieceY, pixelSize, pixelSize);
    ellipse(pieceX + pixelSize / 2, pieceY + pixelSize / 2, pixelSize * 0.7, pixelSize * 0.7);
  }
}