diff --git a/BlobDetection.pde b/BlobDetection.pde
new file mode 100644
index 0000000..96e99b9
--- /dev/null
+++ b/BlobDetection.pde
@@ -0,0 +1,108 @@
+import java.util.ArrayList;
+import java.util.List;
+import java.util.TreeSet;
+
+class BlobDetection {
+  PImage findConnectedComponents(PImage input, boolean onlyBiggest) {
+  PImage img = input.copy();
+  img.loadPixels();
+
+  int w = img.width;
+  int h = img.height;
+
+  int[] sets = new int[200];
+  int[] count = new int[200];
+  sets[1] = 1;
+  int setsNum = 0;
+
+  for (int x = 0; x < w; x++) {
+    img.pixels[x] = Integer.MAX_VALUE;
+  }
+  for (int y = 0; y < h; y++) {
+    img.pixels[y*w] = Integer.MAX_VALUE;
+  }
+
+  int next_set = 1;
+  for (int y = 1; y < h; y++) {
+    for (int x = 1; x < w; x++) {
+      if (img.pixels[y*w + x] == #FFFFFF) {
+        int W = img.pixels[y*w +x-1];
+        int nw = img.pixels[(y-1)*w +x-1];
+        int n = img.pixels[(y-1)*w +x];
+        int ne = img.pixels[(y-1)*w +x+1];
+        int lowest = minimum(W,nw,n,ne);
+        if (lowest == Integer.MAX_VALUE) {
+          img.pixels[y*w + x] = next_set;
+          //new set made
+          if (sets.length == setsNum + 1) {
+            //if not enough room realloc the array
+            int[] newsets = new int[sets.length*2];
+            int[] newcount = new int[sets.length*2];
+            for (int i = 0; i < sets.length; i++) {
+              newsets[i] = sets[i];
+              newcount[i] = count[i];
+            }
+            sets = newsets;
+            count = newcount;
+          }
+          setsNum += 1;
+          sets[next_set] = next_set;
+          count[next_set] = 1;
+          next_set++;
+        } else {
+          img.pixels[y*w + x] = lowest;
+          if (nw != lowest && nw != Integer.MAX_VALUE) sets[nw] = lowest;
+          if (W != lowest && W != Integer.MAX_VALUE) sets[W] = lowest;
+          if (n != lowest && n != Integer.MAX_VALUE) sets[n] = lowest;
+          if (ne != lowest && ne != Integer.MAX_VALUE) sets[ne] = lowest;
+          count[lowest]++; //increase the count for set with label lowest
+        }
+      } else {
+        img.pixels[y*w + x] = Integer.MAX_VALUE;
+      }
+    }
+  }
+
+  int classMaxCount = 1;
+  int maxcount = 0;
+  for (int i = 1; i < setsNum; i++) {
+      while (sets[sets[i]] != sets[i]) {
+        count[sets[i]] += count[i];
+        sets[i] = sets[sets[i]];
+        if (count[sets[i]] > maxcount) {
+          classMaxCount = sets[i];
+          maxcount = count[sets[i]];
+        }
+      }
+  }
+  
+  for (int i = 0; i < img.pixels.length; i++) {
+    if (img.pixels[i] != Integer.MAX_VALUE) { 
+      img.pixels[i] = sets[img.pixels[i]];
+    }
+  }
+
+  if (onlyBiggest) {
+    for (int i = 0; i<img.pixels.length; i++) {
+      if (img.pixels[i] == Integer.MAX_VALUE || img.pixels[i] != classMaxCount) img.pixels[i] = color(0);
+      else img.pixels[i] = color(152,175,114);
+    }
+  } else {
+    for (int i = 0; i<img.pixels.length; i++) {
+    if (img.pixels[i] == Integer.MAX_VALUE) img.pixels[i] = 0;
+    else {
+        int clas = img.pixels[i];
+        img.pixels[i] = color(clas*139%255, clas*173%255, clas*83%255);
+      } 
+    }
+  }
+
+  img.updatePixels();
+  return img;
+}
+
+int minimum(int a, int b, int c, int d) {
+  return min(min(a,b), min(c,d));
+}
+
+}
\ No newline at end of file
diff --git a/Hough.pde b/Hough.pde
new file mode 100644
index 0000000..a3ee7ed
--- /dev/null
+++ b/Hough.pde
@@ -0,0 +1,100 @@
+public class Hough {
+  final int phiDim;
+  final int rDim;
+  int[] accumulator;
+  ArrayList<PVector> lines;
+
+  Hough(PImage edgeImg, int nLines) {
+    phiDim = (int) (Math.PI / discretizationStepsPhi);
+    rDim = (int) (((edgeImg.width + edgeImg.height) * 2 + 1) / discretizationStepsR);
+    // our accumulator (with a 1 pix margin around)
+    accumulator = new int[(phiDim + 2) * (rDim + 2)];
+
+    // Fill the accumulator: on edge points (ie, white pixels of the edge
+    // image), store all possible (r, phi) pairs describing lines going
+    // through the point.
+    for (int y = 0; y < edgeImg.height; y++) {
+      for (int x = 0; x < edgeImg.width; x++) {
+        // Are we on an edge?
+        if (brightness(edgeImg.pixels[y * edgeImg.width + x]) != 0) {
+          for (int phiTab = 0; phiTab < phiDim; phiTab++) {
+            float phi = phiTab * discretizationStepsPhi;
+            float r = (float)(Math.cos(phi)*x + y*Math.sin(phi));
+            int rIndex = Math.round(r/discretizationStepsR);
+
+            int index = (phiTab + 1) * (rDim+2) + (rIndex + 1) + (rDim-1)/2;
+            accumulator[index] ++;
+          }
+        }
+      }
+    }
+
+    lines = getLines(edgeImg, nLines);
+  }
+
+  PImage getHoughImage() {
+    PImage houghImg = createImage(rDim + 2, phiDim + 2, ALPHA);
+    for (int i = 0; i < accumulator.length; i++) {
+      houghImg.pixels[i] = color(min(255, accumulator[i]));
+    }
+
+    houghImg.resize(480, 480);
+    houghImg.updatePixels();
+
+    return houghImg;
+  }
+
+  private ArrayList<PVector> getLines(PImage edgeImg, int nLines) {
+    ArrayList<Integer> bestCandidates = new ArrayList<Integer>();
+    // size of the region we search for a local maximum
+    int neighbourhood = 10;
+    // only search around lines with more that this amount of votes
+    // (to be adapted to your image)
+    int minVotes = 150; //usually 200
+    for (int accR = 0; accR < rDim; accR++) {
+      for (int accPhi = 0; accPhi < phiDim; accPhi++) {
+        // compute current index in the accumulator
+        int idx = (accPhi + 1) * (rDim + 2) + accR + 1;
+        if (accumulator[idx] > minVotes) {
+          boolean bestCandidate=true;
+          // iterate over the neighbourhood
+          for (int dPhi=-neighbourhood/2; dPhi < neighbourhood/2+1; dPhi++) {
+            // check we are not outside the image
+            if ( accPhi+dPhi < 0 || accPhi+dPhi >= phiDim) continue;
+            for (int dR=-neighbourhood/2; dR < neighbourhood/2 +1; dR++) {
+              // check we are not outside the image
+              if (accR+dR < 0 || accR+dR >= rDim) continue;
+              int neighbourIdx = (accPhi + dPhi + 1) * (rDim + 2) + accR + dR + 1;
+              if (accumulator[idx] < accumulator[neighbourIdx]) {
+                // the current idx is not a local maximum!
+                bestCandidate=false;
+                break;
+              }
+            }
+            if (!bestCandidate) break;
+          }
+          if (bestCandidate) {
+            // the current idx *is* a local maximum
+            bestCandidates.add(idx);
+          }
+        }
+      }
+    }
+    Collections.sort(bestCandidates, new HoughComparator(accumulator));
+
+    ArrayList<PVector> pairs = new ArrayList<PVector>();
+    for (int j = 0; j < Math.min(nLines, bestCandidates.size()); j++) {
+      int idx = bestCandidates.get(j);
+      // first, compute back the (r, phi) polar coordinates:
+      int accPhi = (int) (idx / (rDim + 2)) - 1;
+      int accR = idx - (accPhi + 1) * (rDim + 2) - 1;
+      float r = (accR - (rDim - 1) * 0.5f) * discretizationStepsR;
+      float phi = accPhi * discretizationStepsPhi;
+
+      PVector vect = new PVector(r, phi);
+      pairs.add(vect);
+    }
+
+    return pairs;
+  }
+}
\ No newline at end of file
diff --git a/HoughComparator.pde b/HoughComparator.pde
new file mode 100644
index 0000000..45d04d8
--- /dev/null
+++ b/HoughComparator.pde
@@ -0,0 +1,13 @@
+class HoughComparator implements java.util.Comparator<Integer> {
+  int[] accumulator;
+  public HoughComparator(int[] accumulator) {
+    this.accumulator = accumulator;
+  }
+  @Override
+  public int compare(Integer l1, Integer l2) {
+    if (accumulator[l1] > accumulator[l2] ||(accumulator[l1] == accumulator[l2] && l1 < l2)){ 
+        return -1;
+    }
+  return 1;
+  }
+}
\ No newline at end of file
diff --git a/ImageProcessing.pde b/ImageProcessing.pde
new file mode 100644
index 0000000..a0117da
--- /dev/null
+++ b/ImageProcessing.pde
@@ -0,0 +1,172 @@
+final String FILENAME = "board1.jpg";
+
+// Main constants
+final float[][] gaussianKernel = {
+  {9, 12, 9}, 
+  {12, 15, 12}, 
+  {9, 12, 9}};
+final float discretizationStepsPhi = 0.06f;
+final float discretizationStepsR = 2.5f;
+
+Hough hough;
+BlobDetection detect ;
+
+// Main processing methods
+void settings() {
+  size(1440, 360);
+}
+
+void setup() {
+  PImage boardImg = loadImage(FILENAME);
+  boardImg.resize(480, 360);
+  image(boardImg, 0, 0);
+  
+  PImage filtered = thresholdHSB(boardImg, 100, 135, 35, 255, 17, 175);
+  
+  PImage gaussian = convolute(filtered, gaussianKernel, 99);
+  PImage brightness = brightnessThreshold(gaussian, 5);
+  detect = new BlobDetection() ;
+  PImage detectBoard = detect.findConnectedComponents(brightness, true);
+  PImage scharr = scharr(brightness);
+
+  hough = new Hough(scharr, 6);
+  QuadGraph graph = new QuadGraph();
+  List<PVector> edges = graph.findBestQuad(hough.lines, width, height, width*height, width*height/64, false);
+
+  graph.displayLines(edges);
+  graph.displayCorners(edges);
+
+  image(scharr, boardImg.width, 0);
+  image(detectBoard, boardImg.width + detectBoard.width, 0);
+  noLoop();
+}
+
+PImage convolute(PImage img, float[][] kernel, float normFactor) {
+  PImage result = createImage(img.width, img.height, ALPHA);
+  img.loadPixels();
+  for (int i = 1; i < img.height-1; i++) {
+    for (int j = 1; j < img.width-1; j++) {
+      float val = 0;
+      for (int row = 0; row < 3; row++) {
+        for (int column = 0; column < 3; column++) {
+          //control to keep a one-pixel wide border around the image
+          int x_pixel = (j + column) - 1;
+          int y_pixel = (i + row) -1;
+          if (x_pixel < 0) x_pixel=0;
+          if (x_pixel > img.width-1) x_pixel=img.width-1;
+          if (y_pixel < 0) y_pixel=0;
+          if (y_pixel > img.height-1) y_pixel=img.height-1;
+          val += brightness(img.pixels[y_pixel * img.width + x_pixel]) * kernel[row][column];
+        }
+      }
+      result.pixels[i * img.width + j] = color(val / normFactor);
+    }
+  }
+  result.updatePixels();
+  return result;
+}
+
+PImage thresholdHSB(PImage img, int minH, int maxH, int minS, int maxS, int minB, int maxB) {
+  PImage result = createImage(img.width, img.height, HSB);
+  img.loadPixels();
+  result.loadPixels();
+  for (int i = 0; i < img.width*img.height; ++i)
+    if (hue(img.pixels[i]) >= minH && hue(img.pixels[i]) <= maxH &&
+      saturation(img.pixels[i]) >= minS && saturation(img.pixels[i]) <= maxS &&
+      brightness(img.pixels[i]) >= minB && brightness(img.pixels[i]) <= maxB)
+      result.pixels[i] = color(255);
+    else result.pixels[i] = color(0);
+  result.updatePixels();
+
+  return result;
+}
+
+PImage HueThres(PImage img, int min, int max) {
+  // create a new, initially transparent, ’result’ image
+  PImage result = createImage(img.width, img.height, HSB);
+  img.loadPixels();//loadPixels
+  for (int i = 0; i < img.width * img.height; i++) {
+    // do something with the pixel img.pixels[i]
+    if ( (hue(img.pixels[i]) > min) && (hue(img.pixels[i]) < max))
+    {
+      result.pixels[i] = color(hue(img.pixels[i]));
+    } else
+    {
+      result.pixels[i] = img.pixels[i];
+    }
+  }
+  return result;
+}
+
+PImage brightnessThreshold(PImage image, int threshold) {
+  image.loadPixels();
+  PImage result = createImage(image.width, image.height, RGB);
+  for (int i = 0; i < image.width * image.height; i++) {
+    if (brightness(image.pixels[i]) >= threshold) {
+      result.pixels[i] = color(255);
+    } else {
+      result.pixels[i] = color(0);
+    }
+  }
+  result.updatePixels();
+  return result;
+}
+
+PImage scharr(PImage img) {
+
+  float[][] vKernel = {
+    { 3, 0, -3 }, 
+    { 10, 0, -10 }, 
+    { 3, 0, -3 } };
+  float[][] hKernel = {
+    { 3, 10, 3 }, 
+    { 0, 0, 0 }, 
+    { -3, -10, -3 } };
+
+  PImage result = createImage(img.width, img.height, ALPHA);
+  // clear the image
+  for (int i = 0; i < img.width * img.height; i++) {
+    result.pixels[i] = color(0);
+  }
+
+  float max=0;
+  float[] buffer = new float[img.width * img.height];
+
+  float sum_h = 0;
+  float sum_v = 0;
+  float sum = 0;
+  for (int i = 0; i < img.height-1; ++i) {
+    for (int j = 0; j < img.width-1; ++j) {
+      sum_h  = 0;
+      sum_v = 0;
+      // we assume here that the kernel matrix is 3x3
+      for (int row = 0; row < 3; row++) {
+        for (int column = 0; column < 3; column++) {
+          //control to keep a one-pixel wide border around the image
+          int x_pixel = (j + column) - 1;
+          int y_pixel = (i + row) -1;
+          if (x_pixel < 0) x_pixel=0;
+          if (x_pixel > img.width-1) x_pixel=img.width-1;
+          if (y_pixel < 0) y_pixel=0;
+          if (y_pixel > img.height-1) y_pixel=img.height-1;
+          sum_h += brightness(img.get(x_pixel, y_pixel))*hKernel[row][column];
+          sum_v += brightness(img.get(x_pixel, y_pixel))*vKernel[row][column];
+        }
+      }
+      sum = sqrt(pow(sum_h, 2) + pow(sum_v, 2));
+      buffer[i*result.width+j] = sum;
+      
+      if (sum > max) {
+        max = sum;
+      }   
+    }
+  } 
+  
+  for (int y = 2; y < img.height - 2; y++) { // Skip top and bottom edges
+    for (int x = 2; x < img.width - 2; x++) { // Skip left and right
+      int val=(int) ((buffer[y * img.width + x] / max)*255);
+      result.pixels[y * img.width + x]=color(val);
+    }
+  } 
+  return result;
+}
diff --git a/QuadGraph.pde b/QuadGraph.pde
new file mode 100644
index 0000000..8d4b8e8
--- /dev/null
+++ b/QuadGraph.pde
@@ -0,0 +1,370 @@
+import java.util.Collections;
+import java.util.Comparator;
+import java.util.List;
+import java.util.ArrayList;
+import java.util.Map;
+class QuadGraph {
+
+  boolean verbose=false;
+
+  List<int[]> cycles = new ArrayList<int[]>();
+  int[][] graph;
+
+  List<PVector> findBestQuad(List<PVector> lines, int width, int height, int max_quad_area, int min_quad_area, boolean verbose) {
+    this.verbose=verbose;
+    build(lines, width, height);
+    findCycles(verbose);
+    ArrayList<PVector> bestQuad=new ArrayList<PVector>();
+    float bestQuadArea=0;
+    for (int [] cy : cycles) {
+      ArrayList<PVector> quad= new ArrayList<PVector>();
+      PVector l1 = lines.get(cy[0]);
+      PVector l2 = lines.get(cy[1]);
+      PVector l3 = lines.get(cy[2]);
+      PVector l4 = lines.get(cy[3]);
+
+
+      quad.add(intersection(l1, l2));
+      quad.add(intersection(l2, l3));
+      quad.add(intersection(l3, l4));
+      quad.add(intersection(l4, l1));
+      quad=sortCorners(quad);
+
+      PVector c1 = quad.get(0);
+      PVector c2 = quad.get(1);
+      PVector c3 = quad.get(2);
+      PVector c4 = quad.get(3);
+
+      if (isConvex(c1, c2, c3, c4) && 
+        nonFlatQuad(c1, c2, c3, c4)) {
+        float quadArea=validArea(c1, c2, c3, c4, max_quad_area, min_quad_area);
+        if (quadArea>0 && quadArea>bestQuadArea) {
+          bestQuadArea=quadArea;
+          bestQuad=quad;
+        }
+      }
+    }
+    if (bestQuadArea>0)
+      return bestQuad;
+    else
+      return new ArrayList<PVector>();
+  }  
+
+
+  void build(List<PVector> lines, int width, int height) {
+ 
+    int n = lines.size();
+
+    graph = new int[n * (n - 1)/2][2];
+
+    int idx =0;
+
+    for (int i = 0; i < lines.size(); i++) {
+      for (int j = i + 1; j < lines.size(); j++) {
+        if (intersect(lines.get(i), lines.get(j), width, height)) {
+
+          graph[idx][0]=i;
+          graph[idx][1]=j;
+          idx++;
+        }
+      }
+    }
+  }
+
+  boolean intersect(PVector line1, PVector line2, int width, int height) {
+
+    double sin_t1 = Math.sin(line1.y);
+    double sin_t2 = Math.sin(line2.y);
+    double cos_t1 = Math.cos(line1.y);
+    double cos_t2 = Math.cos(line2.y);
+    float r1 = line1.x;
+    float r2 = line2.x;
+
+    double denom = cos_t2 * sin_t1 - cos_t1 * sin_t2;
+
+    int x = (int) ((r2 * sin_t1 - r1 * sin_t2) / denom);
+    int y = (int) ((-r2 * cos_t1 + r1 * cos_t2) / denom);
+
+    if (0 <= x && 0 <= y && width >= x && height >= y)
+      return true;
+    else
+      return false;
+  }
+
+  PVector intersection(PVector line1, PVector line2) {
+
+    double sin_t1 = Math.sin(line1.y);
+    double sin_t2 = Math.sin(line2.y);
+    double cos_t1 = Math.cos(line1.y);
+    double cos_t2 = Math.cos(line2.y);
+    float r1 = line1.x;
+    float r2 = line2.x;
+
+    double denom = cos_t2 * sin_t1 - cos_t1 * sin_t2;
+
+    int x = (int) ((r2 * sin_t1 - r1 * sin_t2) / denom);
+    int y = (int) ((-r2 * cos_t1 + r1 * cos_t2) / denom);
+
+    return new PVector(x, y);
+  }
+
+  void findCycles(boolean verbose) {
+    cycles.clear();
+    for (int i = 0; i < graph.length; i++) {
+      for (int j = 0; j < graph[i].length; j++) {
+        findNewCycles(new int[] {graph[i][j]});
+      }
+    }
+    if (verbose) {
+      for (int[] cy : cycles) {
+        String s = "" + cy[0];
+        for (int i = 1; i < cy.length; i++) {
+          s += "," + cy[i];
+        }
+        System.out.println(s);
+      }
+    }
+  }
+
+  void findNewCycles(int[] path)
+  {
+    int n = path[0];
+    int x;
+    int[] sub = new int[path.length + 1];
+
+    for (int i = 0; i < graph.length; i++)
+      for (int y = 0; y <= 1; y++)
+        if (graph[i][y] == n)
+          //  edge refers to our current node
+        {
+          x = graph[i][(y + 1) % 2];
+          if (!visited(x, path))
+            //  neighbor node not on path yet
+          {
+            sub[0] = x;
+            System.arraycopy(path, 0, sub, 1, path.length);
+            //  explore extended path
+            findNewCycles(sub);
+          } else if ((path.length == 4) && (x == path[path.length - 1]))
+            //  cycle found
+          {
+            int[] p = normalize(path);
+            int[] inv = invert(p);
+            if (isNew(p) && isNew(inv))
+            {
+              cycles.add(p);
+            }
+          }
+        }
+  }
+
+  Boolean equals(int[] a, int[] b)
+  {
+    Boolean ret = (a[0] == b[0]) && (a.length == b.length);
+
+    for (int i = 1; ret && (i < a.length); i++)
+    {
+      if (a[i] != b[i])
+      {
+        ret = false;
+      }
+    }
+
+    return ret;
+  }
+
+  int[] invert(int[] path)
+  {
+    int[] p = new int[path.length];
+
+    for (int i = 0; i < path.length; i++)
+    {
+      p[i] = path[path.length - 1 - i];
+    }
+
+    return normalize(p);
+  }
+
+  int[] normalize(int[] path)
+  {
+    int[] p = new int[path.length];
+    int x = smallest(path);
+    int n;
+
+    System.arraycopy(path, 0, p, 0, path.length);
+
+    while (p[0] != x)
+    {
+      n = p[0];
+      System.arraycopy(p, 1, p, 0, p.length - 1);
+      p[p.length - 1] = n;
+    }
+
+    return p;
+  }
+
+  Boolean isNew(int[] path)
+  {
+    Boolean ret = true;
+
+    for (int[] p : cycles)
+    {
+      if (equals(p, path))
+      {
+        ret = false;
+        break;
+      }
+    }
+
+    return ret;
+  }
+
+  int smallest(int[] path)
+  {
+    int min = path[0];
+
+    for (int p : path)
+    {
+      if (p < min)
+      {
+        min = p;
+      }
+    }
+
+    return min;
+  }
+
+  //  check if vertex n is contained in path
+  Boolean visited(int n, int[] path)
+  {
+    Boolean ret = false;
+
+    for (int p : path)
+    {
+      if (p == n)
+      {
+        ret = true;
+        break;
+      }
+    }
+
+    return ret;
+  }
+
+  boolean isConvex(PVector c1, PVector c2, PVector c3, PVector c4) {
+
+    PVector v21= PVector.sub(c1, c2);
+    PVector v32= PVector.sub(c2, c3);
+    PVector v43= PVector.sub(c3, c4);
+    PVector v14= PVector.sub(c4, c1);
+
+    float i1=v21.cross(v32).z;
+    float i2=v32.cross(v43).z;
+    float i3=v43.cross(v14).z;
+    float i4=v14.cross(v21).z;
+
+    if (   (i1>0 && i2>0 && i3>0 && i4>0) 
+      || (i1<0 && i2<0 && i3<0 && i4<0))
+      return true;
+    else if (verbose)
+      System.out.println("Eliminating non-convex quad");
+    return false;
+  }
+
+  float validArea(PVector c1, PVector c2, PVector c3, PVector c4, float max_area, float min_area) {
+
+    float i1=c1.cross(c2).z;
+    float i2=c2.cross(c3).z;
+    float i3=c3.cross(c4).z;
+    float i4=c4.cross(c1).z;
+
+    float area = Math.abs(0.5f * (i1 + i2 + i3 + i4));
+
+    if (area < max_area && area > min_area)
+      return area;
+    return 0;
+  }
+
+  boolean nonFlatQuad(PVector c1, PVector c2, PVector c3, PVector c4) {
+
+    float min_cos = 0.5f;
+
+    PVector v21= PVector.sub(c1, c2);
+    PVector v32= PVector.sub(c2, c3);
+    PVector v43= PVector.sub(c3, c4);
+    PVector v14= PVector.sub(c4, c1);
+
+    float cos1=Math.abs(v21.dot(v32) / (v21.mag() * v32.mag()));
+    float cos2=Math.abs(v32.dot(v43) / (v32.mag() * v43.mag()));
+    float cos3=Math.abs(v43.dot(v14) / (v43.mag() * v14.mag()));
+    float cos4=Math.abs(v14.dot(v21) / (v14.mag() * v21.mag()));
+
+    if (cos1 < min_cos && cos2 < min_cos && cos3 < min_cos && cos4 < min_cos)
+      return true;
+    else {
+      if (verbose)
+        System.out.println("Flat quad");
+      return false;
+    }
+  }
+
+
+  ArrayList<PVector> sortCorners(ArrayList<PVector> quad) {
+
+    // 1 - Sort corners so that they are ordered clockwise
+    PVector a = quad.get(0);
+    PVector b = quad.get(2);
+
+    PVector center = new PVector((a.x+b.x)/2, (a.y+b.y)/2);
+
+    Collections.sort(quad, new CWComparator(center));
+
+
+
+    // 2 - Sort by upper left most corner
+    PVector origin = new PVector(0, 0);
+    float distToOrigin = 1000;
+
+    for (PVector p : quad) {
+      if (p.dist(origin) < distToOrigin) distToOrigin = p.dist(origin);
+    }
+
+    while (quad.get(0).dist(origin) != distToOrigin)
+      Collections.rotate(quad, 1);
+
+    return quad;
+  }
+
+  void displayLines (List<PVector> points) {
+    stroke(204, 102, 0);
+    for (int i = 0; i< points.size(); i++) {
+      PVector point1 = points.get(i);
+      PVector point2 = points.get((i+1)%points.size());
+      line(point1.x, point1.y, point2.x, point2.y);
+    }
+  }
+  void displayCorners(List<PVector> points) {
+    stroke(204, 102, 0);
+    fill(204, 102, 0);
+    for (int i = 0; i< points.size(); i++) {
+      PVector point = points.get(i);
+      ellipse(point.x, point.y, 6, 6);
+    }
+  }
+}
+
+class CWComparator implements Comparator<PVector> {
+
+  PVector center;
+
+  public CWComparator(PVector center) {
+    this.center = center;
+  }
+
+  @Override
+    public int compare(PVector b, PVector d) {
+    if (Math.atan2(b.y-center.y, b.x-center.x)<Math.atan2(d.y-center.y, d.x-center.x))      
+      return -1; 
+    else return 1;
+  }
+}
\ No newline at end of file