diff --git a/misc/perp b/misc/perp
index d687b927..9d4b48f4 100644
--- a/misc/perp
+++ b/misc/perp
@@ -2,21 +2,22 @@ pkg load optim;
 
 function y = f (x)
 
-  y(1) = (
+  y = [
+    (
       (x(3) - x(1)) * (x(7) - x(5))  +
       (x(4) - x(2)) * (x(8) - x(6))  
-    ) ^ 2;
-  y(2) = (
+    ) ^ 2,
+     (
       (x(11) - x(9)) * (x(7) - x(5))  +
       (x(12) - x(10)) * (x(8) - x(6))  
-    ) ^ 2;
+    ) ^ 2];
   y(3) = 0;y(4) = 0;y(5) = 0;y(6) = 0;y(7) = 0;y(8) = 0;
   y(9) = 0;y(10) = 0;y(11) = 0;y(12) = 0;
 endfunction
 
 x0 = [100, 100, 600, 600, 700, 600, 900, 100, 1100, 100, 1600, 600];
 #x = fminunc(@f, reshape(x0, 12,1));
-x = bfgsmin('f', {reshape(x0, 12,1)}); #WORKS!
+x = fsolve(@f, x0); #WORKS!
 
 #x = x0;
 l1 = [x(1), x(2); x(3), x(4)];
diff --git a/src/cad/fx/App2DCtrl.java b/src/cad/fx/App2DCtrl.java
index 234af8c9..876f8346 100644
--- a/src/cad/fx/App2DCtrl.java
+++ b/src/cad/fx/App2DCtrl.java
@@ -1,46 +1,42 @@
 package cad.fx;
 
 import cad.gcs.Constraint;
-import cad.gcs.GradientDescent;
-import cad.gcs.GradientDescent2;
-import cad.gcs.GradientDescent3;
+import cad.gcs.Figures;
 import cad.gcs.Param;
 import cad.gcs.Solver;
-import cad.gcs.constr.Constraint2;
+import cad.gcs.constr.P2LDistance;
 import cad.gcs.constr.Parallel;
 import cad.gcs.constr.Perpendicular;
-import cad.gcs.constr.Perpendicular2;
-import cad.gcs.constr.X;
-import cad.gcs.constr.XY;
 import cad.math.Vector;
 import gnu.trove.list.TDoubleList;
+import javafx.application.Platform;
 import javafx.event.ActionEvent;
 import javafx.fxml.Initializable;
 import javafx.scene.Group;
 import javafx.scene.control.Button;
 import javafx.scene.layout.Pane;
 import javafx.scene.shape.Line;
-import org.apache.commons.math3.analysis.MultivariateMatrixFunction;
 import org.apache.commons.math3.analysis.MultivariateVectorFunction;
-import org.apache.commons.math3.analysis.function.Max;
-import org.apache.commons.math3.linear.Array2DRowRealMatrix;
-import org.apache.commons.math3.linear.MatrixUtils;
-import org.apache.commons.math3.linear.RealMatrix;
-import org.apache.commons.math3.optim.ConvergenceChecker;
 import org.apache.commons.math3.optim.InitialGuess;
 import org.apache.commons.math3.optim.MaxEval;
 import org.apache.commons.math3.optim.MaxIter;
-import org.apache.commons.math3.optim.PointVectorValuePair;
 import org.apache.commons.math3.optim.nonlinear.vector.ModelFunction;
 import org.apache.commons.math3.optim.nonlinear.vector.ModelFunctionJacobian;
 import org.apache.commons.math3.optim.nonlinear.vector.Target;
 import org.apache.commons.math3.optim.nonlinear.vector.Weight;
-import org.apache.commons.math3.optim.nonlinear.vector.jacobian.GaussNewtonOptimizer;
+import org.apache.commons.math3.optim.nonlinear.vector.jacobian.LevenbergMarquardtOptimizer;
+import org.jacop.constraints.Sum;
 
 import java.net.URL;
+import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.List;
 import java.util.ResourceBundle;
+import java.util.concurrent.ExecutorService;
+import java.util.concurrent.Executors;
+import java.util.concurrent.ThreadPoolExecutor;
+
+import static java.util.Arrays.asList;
 
 public class App2DCtrl implements Initializable {
 
@@ -48,6 +44,7 @@ public class App2DCtrl implements Initializable {
 
   public Pane viewer;
   public Button solve;
+  public Button square;
 
   @Override
   public void initialize(URL location, ResourceBundle resources) {
@@ -62,13 +59,17 @@ public class App2DCtrl implements Initializable {
 
     content.getChildren().addAll(l1, l2, l3);
 
+    square.setOnAction(event -> {
+      solveFigure(Figures.square(100));
+    });
+
 
     solve.setOnAction(event -> {
 
       Vector as = new Vector(l1.getStartX(), l1.getStartY());
-      Vector ae = new Vector(l1.getEndX(),   l1.getEndY());
+      Vector ae = new Vector(l1.getEndX(), l1.getEndY());
       Vector bs = new Vector(l2.getStartX(), l2.getStartY());
-      Vector be = new Vector(l2.getEndX(),   l2.getEndY());
+      Vector be = new Vector(l2.getEndX(), l2.getEndY());
 
 
       Param l1p1x = new Param(l1.getStartX());
@@ -86,57 +87,53 @@ public class App2DCtrl implements Initializable {
       Param l3p2x = new Param(l3.getEndX());
       Param l3p2y = new Param(l3.getEndY());
 
-
+//      l2p2x.setLocked(true);
+//      l2p2y.setLocked(true);
+//      l2p1x.setLocked(true);
+//      l2p1y.setLocked(true);
+      
+      
       Perpendicular perpendicular = new Perpendicular(
-        l1p1x,
-        l1p1y,
-        l1p2x,
-        l1p2y,
-        l2p1x,
-        l2p1y,
-        l2p2x,
-        l2p2y
+              l1p1x,
+              l1p1y,
+              l1p2x,
+              l1p2y,
+              l2p1x,
+              l2p1y,
+              l2p2x,
+              l2p2y
       );
 
       Parallel parallel = new Parallel(
-        l3p1x,
-        l3p1y,
-        l3p2x,
-        l3p2y,
-        l2p1x,
-        l2p1y,
-        l2p2x,
-        l2p2y
+              l3p1x,
+              l3p1y,
+              l3p2x,
+              l3p2y,
+              l2p1x,
+              l2p1y,
+              l2p2x,
+              l2p2y
+      );
+
+      P2LDistance p2l1 = new P2LDistance(
+              10,
+              l3p1x, l3p1y,
+              l2p1x, l2p1y,
+              l2p2x, l2p2y
+      );
+
+      P2LDistance p2l2 = new P2LDistance(
+              10,
+              l1p2x, l1p2y,
+              l2p1x, l2p1y,
+              l2p2x, l2p2y
       );
 
 
-      XY xy = new XY(as, new Vector(100, 100));
-
-      X x = new X(
-        l1p1x,
-        l1p1y,
-        l1p2x,
-        l1p2y,
-        l2p1x,
-        l2p1y,
-        l2p2x,
-        l2p2y
-      );
-      
-      List<Constraint> constrs = Arrays.<Constraint>asList(perpendicular);
-      Solver.SubSystem subSystem = new Solver.SubSystem(constrs);
-//      Solver.optimize(subSystem);
-//
-      
-      
-//      while (subSystem.error() > 0.0001 ) {
-        Solver.solve_LM(subSystem);
-//      }
-      
-//      solveGC(subSystem);
-
-      java.lang.System.out.println("ANGLE |- :" + perpendicular.angle());
-      java.lang.System.out.println("ANGLE || :" + parallel.angle());
+      Runnable update = () -> {
+        System.out.println("ANGLE |- : " + perpendicular.angle());
+        System.out.println("ANGLE || : " + parallel.angle());
+        System.out.println("DISTANCE : " + p2l1.error());
 
 //      Constraint2 constr = xy;
 
@@ -157,27 +154,97 @@ public class App2DCtrl implements Initializable {
 //      l2.setEndX(be.x);
 //      l2.setEndY(be.y);
 
-      l1.setStartX(l1p1x.get());
-      l1.setStartY(l1p1y.get());
-      l1.setEndX(l1p2x.get());
-      l1.setEndY(l1p2y.get());
+        l1.setStartX(l1p1x.get());
+        l1.setStartY(l1p1y.get());
+        l1.setEndX(l1p2x.get());
+        l1.setEndY(l1p2y.get());
 
-      l2.setStartX(l2p1x.get());
-      l2.setStartY(l2p1y.get());
-      l2.setEndX(l2p2x.get());
-      l2.setEndY(l2p2y.get());
+        l2.setStartX(l2p1x.get());
+        l2.setStartY(l2p1y.get());
+        l2.setEndX(l2p2x.get());
+        l2.setEndY(l2p2y.get());
 
-      l3.setStartX(l3p1x.get());
-      l3.setStartY(l3p1y.get());
-      l3.setEndX(l3p2x.get());
-      l3.setEndY(l3p2y.get());
+        l3.setStartX(l3p1x.get());
+        l3.setStartY(l3p1y.get());
+        l3.setEndX(l3p2x.get());
+        l3.setEndY(l3p2y.get());
 
 //      scale(l1);
 //      scale(l2);
 //      scale(l3);
+      };
+
+
+      List<Constraint> constrs = Arrays.<Constraint>asList(parallel);
+//      List<Constraint> constrs = Arrays.<Constraint>asList(p2l1);
+      Solver.SubSystem subSystem = new Solver.SubSystem(constrs);
+//      Solver.optimize(subSystem);
+//
+
+      ExecutorService executor = Executors.newSingleThreadExecutor();
+      executor.execute(() -> {
+        globalSolve(subSystem, () -> Platform.runLater(update));
+        if (true) return;
+        while (subSystem.error() > 0.0001) {
+          Solver.solve_LM(subSystem);
+//          solveLM_COMMONS(subSystem);
+//        Solver.solve_DL(subSystem);
+//        Solver.solve_BFGS(subSystem, true);
+          Platform.runLater(update);
+        }
+      });
     });
   }
 
+  private void solveFigure(Figures.Figure square) {
+
+    Solver.SubSystem subSystem = new Solver.SubSystem(square.constraints);
+
+    List<Line> lines = new ArrayList<>();
+
+    for (Param[] line : square.lines) {
+      Line fxLine = new Line();
+      fxLine.setStartX(line[0].get());
+      fxLine.setStartY(line[1].get());
+      fxLine.setEndX(line[2].get());
+      fxLine.setEndY(line[3].get());
+      lines.add(fxLine);
+    }
+
+    ExecutorService executor = Executors.newSingleThreadExecutor();
+    executor.execute(() -> {
+      globalSolve(subSystem, () -> Platform.runLater(() -> {
+        for (int i = 0; i < square.lines.length; i++) {
+          Param[] line = square.lines[i];
+          Line fxLine = lines.get(i);
+          fxLine.setStartX(line[0].get());
+          fxLine.setStartY(line[1].get());
+          fxLine.setEndX(line[2].get());
+          fxLine.setEndY(line[3].get());
+        }
+      }));
+    });
+  }
+
+  private void globalSolve(Solver.SubSystem subSystem, Runnable linearSolvedCallback) {
+
+    double eps = 0.0001;
+    while (subSystem.error() > eps) {
+      solveLM_COMMONS(subSystem);
+//      Solver.solve_LM(subSystem);
+      TDoubleList residuals = subSystem.calcResidual();
+      double worseValue = residuals.max();
+      if (Math.abs(worseValue) > eps) {
+        int worseId = residuals.indexOf(worseValue);
+        Solver.SubSystem worse = new Solver.SubSystem(asList(subSystem.constraints.get(worseId)));
+        solveLM_COMMONS(worse);
+//        Solver.solve_LM(worse);
+        System.out.println("WORSE FIXED ERROR:" + worse.error());
+      }
+      linearSolvedCallback.run();
+    }
+  }
+
   double xxx = 100;
 
   private void scale(Line l) {
@@ -191,18 +258,12 @@ public class App2DCtrl implements Initializable {
     l.setEndY(l.getStartY() + v.y);
   }
 
-  private void solveGC(final Solver.SubSystem subSystem) {
-    GaussNewtonOptimizer optimizer = new GaussNewtonOptimizer((iteration, previous, current) -> {
-      return subSystem.value() < 0.00001;
-    }) {
+  private void solveLM_COMMONS(final Solver.SubSystem subSystem) {
+    double eps = 1e-10, eps1 = 1e-80;
+      double tau = 1e-3;
 
-      @Override
-      protected double[] computeResiduals(double[] objectiveValue) {
-        TDoubleList residual = subSystem.calcResidual();
-        return residual.toArray();
-      }
+    LevenbergMarquardtOptimizer optimizer = new LevenbergMarquardtOptimizer(eps, eps, eps1);
 
-    };
     double[] wieght = new double[subSystem.cSize()];
     Arrays.fill(wieght, 1);
     optimizer.optimize(
diff --git a/src/cad/fx/app2d.fxml b/src/cad/fx/app2d.fxml
index dea147e3..60e2ae62 100644
--- a/src/cad/fx/app2d.fxml
+++ b/src/cad/fx/app2d.fxml
@@ -15,6 +15,7 @@
     <Region styleClass="spacer"/>
     <HBox styleClass="segmented-button-bar">
       <Button text="solve" fx:id="solve"/>
+      <Button text="square" fx:id="square"/>
     </HBox>
   </ToolBar>
 
@@ -22,4 +23,4 @@
 
   </Pane>
 
-</VBox>
\ No newline at end of file
+</VBox>
diff --git a/src/cad/gcs/Constraint.java b/src/cad/gcs/Constraint.java
index 72b3a041..bdb7dd73 100644
--- a/src/cad/gcs/Constraint.java
+++ b/src/cad/gcs/Constraint.java
@@ -3,7 +3,4 @@ package cad.gcs;
 public interface Constraint extends System {
 
   double error();
-
-  void set(double[] input);
-
 }
diff --git a/src/cad/gcs/Figures.java b/src/cad/gcs/Figures.java
new file mode 100644
index 00000000..b2b49044
--- /dev/null
+++ b/src/cad/gcs/Figures.java
@@ -0,0 +1,58 @@
+package cad.gcs;
+
+import cad.gcs.constr.Equals;
+
+import java.util.ArrayList;
+import java.util.List;
+
+public class Figures {
+
+  public static final int X1 = 0;
+  public static final int Y1 = 1;
+  public static final int X2 = 2;
+  public static final int Y2 = 3;
+  public static final int X3 = 4;
+  public static final int Y3 = 5;
+  public static final int X4 = 6;
+  public static final int Y4 = 7;
+
+  public static Figure square(double size) {
+
+    List<Constraint> constrs = new ArrayList<>();
+
+    Param[] l1 = line();
+    Param[] l2 = line();
+    Param[] l3 = line();
+    Param[] l4 = line();
+
+    constrs.add(new Equals(l1[X1], l4[X2]));
+    constrs.add(new Equals(l1[Y1], l4[Y2]));
+    constrs.add(new Equals(l2[X1], l1[X2]));
+    constrs.add(new Equals(l2[Y1], l1[Y2]));
+    constrs.add(new Equals(l3[X1], l2[X2]));
+    constrs.add(new Equals(l3[Y1], l2[Y2]));
+    constrs.add(new Equals(l4[X1], l4[X2]));
+    constrs.add(new Equals(l4[Y1], l4[Y2]));
+    constrs.add(new Equals(l1[Y1], l1[Y2]));
+    constrs.add(new Equals(l3[Y1], l1[Y2]));
+    constrs.add(new Equals(l2[X1], l1[X2]));
+    constrs.add(new Equals(l4[X1], l1[X2]));
+
+    return new Figure(new Param[][]{l1, l2, l3, l4}, constrs);
+  }
+
+  private static Param[] line() {
+    return new Param[]{new Param(10), new Param(10), new Param(100), new Param(100)};
+  }
+
+  public static class Figure {
+
+    public final Param[][] lines;
+    public final List<Constraint> constraints;
+
+    public Figure(Param[][] lines, List<Constraint> constraints) {
+      this.lines = lines;
+      this.constraints = constraints;
+    }
+  }
+}
diff --git a/src/cad/gcs/GradientDescent.java b/src/cad/gcs/GradientDescent.java
index 16d41850..794e390f 100644
--- a/src/cad/gcs/GradientDescent.java
+++ b/src/cad/gcs/GradientDescent.java
@@ -10,43 +10,43 @@ public class GradientDescent {
   
   public static void solve(Constraint constr) {
 
-
-    double last = value(constr);
-    
-    double alpha = 10;
-    int pSize = constr.pSize();
-
-    RealVector steps = new ArrayRealVector(pSize);
-    steps.set(10);
-    
-    for (int i = 0; i < 1000000; i++) {
-
-
-      double[] gradData = new double[pSize];
-      constr.gradient(gradData);      
-      ArrayRealVector grad = new ArrayRealVector(gradData);
-
-      RealVector dir = grad.mapDivide(grad.getNorm());
-      dir = dir.mapMultiply( alpha);
-      java.lang.System.out.println(dir.getNorm());
-
-
-      ArrayRealVector params = new ArrayRealVector(constr.params());
-      params = params.add(dir);
-      constr.set(params.toArray());
-      java.lang.System.out.println(((Perpendicular) constr).angle());
-//      constr.step(alpha);
-      double err = value(constr);
-      
-      if (err < last) {
-
-      } else if (alpha < EPS) {
-        return;
-      } else {
-        alpha /= 3;       
-      }
-      last = err;
-    }
+//
+//    double last = value(constr);
+//
+//    double alpha = 10;
+//    int pSize = constr.pSize();
+//
+//    RealVector steps = new ArrayRealVector(pSize);
+//    steps.set(10);
+//
+//    for (int i = 0; i < 1000000; i++) {
+//
+//
+//      double[] gradData = new double[pSize];
+//      constr.gradient(gradData);
+//      ArrayRealVector grad = new ArrayRealVector(gradData);
+//
+//      RealVector dir = grad.mapDivide(grad.getNorm());
+//      dir = dir.mapMultiply( alpha);
+//      java.lang.System.out.println(dir.getNorm());
+//
+//
+//      ArrayRealVector params = new ArrayRealVector(constr.params());
+//      params = params.add(dir);
+//      constr.set(params.toArray());
+//      java.lang.System.out.println(((Perpendicular) constr).angle());
+////      constr.step(alpha);
+//      double err = value(constr);
+//
+//      if (err < last) {
+//
+//      } else if (alpha < EPS) {
+//        return;
+//      } else {
+//        alpha /= 3;
+//      }
+//      last = err;
+//    }
   }
 
   private static double value(Constraint constr) {
diff --git a/src/cad/gcs/Param.java b/src/cad/gcs/Param.java
index 35ec7ed9..c052b8c5 100644
--- a/src/cad/gcs/Param.java
+++ b/src/cad/gcs/Param.java
@@ -3,7 +3,8 @@ package cad.gcs;
 public class Param {
 
   public double value;
-
+  public boolean locked;
+  
   public Param(double value) {
     this.value = value;
   }
@@ -15,4 +16,12 @@ public class Param {
   public double set(double value) {
     return this.value = value;
   }
+
+  public boolean isLocked() {
+    return locked;
+  }
+
+  public void setLocked(boolean locked) {
+    this.locked = locked;
+  }
 }
diff --git a/src/cad/gcs/Solver.java b/src/cad/gcs/Solver.java
index b970004a..d9375e8d 100644
--- a/src/cad/gcs/Solver.java
+++ b/src/cad/gcs/Solver.java
@@ -68,6 +68,7 @@ public class Solver {
     double alpha = 0.;
     double nu = 2.;
     int iter = 0, stop = 0, reduce = 0;
+    double mu =  1e-8;
     while (stop == 0) {
 
       // check if finished
@@ -86,9 +87,38 @@ public class Solver {
         alpha = squaredNorm(g) / squaredNorm((Jx.multiply(g)));
         h_sd = g.scalarMultiply(alpha);
 
+        RealMatrix A = Jx.transpose().multiply(Jx);
+        RealMatrix gg = Jx.transpose().multiply(fx.scalarMultiply(-1));
+        double[] diag_A = diagonal(A);
+        
+        double mu_increase_factor_ = 10.0;
+        do  {
+          for (int i = 0; i < xsize; ++i) {
+            A.addToEntry(i, i, mu);
+          }
+
+          boolean success = true;
+          try {
+            h_gn = new LUDecomposition(A).getSolver().solve(gg);
+          } catch (Exception ssse) {
+            java.lang.System.out.println(ssse.getMessage());
+            success = false;
+          }
+          if (success) {
+            break;
+          }
+          mu *= mu_increase_factor_;
+        } while (mu < 1.0);
+        
         // get the gauss-newton step
-        h_gn = lu(Jx, fx.scalarMultiply(-1));
-        double rel_error = (Jx.transpose().multiply(h_gn).add(fx)).getFrobeniusNorm() / fx.getFrobeniusNorm();
+//        h_gn = new LUDecomposition(new Array2DRowRealMatrix(makeSquare(Jx.getData()))).getSolver().solve(fx.scalarMultiply(-1));
+
+
+        for (int i = 0; i < xsize; ++i) // restore diagonal J^T J entries
+        {
+          A.setEntry(i, i, diag_A[i]);
+        }
+        double rel_error = (Jx.multiply(h_gn).add(fx)).getFrobeniusNorm() / fx.getFrobeniusNorm();
         if (rel_error > 1e15)
           break;
 
@@ -142,9 +172,9 @@ public class Solver {
       double rho = dL / dF;
 
       if (dF > 0 && dL > 0) {
-        x = x_new;
-        Jx = Jx_new;
-        fx = fx_new;
+        x = x_new.copy();
+        Jx = Jx_new.copy();
+        fx = fx_new.copy();
         err = err_new;
 
         g = Jx.transpose().multiply(fx.scalarMultiply(-1));
@@ -401,14 +431,22 @@ public class Solver {
 
         //solve augmented functions A*h=-g
 
-        try {
-          h = new LUDecomposition(A).getSolver().solve(g);
-        } catch (Exception ssse) {
-          ssse.printStackTrace();
-          return SolveStatus.Success;
+        for (int _ = 0; _ < 1000; _++) {
+          try {
+            h = new LUDecomposition(A).getSolver().solve(g);
+          } catch (Exception ssse) {
+            mu *= 1./3.;
+            for (int i = 0; i < xsize; ++i) {
+              A.setEntry(i, i, diag_A[i] * mu);
+            }
+            if (_ == 999) {
+              return SolveStatus.Success;
+            }
+            continue;
+          }
+          break;
         }
         
-        ;
         double rel_error = (A.multiply(h).subtract(g)).getFrobeniusNorm() / g.getFrobeniusNorm();
 
         // check if solving works
@@ -690,7 +728,7 @@ public class Solver {
 
   public static class SubSystem {
 
-    private final List<Constraint> constraints;
+    public final List<Constraint> constraints;
     private final LinkedHashMap<Param, ParamInfo> params = new LinkedHashMap<>();
 
     public SubSystem(List<Constraint> constraints) {
@@ -790,7 +828,7 @@ public class Solver {
         for (int p = 0; p < cParams.length; p++) {
           Param param = cParams[p];
           int j = params.get(param).id;
-          jacobi.setEntry(i,j, grad[p]);
+          jacobi.setEntry(i,j, param.isLocked() ? 0 : grad[p]);
         }
       }
     }
@@ -803,7 +841,7 @@ public class Solver {
 
 
     public void setParams(RealMatrix params) {
-      setParams(params.getData()[0]);
+      setParams(params.getColumn(0));
     }
     
     public void setParams(double[] arr) {
diff --git a/src/cad/gcs/System.java b/src/cad/gcs/System.java
index 56a2b5e9..5a9326b9 100644
--- a/src/cad/gcs/System.java
+++ b/src/cad/gcs/System.java
@@ -4,8 +4,6 @@ public interface System {
 
   Param[] getParams();
 
-  double[] params();
-  
   void gradient(double[] out);
   
   int pSize();
diff --git a/src/cad/gcs/constr/Equals.java b/src/cad/gcs/constr/Equals.java
new file mode 100644
index 00000000..12a643f8
--- /dev/null
+++ b/src/cad/gcs/constr/Equals.java
@@ -0,0 +1,34 @@
+package cad.gcs.constr;
+
+import cad.gcs.Constraint;
+import cad.gcs.Param;
+
+public class Equals implements Constraint {
+
+  private final Param[] params;
+
+  public Equals(Param p1, Param p2) {
+    this.params = new Param[]{p1, p2};
+  }
+
+  @Override
+  public double error() {
+    return params[0].get() - params[1].get();
+  }
+
+  @Override
+  public Param[] getParams() {
+    return params;
+  }
+
+  @Override
+  public void gradient(double[] out) {
+    out[0] = 1;
+    out[1] = -1;
+  }
+
+  @Override
+  public int pSize() {
+    return params.length;
+  }
+}
diff --git a/src/cad/gcs/constr/P2LDistance.java b/src/cad/gcs/constr/P2LDistance.java
new file mode 100644
index 00000000..706e40ee
--- /dev/null
+++ b/src/cad/gcs/constr/P2LDistance.java
@@ -0,0 +1,193 @@
+package cad.gcs.constr;
+
+import cad.gcs.Constraint;
+import cad.gcs.Param;
+import cad.math.Vector;
+
+import static java.lang.Math.abs;
+import static java.lang.Math.sqrt;
+
+public class P2LDistance implements Constraint {
+
+  private final Param[] params = new Param[6];
+
+  public static final int tx = 0;
+  public static final int ty = 1;
+  public static final int lp1x = 2;
+  public static final int lp1y = 3;
+  public static final int lp2x = 4;
+  public static final int lp2y = 5;
+
+  private final double distance;
+
+  public P2LDistance(double distance, Param...params) {
+    this.distance = distance;
+    System.arraycopy(params, 0, this.params, 0, params.length);
+  }
+
+  public double error() {
+    double x0 = p0x(), x1 = p1x(), x2 = p2x();
+    double y0 = p0y(), y1 = p1y(), y2 = p2y();
+    double dist = distance();
+    double dx = x2 - x1;
+    double dy = y2 - y1;
+    double d = sqrt(dx * dx + dy * dy);
+    double area = abs
+            (-x0 * dy + y0 * dx + x1 * y2 - x2 * y1); // = x1y2 - x2y1 - x0y2 + x2y0 + x0y1 - x1y0 = 2*(triangle area)
+    return (area / d - dist);
+
+  }
+
+
+  public double error2() {
+//    //Basis
+    double dx = params[lp2x].get() - params[lp1x].get();
+    double dy = params[lp2y].get() - params[lp1y].get();
+    Vector n = new Vector(-dy, dx).normalize();
+    Vector target = new Vector(params[tx].get() - params[lp1x].get(), params[ty].get() - params[lp1y].get());
+    return distance - target.dot(n);
+
+  }
+
+  private double distance() {
+    return distance;
+  }
+
+  private double p1x() {
+    return params[lp1x].get();
+  }
+
+  private double p1y() {
+    return params[lp1y].get();
+  }
+
+  private double p2x() {
+    return params[lp2x].get();
+  }
+
+  private double p2y() {
+    return params[lp2y].get();
+  }
+
+  private double p0x() {
+    return params[tx].get();
+  }
+
+  private double p0y() {
+    return params[ty].get();
+  }
+
+  public void gradient1(double[] out) {
+    double x1 = params[lp1x].get();
+    double x3 = params[lp2x].get();
+    double x2 = params[lp1y].get();
+    double x4 = params[lp2y].get();
+    double x5 = params[tx].get();
+    double x6 = params[ty].get();
+
+//    double dx = x3 - x1;
+//    double dy = x4 - x2;
+//    Vector n = new Vector(-dy, dx).normalize();
+//    Vector target = new Vector(x5 - x1, x6 - x2);
+//
+//    double nx = (x2 - x4) / sqrt( (x2 - x4)^2 + (x3 - x1)^2 );
+//    double ny = (x4 - x2) / sqrt( (x2 - x4)^2 + (x3 - x1)^2 );
+//
+//    double dot = (x5 - x1)*nx + (x6 - x2)*ny;
+//    g(x1, x2, x3, x4)=sqrt( (x2 - x4)^2 + (x3 - x1)^2 );
+//    f(x1, x2, x3, x4, x5, x6) = distance - (x5 - x1)*(x2 - x4) / g(x1, x2, x3, x4) + (x6 - x2) * (x4 - x2) / g(x1, x2, x3, x4);
+//
+//    f(x1) = distance - (x5 - x1)*(x2 - x4) / sqrt( (x2 - x4)^2 + (x3 - x1)^2 ) + (x6 - x2) * (x4 - x2) / sqrt( (x2 - x4)^2 + (x3 - x1)^2 );
+
+
+    //MAXIMA
+//    diff(distance - (x5 - x1)*(x2 - x4) / sqrt( (x2 - x4)^2 + (x3 - x1)^2 ) + (
+//    x6 - x2) * (x4 - x2) / sqrt( (x2 - x4)^2 + (x3 - x1)^2 ), x1);
+
+
+
+
+//    (x3-x1)*(x4-x2)*(x6-x2) / Math.pow(())
+
+
+
+  }
+
+  public void gradient(double[] out) {
+    double x0 = p0x(), x1 = p1x(), x2 = p2x();
+    double y0 = p0y(), y1 = p1y(), y2 = p2y();
+    double dx = x2 - x1;
+    double dy = y2 - y1;
+    double d2 = dx * dx + dy * dy;
+    double d = sqrt(d2);
+    double area = -x0 * dy + y0 * dx + x1 * y2 - x2 * y1;
+    out[tx]   = (y1-y2)*(x1*y2-x0*y2-x2*y1+x0*y1+x2*y0-x1*y0)/(sqrt(sq(y2-y1)+sq(x2-x1))
+    *abs(x1*y2-x0*y2-x2*y1+x0*y1+x2*y0-x1*y0));
+    
+    out[ty]   = (x2-x1)*(x1*y2-x0*y2-x2*y1+x0*y1+x2*y0-x1*y0)/(sqrt(sq(y2-y1)+sq(x2-x1))
+    *abs(x1*y2-x0*y2-x2*y1+x0*y1+x2*y0-x1*y0));
+    
+    out[lp1x] = (x2-x1)* abs(x1 * y2 - x0 * y2 - x2 * y1 + x0 * y1 + x2 * y0 - x1 * y0)/p(sq(y2-y1)+sq(x2-x1), 3/2)
+            +(y2-y0)*(x1*y2-x0*y2-x2*y1+x0*y1+x2*y0-x1*y0)/(sqrt(sq(y2-y1)+sq(x2-x1))*Math.abs(x1 * y2 - x0 * y2 - x2 * y1 + x0 * y1 + x2 * y0 - x1 * y0));
+
+    out[lp1y] = (y2-y1)*abs(x1*y2-x0*y2-x2*y1+x0*y1+x2*y0-x1*y0)/p(sq(y2-y1)+sq(x2-x1),3/2)
+     +(x0-x2)*(x1*y2-x0*y2-x2*y1+x0*y1+x2*y0-x1*y0)/(sqrt(sq(y2-y1)+sq(x2-x1))*
+    abs(x1*y2-x0*y2-x2*y1+x0*y1+x2*y0-x1*y0));
+    
+    out[lp2x] = (y0-y1)*(x1*y2-x0*y2-x2*y1+x0*y1+x2*y0-x1*y0)/(sqrt(sq(y2-y1)+sq(x2-x1))
+                *abs(x1*y2-x0*y2-x2*y1+x0*y1+x2*y0-x1*y0))-(x2-x1)*abs(x1*y2-x0*y2-x2*y1+x0*y1
+                +x2*y0-x1*y0)/p(sq(y2-y1)+sq(x2-x1), 3/2);
+    
+    out[lp2y] = (x1-x0)*(x1*y2-x0*y2-x2*y1+x0*y1+x2*y0-x1*y0)/(sqrt(sq(y2-y1)+sq(x2-x1))
+    *abs(x1*y2-x0*y2-x2*y1+x0*y1+x2*y0-x1*y0))-(y2-y1)*abs(x1*y2-x0*y2-x2*y1+x0*y1
+            +x2*y0-x1*y0)/p(sq(y2-y1)+sq(x2-x1),3/2);
+
+//    if (area < 0) {
+//      for (int i = 0; i < 6; i++) {
+//        out[i] *= -1;
+//      }
+//    }
+  }
+
+  private double p(double v, int i) {
+    return Math.pow(v, i);
+  }
+
+  private double sq(double a) {
+    return a*a;
+  }
+
+  public void gradient2(double[] out) {
+    double x0 = p0x(), x1 = p1x(), x2 = p2x();
+    double y0 = p0y(), y1 = p1y(), y2 = p2y();
+    double dx = x2 - x1;
+    double dy = y2 - y1;
+    double d2 = dx * dx + dy * dy;
+    double d = sqrt(d2);
+    double area = -x0 * dy + y0 * dx + x1 * y2 - x2 * y1;
+    out[tx]   = ((y1 - y2) / d);
+    out[ty]   = ((x2 - x1) / d);
+    out[lp1x] = (((y2 - y0) * d + (dx / d) * area) / d2);
+    out[lp1y] = (((x0 - x2) * d + (dy / d) * area) / d2);
+    out[lp2x] = (((y0 - y1) * d - (dx / d) * area) / d2);
+    out[lp2y] = (((x1 - x0) * d - (dy / d) * area) / d2);
+
+    if (area < 0) {
+      for (int i = 0; i < 6; i++) {
+        out[i] *= -1;
+      }
+    }
+  }
+
+  @Override
+  public Param[] getParams() {
+    return params;
+  }
+
+  @Override
+  public int pSize() {
+    return 6;
+  }
+
+
+}
diff --git a/src/cad/gcs/constr/Parallel.java b/src/cad/gcs/constr/Parallel.java
index 95e38cde..818dc6a8 100644
--- a/src/cad/gcs/constr/Parallel.java
+++ b/src/cad/gcs/constr/Parallel.java
@@ -53,20 +53,6 @@ public class Parallel implements Constraint {
     return params;
   }
 
-  @Override
-  public double[] params() {
-    double[] _params = new double[8];
-    _params[l1p1x] = params[l1p1x].get();
-    _params[l1p1y] = params[l1p1y].get();
-    _params[l1p2x] = params[l1p2x].get();
-    _params[l1p2y] = params[l1p2y].get();
-    _params[l2p1x] = params[l2p1x].get();
-    _params[l2p1y] = params[l2p1y].get();
-    _params[l2p2x] = params[l2p2x].get();
-    _params[l2p2y] = params[l2p2y].get();
-    return _params;
-  }
-
   @Override
   public double error() {
     double dx1 =  (params[l1p2x].get() - params[l1p1x].get());
@@ -141,7 +127,6 @@ public class Parallel implements Constraint {
     return 8;
   }
 
-  @Override
   public void set(double[] input) {
     params[l1p1x].set(input[l1p1x]);
     params[l1p1y].set(input[l1p1y]);
diff --git a/src/cad/gcs/constr/Perpendicular.java b/src/cad/gcs/constr/Perpendicular.java
index 72aaa938..1c40a546 100644
--- a/src/cad/gcs/constr/Perpendicular.java
+++ b/src/cad/gcs/constr/Perpendicular.java
@@ -53,20 +53,6 @@ public class Perpendicular implements Constraint {
     return params;
   }
 
-  @Override
-  public double[] params() {
-    double[] _params = new double[8];
-    _params[l1p1x] = params[l1p1x].get();
-    _params[l1p1y] = params[l1p1y].get();
-    _params[l1p2x] = params[l1p2x].get();
-    _params[l1p2y] = params[l1p2y].get();
-    _params[l2p1x] = params[l2p1x].get();
-    _params[l2p1y] = params[l2p1y].get();
-    _params[l2p2x] = params[l2p2x].get();
-    _params[l2p2y] = params[l2p2y].get();
-    return _params;
-  }
-
   @Override
   public double error() {
     double dx1 = (params[l1p2x].get() - params[l1p1x].get());
@@ -178,7 +164,6 @@ public class Perpendicular implements Constraint {
     return 8;
   }
 
-  @Override
   public void set(double[] input) {
     params[l1p1x].set(input[l1p1x]);
     params[l1p1y].set(input[l1p1y]);
diff --git a/src/cad/gcs/constr/X.java b/src/cad/gcs/constr/X.java
deleted file mode 100644
index 7c72b985..00000000
--- a/src/cad/gcs/constr/X.java
+++ /dev/null
@@ -1,156 +0,0 @@
-package cad.gcs.constr;
-
-import cad.gcs.Constraint;
-import cad.gcs.Param;
-import cad.math.Vector;
-
-public class X implements Constraint {
-
-  public static final int l1p1x = 0;
-  public static final int l1p1y = 1;
-  public static final int l1p2x = 2;
-  public static final int l1p2y = 3;
-  public static final int l2p1x = 4;
-  public static final int l2p1y = 5;
-  public static final int l2p2x = 6;
-  public static final int l2p2y = 7;
-
-  private final Param[] params = new Param[8];
-
-  public X(
-          Param _l1p1x,
-          Param _l1p1y,
-          Param _l1p2x,
-          Param _l1p2y,
-          Param _l2p1x,
-          Param _l2p1y,
-          Param _l2p2x,
-          Param _l2p2y
-  ) {
-    params[l1p1x] = _l1p1x;
-    params[l1p1y] = _l1p1y;
-    params[l1p2x] = _l1p2x;
-    params[l1p2y] = _l1p2y;
-    params[l2p1x] = _l2p1x;
-    params[l2p1y] = _l2p1y;
-    params[l2p2x] = _l2p2x;
-    params[l2p2y] = _l2p2y;
-  }
-
-  public void out(Vector p1, Vector p2, Vector p3, Vector p4) {
-    p1.x = params[l1p1x].get();
-    p1.y = params[l1p1y].get();
-    p2.x = params[l1p2x].get();
-    p2.y = params[l1p2y].get();
-    p3.x = params[l2p1x].get();
-    p3.y = params[l2p1y].get();
-    p4.x = params[l2p2x].get();
-    p4.y = params[l2p2y].get();
-  }
-
-  public double valueX() {
-    return (params[l2p2x].get() - params[l2p1x].get()) * .5 - (params[l1p2x].get() - params[l1p1x].get()) * .5;
-  }
-  
-  public double valueY() {
-    return (params[l2p2y].get() - params[l2p1y].get()) * .5 - (params[l1p2y].get() - params[l1p1y].get()) * .5;
-  }
-
-  @Override
-  public Param[] getParams() {
-    return params;
-  }
-
-  @Override
-  public double[] params() {
-    double[] _params = new double[8];
-    _params[l1p1x] = params[l1p1x].get();
-    _params[l1p1y] = params[l1p1y].get();
-    _params[l1p2x] = params[l1p2x].get();
-    _params[l1p2y] = params[l1p2y].get();
-    _params[l2p1x] = params[l2p1x].get();
-    _params[l2p1y] = params[l2p1y].get();
-    _params[l2p2x] = params[l2p2x].get();
-    _params[l2p2y] = params[l2p2y].get();
-    return _params;
-  }
-
-  @Override
-  public double error() {
-    double v = valueX() + valueY();
-    return v;
-  }
-
-  @Override
-  public void gradient(double[] out) {
-
-    double valueX = valueX();
-    double valueY = valueY();
-
-    Vector p1 = new Vector(); 
-    Vector p2  = new Vector(); 
-    Vector p3  = new Vector(); 
-    Vector p4  = new Vector();    
-    
-    out(p1, p2, p3, p4);
-    
-    Vector da = p2.minus(p1);
-    Vector db = p4.minus(p3);
-
-    Vector g1 = p1.multi(valueX, valueY, 0);
-    Vector g2 = p2.multi(valueX, valueY, 0).multi(-1);
-    Vector g3 = p3.multi(valueX, valueY, 0).multi(-1);
-    Vector g4 = p4.multi(valueX, valueY, 0);
-    
-    out[l1p1x] =  g1.x; // = dx2
-    out[l1p1y] =  g1.y; // = dx2
-    
-    out[l1p2x] = g2.x;
-    out[l1p2y] = g2.y;
-    
-    out[l2p1x] =  g3.x;
-    out[l2p1y] =  g3.y;
-    
-    out[l2p2x] = g4.x;
-    out[l2p2y] = g4.y;
-  }
-
-
-  public double angle() {
-    double dx1 = (params[l1p1x].get() - params[l1p2x].get());
-    double dy1 = (params[l1p1y].get() - params[l1p2y].get());
-    double dx2 = (params[l2p1x].get() - params[l2p2x].get());
-    double dy2 = (params[l2p1y].get() - params[l2p2y].get());
-    //dot product shows how the lines off to be perpendicular
-    double xl = Math.abs(dx1 * dx1 - dx2 * dx2);
-    double yl = Math.abs(dy1*dy1 - dy2*dy2);
-    double off = (dx1 * dx2 + dy1 * dy2) / (xl*yl);
-
-    return Math.acos(off) / Math.PI * 180;
-  }
-
-  private void step(int px, int py, double gx, double gy, double alpha) {
-    Vector dd = new Vector(gx, gy).normalize().multi(alpha);
-    Vector n = new Vector(params[px].get(), params[py].get()).plus(dd);
-    params[px].set(n.x);
-    params[py].set(n.y);
-  }
-
-  @Override
-  public int pSize() {
-    return 8;
-  }
-
-  @Override
-  public void set(double[] input) {
-    params[l1p1x].set(input[l1p1x]);
-    params[l1p1y].set(input[l1p1y]);
-    params[l1p2x].set(input[l1p2x]);
-    params[l1p2y].set(input[l1p2y]);
-    params[l2p1x].set(input[l2p1x]);
-    params[l2p1y].set(input[l2p1y]);
-    params[l2p2x].set(input[l2p2x]);
-    params[l2p2y].set(input[l2p2y]);
-  }
-
-}