diff --git a/misc/perp b/misc/perp
new file mode 100644
index 00000000..d687b927
--- /dev/null
+++ b/misc/perp
@@ -0,0 +1,26 @@
+pkg load optim;
+
+function y = f (x)
+
+  y(1) = (
+      (x(3) - x(1)) * (x(7) - x(5))  +
+      (x(4) - x(2)) * (x(8) - x(6))  
+    ) ^ 2;
+  y(2) = (
+      (x(11) - x(9)) * (x(7) - x(5))  +
+      (x(12) - x(10)) * (x(8) - x(6))  
+    ) ^ 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 = x0;
+l1 = [x(1), x(2); x(3), x(4)];
+l2 = [x(5), x(6); x(7), x(8)];
+l3 = [x(9), x(10); x(11), x(12)];
+
+plot(l1(:,1), l1(:,2), l2(:,1), l2(:,2), l3(:,1), l3(:,2),'-');
diff --git a/misc/simple.m b/misc/simple.m
new file mode 100644
index 00000000..7c449e13
--- /dev/null
+++ b/misc/simple.m
@@ -0,0 +1,31 @@
+function y = angle (x)
+
+a = [x(3) - x(1); x(4) - x(2)];
+b = [x(7) - x(5); x(8) - x(6)];
+
+y = acos(dot(a, b) / (norm(a) * norm(b))) / pi * 180;
+
+endfunction
+
+function y = f (x)
+  y(1) = (
+    (x(3) - x(1)) * (x(7) - x(5))  +
+    (x(4) - x(2)) * (x(8) - x(6))  
+  ) ^ 2;
+endfunction
+
+x0 = [100, 100, 600, 600, 700, 600, 900, 100];
+x = sqp(x0, @f);
+
+#pkg load optim;
+#x = bfgsmin('f', {reshape(x0, 8,1)}); #WORKS!
+
+
+l1 = [x(1), x(2); x(3), x(4)];
+l2 = [x(5), x(6); x(7), x(8)];
+
+plot(l1(:,1), l1(:,2), l2(:,1), l2(:,2), '-');
+
+
+#d = (x(3) - x(1)) * (x(7) - x(5))  + (x(4) - x(2)) * (x(8) - x(6)) ;
+disp("Angle: "), disp(angle(x));
diff --git a/src/cad/fx/App2DCtrl.java b/src/cad/fx/App2DCtrl.java
index 304beae1..17b26c62 100644
--- a/src/cad/fx/App2DCtrl.java
+++ b/src/cad/fx/App2DCtrl.java
@@ -1,15 +1,40 @@
 package cad.fx;
 
 import cad.gcs.Constraint;
+import cad.gcs.GradientDescent;
+import cad.gcs.GradientDescent2;
+import cad.gcs.GradientDescent3;
+import cad.gcs.Param;
 import cad.gcs.Solver;
+import cad.gcs.constr.Constraint2;
 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.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 java.net.URL;
 import java.util.Arrays;
@@ -35,33 +60,135 @@ public class App2DCtrl implements Initializable {
     content.getChildren().addAll(l1, l2);
 
 
+
+
     solve.setOnAction(event -> {
 
-      Vector a1 = new Vector(l1.getStartX(), l1.getStartY());
-      Vector b1 = new Vector(l1.getEndX(),   l1.getEndY());
-      Vector a2 = new Vector(l2.getStartX(), l2.getStartY());
-      Vector b2 = new Vector(l2.getEndX(),   l2.getEndY());
-      Perpendicular perpendicular = new Perpendicular(a1, b1, a2, b2);
+      Vector as = new Vector(l1.getStartX(), l1.getStartY());
+      Vector ae = new Vector(l1.getEndX(),   l1.getEndY());
+      Vector bs = new Vector(l2.getStartX(), l2.getStartY());
+      Vector be = new Vector(l2.getEndX(),   l2.getEndY());
 
-      List<Constraint> parallels = Arrays.<Constraint>asList(perpendicular);
-      Solver.SubSystem subSystem = new Solver.SubSystem(parallels);
-      Solver.solve_DL(subSystem);
 
-      perpendicular.out(a1, b1, a2, b2);
+      Param l1p1x = new Param(l1.getStartX());
+      Param l1p1y = new Param(l1.getStartY());
+      Param l1p2x = new Param(l1.getEndX());
+      Param l1p2y = new Param(l1.getEndY());
+      Param l2p1x = new Param(l2.getStartX());
+      Param l2p1y = new Param(l2.getStartY());
+      Param l2p2x = new Param(l2.getEndX());
+      Param l2p2y = new Param(l2.getEndY());
 
-      l1.setStartX(a1.x);
-      l1.setStartY(a1.y);
-      l1.setEndX(b1.x);
-      l1.setEndY(b1.y);
 
-      l2.setStartX(a2.x);
-      l2.setStartY(a2.y);
-      l2.setEndX(b2.x);
-      l2.setEndY(b2.y);
+      Perpendicular2 perpendicular2 = new Perpendicular2(as, ae, bs, be);
+
+
+      Perpendicular perpendicular = new Perpendicular(
+        l1p1x,
+        l1p1y,
+        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(perpendicular.angle());
+
+      Constraint2 constr = perpendicular2;
+//      Constraint2 constr = xy;
+
+//      Constraint constr = perpendicular;
+//      GradientDescent.solve(constr);
+//      perpendicular.out(a1, b1, a2, b2);
+
+//      GradientDescent2.solve(constr);
+
+
+//      l1.setStartX(as.x);
+//      l1.setStartY(as.y);
+//      l1.setEndX(ae.x);
+//      l1.setEndY(ae.y);
+//
+//      l2.setStartX(bs.x);
+//      l2.setStartY(bs.y);
+//      l2.setEndX(be.x);
+//      l2.setEndY(be.y);
+
+      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());
     });
 
   }
 
+  private void solveGC(final Solver.SubSystem subSystem) {
+    GaussNewtonOptimizer optimizer = new GaussNewtonOptimizer((iteration, previous, current) -> {
+      return subSystem.value() < 0.00000001;
+    }) {
+
+      @Override
+      protected double[] computeResiduals(double[] objectiveValue) {
+        TDoubleList residual = subSystem.calcResidual();
+        return residual.toArray();
+      }
+
+    };
+    double[] wieght = new double[subSystem.cSize()];
+    Arrays.fill(wieght, 1);
+    optimizer.optimize(
+            new MaxEval(10000),
+            new MaxIter(10000),
+            new InitialGuess(subSystem.getParams().toArray()),
+            new Target(new double[subSystem.cSize()]),
+            new Weight(wieght),
+            new ModelFunctionJacobian(point -> {
+              subSystem.setParams(point);
+              return subSystem.makeJacobi().getData();
+            }),
+            new ModelFunction(new MultivariateVectorFunction() {
+              @Override
+              public double[] value(double[] point) throws IllegalArgumentException {
+                subSystem.setParams(point);
+                return subSystem.getValues().toArray();
+              }
+            })
+
+    );
+  }
+
   private void solve(ActionEvent e) {
 
 //    UnconstrainedLeastSquares opt = FactoryOptimization.leastSquaresTrustRegion(100, RegionStepType.DOG_LEG_FTF, false);
diff --git a/src/cad/fx/CadContext.java b/src/cad/fx/CadContext.java
index d1b449fd..e944ee0f 100644
--- a/src/cad/fx/CadContext.java
+++ b/src/cad/fx/CadContext.java
@@ -112,7 +112,7 @@ public class CadContext {
     }
 
     Sketch sketch = selection.csgNode.getSketch(selection.poly);
-    Vector dir = sketch.owner.normal.scale(height);
+    Vector dir = sketch.owner.normal.multi(height);
     for (List<Vector> polygon : sketch.polygons) {
       if (polygon.isEmpty()) {
         continue;
diff --git a/src/cad/fx/Utils3D.java b/src/cad/fx/Utils3D.java
index bde9d94e..c2093ff9 100644
--- a/src/cad/fx/Utils3D.java
+++ b/src/cad/fx/Utils3D.java
@@ -149,7 +149,7 @@ public class Utils3D {
 
   public static List<Polygon> createCube(double width) {
     Polygon square = createSquare(width);
-    return Polygon.extrude(square, square.normal.scale(width)); 
+    return Polygon.extrude(square, square.normal.multi(width));
   }
 
   public static Polygon createSquare(double width) {
diff --git a/src/cad/gcs/Constraint.java b/src/cad/gcs/Constraint.java
index b712d1cb..46a93aeb 100644
--- a/src/cad/gcs/Constraint.java
+++ b/src/cad/gcs/Constraint.java
@@ -1,11 +1,11 @@
 package cad.gcs;
 
-import java.util.List;
-
 public interface Constraint extends System {
 
   double error();
 
+  void step(double alpha);
+
   void set(double[] input);
 
 }
diff --git a/src/cad/gcs/GradientDescent.java b/src/cad/gcs/GradientDescent.java
new file mode 100644
index 00000000..16d41850
--- /dev/null
+++ b/src/cad/gcs/GradientDescent.java
@@ -0,0 +1,56 @@
+package cad.gcs;
+
+import cad.gcs.constr.Perpendicular;
+import org.apache.commons.math3.linear.ArrayRealVector;
+import org.apache.commons.math3.linear.RealVector;
+
+public class GradientDescent {
+  
+  static double EPS = 0.0000001;
+  
+  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;
+    }
+  }
+
+  private static double value(Constraint constr) {
+    double err = constr.error();
+    return err * err;
+  }
+}
diff --git a/src/cad/gcs/GradientDescent2.java b/src/cad/gcs/GradientDescent2.java
new file mode 100644
index 00000000..1f62fdaa
--- /dev/null
+++ b/src/cad/gcs/GradientDescent2.java
@@ -0,0 +1,54 @@
+package cad.gcs;
+
+import cad.gcs.constr.Constraint2;
+import cad.gcs.constr.Perpendicular2;
+import cad.math.Vector;
+
+import java.lang.*;
+import java.util.List;
+
+public class GradientDescent2 {
+
+  private static final double DBL_EPSILON = Double.MIN_VALUE;
+
+  static double EPS = 0.0000000001;
+  
+  public static void solve(Constraint2 constr) {
+
+
+    double last = value(constr);
+
+    double alpha = .01;
+
+    List<Vector> params = constr.params();
+    for (int i = 0; i < 100000; i++) {
+
+
+      List<Vector> grad = constr.gradient();
+      for (int j = 0; j < grad.size(); j++) {
+        Vector gr = grad.get(j);
+        Vector param = params.get(j);
+        Vector step = gr.normalize().multi(alpha);
+        param._plus(step);
+      }
+      double err = value(constr);
+
+      java.lang.System.out.println(constr.debug() + "===" + err + "====>" + alpha );
+      if (err < last) {
+
+      } else {
+        if (alpha < EPS) {
+          return;
+        } else {
+          alpha /= 3;
+        }
+      }
+      last = err;
+    }
+  }
+
+  private static double value(Constraint2 constr) {
+    double err = constr.error();
+    return Math.abs(err);
+  }
+}
diff --git a/src/cad/gcs/GradientDescent3.java b/src/cad/gcs/GradientDescent3.java
new file mode 100644
index 00000000..bc33a5c6
--- /dev/null
+++ b/src/cad/gcs/GradientDescent3.java
@@ -0,0 +1,78 @@
+package cad.gcs;
+
+import cad.gcs.constr.Constraint2;
+import cad.gcs.constr.Perpendicular2;
+import cad.math.Vector;
+import gnu.trove.list.TDoubleList;
+import gnu.trove.list.array.TDoubleArrayList;
+import gnu.trove.map.hash.TObjectDoubleHashMap;
+
+import java.lang.*;
+import java.util.HashMap;
+import java.util.List;
+
+public class GradientDescent3 {
+
+  private static final double DBL_EPSILON = Double.MIN_VALUE;
+
+  static double EPS = 0.0000001;
+
+  public static void solve(Constraint2... constrs) {
+
+    TObjectDoubleHashMap<Vector> alphas = new TObjectDoubleHashMap<>();
+    double[] values = new double[constrs.length];
+    double[] calphas = new double[constrs.length];
+
+    for (int k = 0; k < constrs.length; k++) {
+      Constraint2 constr = constrs[k];
+      for (Vector p : constr.params()) {
+        alphas.put(p, 1);
+      }
+      values[k] = (value(constr));
+    }
+
+
+    for (int i = 0; i < 100000; i++) {
+      for (int k = 0; k < constrs.length; k++) {
+        Constraint2 constr = constrs[k];
+        List<Vector> params = constr.params();
+        double calpha = calphas[k];
+        List<Vector> grad = constr.gradient();
+        for (int j = 0; j < grad.size(); j++) {
+          Vector gr = grad.get(j);
+          Vector param = params.get(j);
+          double alpha = alphas.get(param);
+          Vector step = gr.normalize().multi(alpha);
+          param._plus(step);
+        }
+        double err = value(constr);
+        double last = values[k];
+//        java.lang.System.out.println(constr.debug() + "===" + err + "====>" + alpha );
+        if (err < last) {
+        } else {
+          boolean divergence = true;
+          for (double a : calphas) {
+            if (a >= EPS) {
+              divergence = false;
+            }
+          }
+          if (divergence) {
+            return;
+          } else {
+            calpha /= 3;
+            calphas[k] = calpha;
+            for (Vector param : params) {
+              alphas.put(param, Math.min(alphas.get(param), calpha));
+            }
+          }
+        }
+        values[k] = err;
+      }
+    }
+  }
+
+  private static double value(Constraint2 constr) {
+    double err = constr.error();
+    return Math.abs(err);
+  }
+}
diff --git a/src/cad/gcs/LM.java b/src/cad/gcs/LM.java
new file mode 100644
index 00000000..42a5d7fe
--- /dev/null
+++ b/src/cad/gcs/LM.java
@@ -0,0 +1,256 @@
+// levenberg-marquardt in java 
+//
+// To use this, implement the functions in the LMfunc interface.
+//
+// This library uses simple matrix routines from the JAMA java matrix package,
+// which is in the public domain.  Reference:
+//    http://math.nist.gov/javanumerics/jama/
+// (JAMA has a matrix object class.  An earlier library JNL, which is no longer
+// available, represented matrices as low-level arrays.  Several years 
+// ago the performance of JNL matrix code was better than that of JAMA,
+// though improvements in java compilers may have fixed this by now.)
+//
+// One further recommendation would be to use an inverse based
+// on Choleski decomposition, which is easy to implement and
+// suitable for the symmetric inverse required here.  There is a choleski
+// routine at idiom.com/~zilla.
+//
+// If you make an improved version, please consider adding your
+// name to it ("modified by ...") and send it back to me
+// (and put it on the web).
+//
+// ----------------------------------------------------------------
+// 
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Library General Public
+// License as published by the Free Software Foundation; either
+// version 2 of the License, or (at your option) any later version.
+// 
+// This library is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// Library General Public License for more details.
+// 
+// You should have received a copy of the GNU Library General Public
+// License along with this library; if not, write to the
+// Free Software Foundation, Inc., 59 Temple Place - Suite 330,
+// Boston, MA  02111-1307, USA.
+//
+// initial author contact info:  
+// jplewis  www.idiom.com/~zilla  zilla # computer.org,   #=at
+//
+// Improvements by:
+// dscherba  www.ncsa.uiuc.edu/~dscherba  
+// Jonathan Jackson   j.jackson # ucl.ac.uk
+
+
+package cad.gcs;
+
+// see comment above
+
+import Jama.*;
+
+/**
+ * Levenberg-Marquardt, implemented from the general description
+ * in Numerical Recipes (NR), then tweaked slightly to mostly
+ * match the results of their code.
+ * Use for nonlinear least squares assuming Gaussian errors.
+ * <p>
+ * TODO this holds some parameters fixed by simply not updating them.
+ * this may be ok if the number if fixed parameters is small,
+ * but if the number of varying parameters is larger it would
+ * be more efficient to make a smaller hessian involving only
+ * the variables.
+ * <p>
+ * The NR code assumes a statistical context, e.g. returns
+ * covariance of parameter errors; we do not do this.
+ */
+public final class LM {
+
+  /**
+   * calculate the current sum-squared-error
+   * (Chi-squared is the distribution of squared Gaussian errors,
+   * thus the name)
+   */
+  static double chiSquared(double[] a, double[] y, double[] s,
+                           LMfunc f) {
+    int npts = y.length;
+    double sum = 0.;
+
+    double[] val = f.val(a);
+    for (int i = 0; i < npts; i++) {
+      double d = y[i] - val[i];
+      d = d / s[i];
+      sum = sum + (d * d);
+    }
+
+    return sum;
+  } //chiSquared
+
+
+  /**
+   * Minimize E = sum {(y[k] - f(x[k],a)) / s[k]}^2
+   * The individual errors are optionally scaled by s[k].
+   * Note that LMfunc implements the value and gradient of f(x,a),
+   * NOT the value and gradient of E with respect to a!
+   *
+   * @param y           corresponding array of values
+   * @param a           the parameters/state of the model
+   * @param vary        false to indicate the corresponding a[k] is to be held fixed
+   * @param s           sigma^2 for point i
+   * @param lambda      blend between steepest descent (lambda high) and
+   *                    jump to bottom of quadratic (lambda zero).
+   *                    Start with 0.001.
+   * @param termepsilon termination accuracy (0.01)
+   * @param maxiter     stop and return after this many iterations if not done
+   * @param verbose     set to zero (no prints), 1, 2
+   * @return the new lambda for future iterations.
+   * Can use this and maxiter to interleave the LM descent with some other
+   * task, setting maxiter to something small.
+   */
+  public static double solve(double[] a, double[] y, double[] s,
+                             boolean[] vary, LMfunc f,
+                             double lambda, double termepsilon, int maxiter,
+                             int verbose)
+          throws Exception {
+    int npts = y.length;
+    int nparm = a.length;
+    assert s.length == npts;
+    if (verbose > 0) {
+      out().print(" a[" + a.length + "]");
+      out().println(" y[" + y.length + "]");
+    }
+
+    double e0 = chiSquared(a, y, s, f);
+    //double lambda = 0.001;
+    boolean done = false;
+
+    // g = gradient, H = hessian, d = step to minimum
+    // H d = -g, solve for d
+    double[][] H = new double[nparm][nparm];
+    double[] g = new double[nparm];
+    //double[] d = new double[nparm];
+
+    double[] oos2 = new double[s.length];
+    for (int i = 0; i < npts; i++) {
+      oos2[i] = 1. / (s[i] * s[i]);
+    }
+
+    int iter = 0;
+    int term = 0;  // termination count test
+
+    do {
+      ++iter;
+
+      // hessian approximation
+      for (int r = 0; r < nparm; r++) {
+        for (int c = 0; c < nparm; c++) {
+          for (int i = 0; i < npts; i++) {
+            if (i == 0) {
+              H[r][c] = 0.;
+            }
+            double[] grad = f.grad(a);
+            H[r][c] += (oos2[i] * grad[r] * grad[c]);
+          }  //npts
+        } //c
+      } //r
+
+      // boost diagonal towards gradient descent
+      for (int r = 0; r < nparm; r++) {
+        H[r][r] *= (1. + lambda);
+      }
+
+      // gradient
+      for (int r = 0; r < nparm; r++) {
+        for (int i = 0; i < npts; i++) {
+          if (i == 0) {
+            g[r] = 0.;
+          }
+          double[] grad = f.grad(a);
+          double[] val = f.val(a);
+          g[r] += (oos2[i] * (y[i] - val[i]) * grad[r]);
+        }
+      } //npts
+
+      // scale (for consistency with NR, not necessary)
+      if (false) {
+        for (int r = 0; r < nparm; r++) {
+          g[r] = -0.5 * g[r];
+          for (int c = 0; c < nparm; c++) {
+            H[r][c] *= 0.5;
+          }
+        }
+      }
+
+      // solve H d = -g, evaluate error at new location
+      //double[] d = DoubleMatrix.solve(H, g);
+      double[] d = (new Matrix(H)).lu().solve(new Matrix(g, nparm)).getRowPackedCopy();
+      //double[] na = DoubleVector.add(a, d);
+      double[] na = (new Matrix(a, nparm)).plus(new Matrix(d, nparm)).getRowPackedCopy();
+      double e1 = chiSquared(na, y, s, f);
+
+      if (verbose > 0) {
+        out().println("\n\niteration " + iter + " lambda = " + lambda);
+        out().print("a = ");
+        (new Matrix(a, nparm)).print(10, 2);
+        if (verbose > 1) {
+          out().print("H = ");
+          (new Matrix(H)).print(10, 2);
+          out().print("g = ");
+          (new Matrix(g, nparm)).print(10, 2);
+          out().print("d = ");
+          (new Matrix(d, nparm)).print(10, 2);
+        }
+        out().print("e0 = " + e0 + ": ");
+        out().print("moved from ");
+        (new Matrix(a, nparm)).print(10, 2);
+        out().print("e1 = " + e1 + ": ");
+        if (e1 < e0) {
+          out().print("to ");
+          (new Matrix(na, nparm)).print(10, 2);
+        } else {
+          out().println("move rejected");
+        }
+      }
+
+      // termination test (slightly different than NR)
+      if (Math.abs(e1 - e0) > termepsilon) {
+        term = 0;
+      } else {
+        term++;
+        if (term == 4) {
+          out().println("terminating after " + iter + " iterations");
+          done = true;
+        }
+      }
+      if (iter >= maxiter) {
+        done = true;
+      }
+
+      // in the C++ version, found that changing this to e1 >= e0
+      // was not a good idea.  See comment there.
+      //
+      if (e1 > e0 || Double.isNaN(e1)) { // new location worse than before
+        lambda *= 10.;
+      } else {    // new location better, accept new parameters
+        lambda *= 0.1;
+        e0 = e1;
+        // simply assigning a = na will not get results copied back to caller
+        for (int i = 0; i < nparm; i++) {
+          if (vary[i]) {
+            a[i] = na[i];
+          }
+        }
+      }
+
+    } while (!done);
+
+    return lambda;
+  } //solve
+
+  private static java.io.PrintStream out() {
+    return java.lang.System.out; 
+  }
+
+  //----------------------------------------------------------------
+} //LM
diff --git a/src/cad/gcs/LMfunc.java b/src/cad/gcs/LMfunc.java
new file mode 100644
index 00000000..aee8991c
--- /dev/null
+++ b/src/cad/gcs/LMfunc.java
@@ -0,0 +1,26 @@
+// LMfunc.java
+
+package cad.gcs;
+
+/**
+ * Caller implement this interface to specify the
+ * function to be minimized and its gradient.
+ * 
+ * Optionally return an initial guess and some test data,
+ * though the LM.java only uses this in its optional main() test program.
+ * Return null if these are not needed.
+ */
+public interface LMfunc
+{
+
+  /**
+   * x is a single point, but domain may be mulidimensional
+   */
+  double[] val(double[] a);
+
+  /**
+   * return the kth component of the gradient df(x,a)/da_k
+   */
+  double[] grad(double[] a);
+
+} //LMfunc
diff --git a/src/cad/gcs/Param.java b/src/cad/gcs/Param.java
index 764cb308..35ec7ed9 100644
--- a/src/cad/gcs/Param.java
+++ b/src/cad/gcs/Param.java
@@ -1,12 +1,18 @@
 package cad.gcs;
 
-public abstract class Param {
+public class Param {
 
-  public final Constraint constraint;
+  public double value;
 
-  protected Param(Constraint constraint) {
-    this.constraint = constraint;
+  public Param(double value) {
+    this.value = value;
   }
 
-  public abstract double value();
+  public double get() {
+    return value;
+  }
+  
+  public double set(double value) {
+    return this.value = value;
+  }
 }
diff --git a/src/cad/gcs/Solver.java b/src/cad/gcs/Solver.java
index 7423f4fe..704fa5aa 100644
--- a/src/cad/gcs/Solver.java
+++ b/src/cad/gcs/Solver.java
@@ -2,15 +2,26 @@ package cad.gcs;
 
 import gnu.trove.list.TDoubleList;
 import gnu.trove.list.array.TDoubleArrayList;
+import org.apache.commons.math3.exception.ConvergenceException;
 import org.apache.commons.math3.exception.DimensionMismatchException;
+import org.apache.commons.math3.exception.MathInternalError;
+import org.apache.commons.math3.exception.util.LocalizedFormats;
 import org.apache.commons.math3.linear.Array2DRowRealMatrix;
 import org.apache.commons.math3.linear.ArrayRealVector;
+import org.apache.commons.math3.linear.BlockRealMatrix;
+import org.apache.commons.math3.linear.DecompositionSolver;
+import org.apache.commons.math3.linear.LUDecomposition;
+import org.apache.commons.math3.linear.QRDecomposition;
 import org.apache.commons.math3.linear.RealMatrix;
+import org.apache.commons.math3.linear.SingularMatrixException;
 
 import java.util.List;
 
 public class Solver {
 
+  public static final boolean useLU = true;
+  private static final double DBL_EPSILON = Double.MIN_VALUE;
+
   enum SolveStatus {
     Success,   // Found a solution zeroing the error function
     Converged, // Found a solution minimizing the error function
@@ -164,12 +175,108 @@ public class Solver {
 
   private static RealMatrix lu(RealMatrix jx, RealMatrix fx) {
 
-    return new cad.gcs.LUDecomposition(jx).solve(fx);
-    
+    return new org.apache.commons.math3.linear.LUDecomposition(new Array2DRowRealMatrix(makeSquare(jx.getData()))).getSolver().solve(fx);
+
 //    DoubleMatrix2D solve = new LUDecomposition(new DenseDoubleMatrix2D(jx.getData()))
 //        .solve(new DenseDoubleMatrix2D(fx.getData()));
   }
 
+  public static double[][] makeSquare(double[][] m) {
+    if (m.length > m[0].length) {
+      for (int r = 0; r < m.length; r++) {
+        double[] row = m[r];
+        m[r] = new double[m.length];
+        java.lang.System.arraycopy(row, 0, m[r], 0, row.length);
+      }
+    } else {
+      double[][] _m = new double[m[0].length][];
+      for (int r = 0; r < m.length; r++) {
+        _m[r] = m[r];
+      }
+      for (int r = m.length; r < _m.length; r++) {
+        _m[r] = new double[m[0].length];
+      }
+      m = _m;
+    }
+    return m;
+  }
+
+
+  public static void optimize(SubSystem subSystem) {
+
+    final int cSize = subSystem.cSize();
+
+    final double[] currentPoint = subSystem.getParams().toArray();
+    final int pSize = currentPoint.length;
+
+    // iterate until convergence is reached
+    double[] current = null;
+    int iter = 0;
+    for (boolean converged = false; !converged; ) {
+      ++iter;
+
+      // evaluate the objective function and its jacobian
+      // Value of the objective function at "currentPoint".
+      final double[] currentResiduals = subSystem.calcResidual().toArray();
+      final RealMatrix jacobian = new Array2DRowRealMatrix(makeSquare(subSystem.makeJacobi().getData()));
+
+      // build the linear problem
+      final double[] b = new double[pSize];
+      final double[][] a = new double[pSize][pSize];
+      for (int i = 0; i < cSize; ++i) {
+
+        final double[] grad = jacobian.getRow(i);
+        final double weight = 1;
+        final double residual = currentResiduals[i];
+
+        // compute the normal equation
+        final double wr = weight * residual;
+        for (int j = 0; j < pSize; ++j) {
+          b[j] += wr * grad[j];
+        }
+
+        // build the contribution matrix for measurement i
+        for (int k = 0; k < pSize; ++k) {
+          double[] ak = a[k];
+          double wgk = weight * grad[k];
+          for (int l = 0; l < pSize; ++l) {
+            ak[l] += wgk * grad[l];
+          }
+        }
+      }
+
+      try {
+        // solve the linearized least squares problem
+        RealMatrix mA = new BlockRealMatrix(a);
+        DecompositionSolver solver = useLU ?
+            new org.apache.commons.math3.linear.LUDecomposition(jacobian).getSolver() :
+            new QRDecomposition(jacobian).getSolver();
+        final double[] dX = solver.solve(new ArrayRealVector(currentPoint).mapMultiply(-1)).toArray();
+        // update the estimated parameters
+        for (int i = 0; i < pSize; ++i) {
+          currentPoint[i] += dX[i];
+        }
+        subSystem.setParams(currentPoint);
+      } catch (SingularMatrixException e) {
+        throw new ConvergenceException(LocalizedFormats.UNABLE_TO_SOLVE_SINGULAR_PROBLEM);
+      }
+
+      // Check convergence.
+      if (iter != 0) {
+//        converged = checker.converged(iter, previous, current);
+//        if (converged) {
+//          setCost(computeCost(currentResiduals));
+//          return current;
+//        }
+        if (subSystem.value() < 0.0001) {
+          return;
+        }
+      }
+    }
+    // Must never happen.
+    throw new MathInternalError();
+  }
+
   public RealMatrix solve(RealMatrix b, int[] pivot, double[][] lu) {
 
     final int m = pivot.length;
@@ -218,7 +325,334 @@ public class Solver {
 
     return new Array2DRowRealMatrix(bp, false);
   }
+
+
+  public static SolveStatus solve_LM(SubSystem subsys) {
+    int xsize = subsys.pSize();
+    int csize = subsys.cSize();
+
+    if (xsize == 0) {
+      return SolveStatus.Success;
+    }
+
+    RealMatrix e = mtrx(csize), e_new = mtrx(csize); // vector of all function errors (every constraint is one function)
+    RealMatrix J = mtrx(csize, xsize);        // Jacobi of the subsystem
+    RealMatrix A = mtrx(xsize, xsize);
+    RealMatrix x = mtrx(xsize), h = mtrx(xsize), x_new = mtrx(xsize), g = mtrx(xsize);
+    double[] diag_A;
+
+//    subsys.redirectParams();
+
+    subsys.fillParams(x);
+    subsys.calcResidual(e);
+    e = e.scalarMultiply(-1);
+
+    int maxIterNumber = MaxIterations * xsize;
+    double divergingLim = 1e6 * squaredNorm(e) + 1e12;
+
+    double eps = 1e-10, eps1 = 1e-80;
+    double tau = 1e-3;
+    double nu = 2, mu = 0;
+    int iter = 0, stop = 0;
+    for (iter = 0; iter < maxIterNumber && stop == 0; ++iter) {
+
+      // check error
+      double err = squaredNorm(e);
+      if (err <= eps) { // error is small, Success
+        stop = 1;
+        break;
+      } else if (err > divergingLim || err != err) { // check for diverging and NaN
+        stop = 6;
+        break;
+      }
+
+      // J^T J, J^T e
+      subsys.calcJacobi(J);
+      ;
+
+      A = J.transpose().multiply(J);
+      g = J.transpose().multiply(e);
+
+      // Compute ||J^T e||_inf
+      double g_inf = infinityNorm(g);
+      diag_A = diagonal(A); // save diagonal entries so that augmentation can be later canceled
+
+      // check for convergence
+      if (g_inf <= eps1) {
+        stop = 2;
+        break;
+      }
+
+      // compute initial damping factor
+      if (iter == 0) {
+        mu = tau *  new ArrayRealVector(diag_A).getLInfNorm() ;
+      }
+
+      // determine increment using adaptive damping
+      int k = 0;
+      while (k < 50) {
+        // augment normal equations A = A+uI
+        for (int i = 0; i < xsize; ++i) {
+          A.addToEntry(i, i, mu);
+        }
+
+        //solve augmented functions A*h=-g
+
+        try {
+          h = new LUDecomposition(A).getSolver().solve(g);
+        } catch (Exception ssse) {
+          java.lang.System.out.println( "");
+          return SolveStatus.Success;
+        }
+        
+        ;
+        double rel_error = (A.multiply(h).subtract(g)).getFrobeniusNorm() / g.getFrobeniusNorm();
+
+        // check if solving works
+        if (rel_error < 1e-5) {
+
+          // restrict h according to maxStep
+//          double scale = subsys.maxStep(h);
+//          if (scale < 1.) {
+//            h = h.scalarMultiply(scale);
+//          }
+
+          // compute par's new estimate and ||d_par||^2
+          x_new = x.add(h);
+          double h_norm = squaredNorm(h);
+
+          if (h_norm <= eps1 * eps1 * x.getFrobeniusNorm()) { // relative change in p is small, stop
+            stop = 3;
+            break;
+          } else if (h_norm >= (x.getFrobeniusNorm() + eps1) / (DBL_EPSILON * DBL_EPSILON)) { // almost singular
+            stop = 4;
+            break;
+          }
+
+          subsys.setParams(x_new); 
+          subsys.calcResidual(e_new);
+          e_new = e_new.scalarMultiply(-1);
+
+          double dF = squaredNorm(e) - squaredNorm(e_new);
+          double dL = dot(h, (h.scalarMultiply(mu).add(g)));
+
+          if (dF > 0. && dL > 0.) { // reduction in error, increment is accepted
+            double tmp = 2 * dF / dL - 1.;
+            mu *= Math.max(1. / 3., 1. - tmp * tmp * tmp);
+            nu = 2;
+
+            // update par's estimate
+            x = x_new.copy();
+            e = e_new.copy();
+            break;
+          }
+        }
+
+        // if this point is reached, either the linear system could not be solved or
+        // the error did not reduce; in any case, the increment must be rejected
+
+        mu *= nu;
+        nu *= 2.0;
+        for (int i = 0; i < xsize; ++i) // restore diagonal J^T J entries
+        {
+          A.setEntry(i, i, diag_A[i]);
+        }
+
+        k++;
+      }
+      if (k > 50) {
+        stop = 7;
+        break;
+      }
+    }
+
+    if (iter >= maxIterNumber) {
+      stop = 5;
+    }
+
+//    subsys.revertParams();
+
+    return (stop == 1) ? SolveStatus.Success : SolveStatus.Failed;
+  }
+
+  private static void identity(RealMatrix m) {
+    for (int i = 0; i < m.getColumnDimension() && i < m.getRowDimension(); i++) {
+      m.setEntry(i, i, 1.0);
+    }
+  }
+
+
+  static double lineSearch(SubSystem subsys, RealMatrix xdir) {
+    double f1, f2, f3, alpha1, alpha2, alpha3, alphaStar;
+
+    double alphaMax = 1;//subsys.maxStep(xdir);
+
+    int pSize = subsys.pSize();
+    RealMatrix x0 = mtrx(pSize), x = mtrx(pSize);
+
+    //Save initial values
+    subsys.fillParams(x0);
+
+    //Start at the initial position alpha1 = 0
+    alpha1 = 0.;
+    f1 = subsys.error();
+
+    //Take a step of alpha2 = 1
+    alpha2 = 1.;
+    x = x0.add(xdir.scalarMultiply(alpha2));
+    subsys.setParams(x);
+    f2 = subsys.error();
+
+    //Take a step of alpha3 = 2*alpha2
+    alpha3 = alpha2 * 2;
+    x = x0.add(xdir.scalarMultiply(alpha3));
+    subsys .setParams(x);
+    f3 = subsys . error();
+
+    //Now reduce or lengthen alpha2 and alpha3 until the minimum is
+    //Bracketed by the triplet f1>f2<f3
+    while (f2 > f1 || f2 > f3) {
+      if (f2 > f1) {
+        //If f2 is greater than f1 then we shorten alpha2 and alpha3 closer to f1
+        //Effectively both are shortened by a factor of two.
+        alpha3 = alpha2;
+        f3 = f2;
+        alpha2 = alpha2 / 2;
+        x = x0.add( xdir.scalarMultiply(alpha2 ));
+        subsys . setParams(x);
+        f2 = subsys . error();
+      } else if (f2 > f3) {
+        if (alpha3 >= alphaMax) {
+          break;
+        }
+        //If f2 is greater than f3 then we increase alpha2 and alpha3 away from f1
+        //Effectively both are lengthened by a factor of two.
+        alpha2 = alpha3;
+        f2 = f3;
+        alpha3 = alpha3 * 2;
+        x = x0.add( xdir.scalarMultiply(alpha3));
+        subsys . setParams(x);
+        f3 = subsys . error();
+      }
+    }
+    //Get the alpha for the minimum f of the quadratic approximation
+    alphaStar = alpha2 + ((alpha2 - alpha1) * (f1 - f3)) / (3 * (f1 - 2 * f2 + f3));
+
+    //Guarantee that the new alphaStar is within the bracket
+    if (alphaStar >= alpha3 || alphaStar <= alpha1) {
+      alphaStar = alpha2;
+    }
+
+    if (alphaStar > alphaMax) {
+      alphaStar = alphaMax;
+    }
+
+    if (alphaStar != alphaStar) {
+      alphaStar = 0.;
+    }
+
+    //Take a final step to alphaStar
+    x = x0 .add( xdir.scalarMultiply( alphaStar ) );
+    subsys . setParams(x);
+
+    return alphaStar;
+  }
   
+  public static SolveStatus solve_BFGS(SubSystem subsys, boolean isFine) {
+    int xsize = subsys.pSize();
+    if (xsize == 0) {
+      return SolveStatus.Success;
+    }
+
+
+    RealMatrix D = new Array2DRowRealMatrix(xsize, xsize);
+    identity(D);
+//    
+    RealMatrix x = new Array2DRowRealMatrix(xsize, 1);
+    RealMatrix xdir = new Array2DRowRealMatrix(xsize, 1);
+    RealMatrix grad = new Array2DRowRealMatrix(xsize, 1);
+    RealMatrix h = new Array2DRowRealMatrix(xsize, 1);
+    RealMatrix y = new Array2DRowRealMatrix(xsize, 1);
+    RealMatrix Dy = new Array2DRowRealMatrix(xsize, 1);
+
+    // Initial unknowns vector and initial gradient vector
+    subsys.fillParams(x);
+    subsys.calcGrad(grad);
+
+    // Initial search direction oposed to gradient (steepest-descent)
+    xdir = grad.scalarMultiply(-1);
+    lineSearch(subsys, xdir);
+    double err = subsys.error();
+
+    h = new Array2DRowRealMatrix(x.getData());
+    subsys.fillParams(x);
+    h = x.subtract(h); // = x - xold
+
+    double convergence = isFine ? XconvergenceFine : XconvergenceRough;
+    int maxIterNumber = MaxIterations * xsize;
+    double divergingLim = 1e6 * err + 1e12;
+
+    for (int iter = 1; iter < maxIterNumber; iter++) {
+
+      if (h.getFrobeniusNorm() <= convergence || err <= smallF) {
+        break;
+      }
+      if (err > divergingLim || err != err) // check for diverging and NaN
+      {
+        break;
+      }
+
+      y = new Array2DRowRealMatrix(grad.getData());
+      subsys.calcGrad(grad);
+      y = grad.subtract(y); // = grad - gradold
+
+      double hty = dotProduct(h, y);
+      //make sure that hty is never 0
+      if (hty == 0) {
+        hty = .0000000001;
+      }
+
+      Dy = D.multiply(y);
+
+      double ytDy = dotProduct(y, Dy);
+
+      //Now calculate the BFGS update on D
+      D = D.add(h.scalarMultiply((1. + ytDy / hty) / hty).multiply(h.transpose()));
+      D = D.subtract((
+                      h.multiply(Dy.transpose())
+                              .add(Dy.multiply(h.transpose()))
+              ).scalarMultiply(1. / hty)
+      );
+
+      xdir = D.scalarMultiply(-1).multiply(grad);
+      lineSearch(subsys, xdir);
+      err = subsys.error();
+
+      h = new Array2DRowRealMatrix(x.getData());
+      subsys.fillParams(x);
+      h = x.subtract(h); // = x - xold
+    }
+
+//    subsys.revertParams();
+
+    if (err <= smallF) {
+      return SolveStatus.Success;
+    }
+    if (h.getFrobeniusNorm() <= convergence) {
+      return SolveStatus.Converged;
+    }
+    return SolveStatus.Failed;
+  }
+
+
+  private static double[] diagonal(RealMatrix a) {
+    int s = Math.min(a.getColumnDimension(), a.getRowDimension());
+    double[] d = new double[s];
+    for (int i = 0; i < s; i++) {
+      d[i] = a.getEntry(i, i);
+    }
+    return d;
+  }
 
   private static double dot(RealMatrix m1, RealMatrix m2) {
     return new ArrayRealVector(m1.getData()[0]).dotProduct(new ArrayRealVector(m2.getData()[0]));
@@ -271,6 +705,34 @@ public class Solver {
       x.setColumn(0, params.toArray());
     }
 
+
+    public TDoubleList getParams() {
+      int i = 0;
+      TDoubleList params = new TDoubleArrayList();
+      for (Constraint c : constraints) {
+        params.add(c.params());
+      }
+      return params;
+    }
+
+    public TDoubleList getValues2() {
+      TDoubleList params = new TDoubleArrayList();
+      for (Constraint c : constraints) {
+        double err = c.error();
+        params.add(err);
+      }
+      return params;
+    }
+
+
+    public TDoubleList getValues() {
+      TDoubleList params = new TDoubleArrayList();
+      for (Constraint c : constraints) {
+        params.add(c.error());
+      }
+      return params;
+    }
+    
     public double calcResidual(RealMatrix r) {
       double err = 0.;
       int i = 0;
@@ -283,6 +745,30 @@ public class Solver {
       return err;
     }
 
+    public TDoubleList calcResidual() {
+      TDoubleList r = new TDoubleArrayList();
+      double err = 0.;
+      int i = 0;
+      for (Constraint c : constraints) {
+        double v = c.error();
+        r.add(v);
+        err += v * v;
+      }
+      err *= 0.5;
+      return r;
+    }
+
+    public double value() {
+      double err = 0.;
+      for (Constraint c : constraints) {
+        double v = c.error();
+        err += v * v;
+      }
+      err *= 0.5;
+      return err;
+    }
+
+
     public void calcJacobi(RealMatrix jacobi) {
 //      jacobi.setZero(csize, params.size());
       for (int j=0; j < pSize(); j++) {
@@ -300,6 +786,25 @@ public class Solver {
       }
     }
 
+    public RealMatrix makeJacobi() {
+      RealMatrix jacobi = new Array2DRowRealMatrix(cSize(), pSize());
+      for (int j=0; j < pSize(); j++) {
+        for (int i=0; i < constraints.size(); i++) {
+          jacobi.setEntry(i, j, 0);
+        }
+      }
+      for (int i=0; i < constraints.size(); i++) {
+        Constraint c = constraints.get(i);
+        double[] grad = new double[c.params().length];
+        c.gradient(grad);
+        for (int j=0; j < grad.length; j++) {
+          jacobi.setEntry(i,j, grad[j]);
+        }
+      }
+      return jacobi;
+    }
+
+
     public void setParams(RealMatrix params) {
       int off = 0;
       double[] arr = params.getColumn(0);
@@ -310,5 +815,40 @@ public class Solver {
         c.set(cp);
       }
     }
+    
+    public void setParams(double[] arr) {
+      int off = 0;
+      for (Constraint c : constraints) {
+        int l = c.params().length;
+        double[] cp = new double[l];
+        java.lang.System.arraycopy(arr, off, cp, 0, l);
+        c.set(cp);
+      }
+    }
+
+    public double error() {
+      return value();
+    }
+
+    public void calcGrad(RealMatrix out) {
+
+      int cc = 0;
+      for (Constraint c : constraints) {
+        double[] grad = new double[c.params().length];
+        c.gradient(grad);
+        for (double aGrad : grad) {
+          out.setEntry(cc++, 0, aGrad);
+        }
+      }      
+    }
   }
+
+  private static double dotProduct(RealMatrix m1, RealMatrix m2) {
+    return new ArrayRealVector(m1.getData()[0]).dotProduct(new ArrayRealVector(m2.getData()[0]));
+  }
+
+
+  static double XconvergenceRough = 1e-8;
+  static double XconvergenceFine = 1e-10;
+  static double smallF = 1e-20;
 }
diff --git a/src/cad/gcs/System.java b/src/cad/gcs/System.java
index cd9ba33e..e0c756af 100644
--- a/src/cad/gcs/System.java
+++ b/src/cad/gcs/System.java
@@ -5,4 +5,6 @@ public interface System {
   double[] params();
   
   void gradient(double[] out);
+  
+  int pSize();
 }
diff --git a/src/cad/gcs/constr/AbstractConstraint.java b/src/cad/gcs/constr/AbstractConstraint.java
new file mode 100644
index 00000000..56bc80d9
--- /dev/null
+++ b/src/cad/gcs/constr/AbstractConstraint.java
@@ -0,0 +1,9 @@
+package cad.gcs.constr;
+
+import cad.gcs.Constraint;
+
+/**
+ * Created by verastov
+ */
+public abstract class AbstractConstraint implements Constraint {
+}
diff --git a/src/cad/gcs/constr/Constraint2.java b/src/cad/gcs/constr/Constraint2.java
new file mode 100644
index 00000000..22440f6b
--- /dev/null
+++ b/src/cad/gcs/constr/Constraint2.java
@@ -0,0 +1,15 @@
+package cad.gcs.constr;
+
+import cad.math.Vector;
+
+import java.util.List;
+
+public interface Constraint2 {
+  double error();
+
+  List<Vector> params();
+
+  List<Vector> gradient();
+
+  Object debug();
+}
diff --git a/src/cad/gcs/constr/PRef.java b/src/cad/gcs/constr/PRef.java
new file mode 100644
index 00000000..9c666c16
--- /dev/null
+++ b/src/cad/gcs/constr/PRef.java
@@ -0,0 +1,17 @@
+package cad.gcs.constr;
+
+import cad.gcs.Param;
+
+/**
+ * Created by verastov
+ */
+public class PRef {
+
+  private final Param x;
+  private final Param y;
+
+  public PRef(Param x, Param y) {
+    this.x = x;
+    this.y = y;
+  }
+}
diff --git a/src/cad/gcs/constr/Perpendicular.java b/src/cad/gcs/constr/Perpendicular.java
index edc69929..c8a2fd5f 100644
--- a/src/cad/gcs/constr/Perpendicular.java
+++ b/src/cad/gcs/constr/Perpendicular.java
@@ -4,8 +4,6 @@ import cad.gcs.Constraint;
 import cad.gcs.Param;
 import cad.math.Vector;
 
-import java.util.List;
-
 public class Perpendicular implements Constraint {
 
   public static final int l1p1x = 0;
@@ -16,66 +14,187 @@ public class Perpendicular implements Constraint {
   public static final int l2p1y = 5;
   public static final int l2p2x = 6;
   public static final int l2p2y = 7;
-  
-  private final double[] params = new double[8];
 
-  public Perpendicular(Vector a1, Vector a2, Vector b1, Vector b2) {
-    params[l1p1x] = a1.x;
-    params[l1p1y] = a1.y;
-    params[l1p2x] = b1.x;
-    params[l1p2y] = b1.y;
-    params[l2p1x] = a2.x;
-    params[l2p1y] = a2.y;
-    params[l2p2x] = b2.x;
-    params[l2p2y] = b2.y;
+  private final Param[] params = new Param[8];
+
+  public Perpendicular(
+    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 a1, Vector a2, Vector b1, Vector b2) {
-    a1.x = params[l1p1x];
-    a1.y = params[l1p1y];
-    b1.x = params[l1p2x];
-    b1.y = params[l1p2y];
-    a2.x = params[l2p1x];
-    a2.y = params[l2p1y];
-    b2.x = params[l2p2x];
-    b2.y = params[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();
   }
 
   @Override
   public double[] params() {
-    return 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[l1p1x] - params[l1p2x]);
-    double dy1 = (params[l1p1y] - params[l1p2y]);
-    double dx2 = (params[l2p1x] - params[l2p2x]);
-    double dy2 = (params[l2p1y] - params[l2p2y]);
+    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 off = dx1 * dx2 + dy1 * dy2;
-    return off;
+    return off * off;
+  }
+
+  //derivative of ((x-a1)*a2 + a3)^2
+  public double partDerivative1(double a1, double a2, double a3, double x) {
+    return 2*a2*(-a1*a2 + a2*x+a3);
+  }
+
+  //derivative of ((a1-x)*a2 + a3)^2
+  public double partDerivative2(double a1, double a2, double a3, double x) {
+    return -2*a2*(a1*a2 - a2*x+a3);
   }
 
   @Override
   public void gradient(double[] out) {
-    out[l1p1x] =  (params[l2p1x] - params[l2p2x]); // = dx2
-    out[l1p2x] = -(params[l2p1x] - params[l2p2x]); // = -dx2
-    out[l1p1y] =  (params[l2p1y] - params[l2p2y]); // = dy2
-    out[l1p2y] = -(params[l2p1y] - params[l2p2y]); // = -dy2
-    out[l2p1x] =  (params[l1p1x] - params[l1p2x]); // = dx1
-    out[l2p2x] = -(params[l1p1x] - params[l1p2x]); // = -dx1
-    out[l2p1y] =  (params[l1p1y] - params[l1p2y]); // = dy1
-    out[l2p2y] = -(params[l1p1y] - params[l1p2y]); // = -dy1
 
-//    for (int i = 0; i < out.length; i++) {
-//      out[i] *= err;
-//    }
+    double x1 = params[l1p1x].get();
+    double x2 = params[l1p1y].get();
+    double x3 = params[l1p2x].get();
+    double x4 = params[l1p2y].get();
+    double x5 = params[l2p1x].get();
+    double x6 = params[l2p1y].get();
+    double x7 = params[l2p2x].get();
+    double x8 = params[l2p2y].get();
+
+    double c1 = x3 - x1;
+    double c2 = x7 - x5;
+    double c3 = x4 - x2;
+    double c4 = x8 - x6;
+
+    //f(x) = ( (x3 - x1) * (x7 - x5) + (x4 - x2) * (x8 - x6) ) ^ 2
+
+    out[l1p1x] = partDerivative2(x3, c2, c3 * c4, x1);
+    out[l1p1y] = partDerivative2(x4, c4, c1 * c2, x2);
+
+    out[l1p2x] = partDerivative1(x1, c2, c3 * c4, x3);
+    out[l1p2y] = partDerivative1(x2, c4, c1 * c2, x4);
+
+    out[l2p1x] = partDerivative2(x7, c1, c3 * c4, x5);
+    out[l2p1y] = partDerivative2(x8, c3, c1 * c2, x6);
+
+    out[l2p2x] = partDerivative1(x5, c1, c3 * c4, x7);
+    out[l2p2y] = partDerivative1(x6, c3, c1 * c2, x8);
+  }
+
+  public void gradient2(double[] out) {
+
+    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);
+    
+    double k =  (da.dot(db) * 2);
+    
+    Vector g1 = p1.multi(db.x, db.y, db.z).multi(-k);
+    Vector g2 = p2.multi(db.x, db.y, db.z).multi(k);
+    Vector g3 = p3.multi(da.x, da.y, da.z).multi(-k);
+    Vector g4 = p4.multi(da.x, da.y, da.z).multi(k);
+    
+    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;
+  }
+
+
+  @Override
+  public void step(double alpha) {
+
+    System.out.println(angle());
+    
+    step(l1p1x, l1p1y, params[l2p1x].get() - params[l2p2x].get(), params[l2p1y].get() - params[l2p2y].get(), alpha);
+    step(l1p2x, l1p2y, -(params[l2p1x].get() - params[l2p2x].get()), -(params[l2p1y].get() - params[l2p2y].get()), alpha);
+
+    step(l2p1x, l2p1y, (params[l1p1x].get() - params[l1p2x].get()), (params[l1p1y].get() - params[l1p2y].get()), alpha);
+    step(l2p2x, l2p2y, -(params[l1p1x].get() - params[l1p2x].get()), -(params[l1p1y].get() - params[l1p2y].get()), alpha);
+  }
+
+  public double angle() {
+    double dx1 = (params[l1p2x].get() - params[l1p1x].get());
+    double dy1 = (params[l1p2y].get() - params[l1p1y].get());
+    double dx2 = (params[l2p2x].get() - params[l2p1x].get());
+    double dy2 = (params[l2p2y].get() - params[l2p1y].get());
+    //dot product shows how the lines off to be perpendicular
+    double xl = Math.sqrt(dx1 * dx1 + dx2 * dx2);
+    double yl = Math.sqrt(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) {
-    System.arraycopy(input, 0, params, 0, params.length);
+    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]);
   }
 
 }
diff --git a/src/cad/gcs/constr/Perpendicular2.java b/src/cad/gcs/constr/Perpendicular2.java
new file mode 100644
index 00000000..a6516569
--- /dev/null
+++ b/src/cad/gcs/constr/Perpendicular2.java
@@ -0,0 +1,74 @@
+package cad.gcs.constr;
+
+import cad.math.Vector;
+
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+
+public class Perpendicular2 implements Constraint2 {
+
+  public final Vector a1;
+  public final Vector a2;
+  public final Vector b1;
+  public final Vector b2;
+  private double target;
+
+  public Perpendicular2(Vector a1, Vector a2, Vector b1, Vector b2) {
+    this.a1 = a1;
+    this.a2 = a2;
+    this.b1 = b1;
+    this.b2 = b2;
+    this.target = target;
+  }
+
+  @Override
+  public double error() {
+    return da().dot(db());
+  }
+
+  @Override
+  public List<Vector> params() {
+    return Arrays.asList(a1, a2, b1, b2);
+  }
+
+  @Override
+  public List<Vector> gradient() {
+    List<Vector> grad = new ArrayList<>(4);
+//    Vector da = da();
+//    Vector db = db();
+//    double k = da().dot(db()) > 0 ? -1 : 1;
+////    double k = 1;
+//    grad.add(db.multi(- k));
+//    grad.add(db.multi(  k));
+//    grad.add(da.multi(- k));
+//    grad.add(da.multi(  k));
+//    return grad;
+    
+    double k =  (da().dot(db()) * 2);
+
+    Vector da = da();
+    Vector db = db();
+    grad.add(a1.multi(db.x, db.y, db.z).multi(-1));
+    grad.add(a2.multi(db.x, db.y, db.z));
+    
+    grad.add(b1.multi(da.x, da.y, da.z).multi(-1));
+    grad.add(b2.multi(da.x, da.y, da.z));
+    
+    return grad;
+    
+  }
+
+  private Vector db() {
+    return b2.minus(b1);
+  }
+
+  private Vector da() {
+    return a2.minus(a1);
+  }
+
+  @Override
+  public Object debug() {
+    return Math.acos(error() / (da().length() * db().length()) ) / Math.PI * 180;
+  }
+}
diff --git a/src/cad/gcs/constr/X.java b/src/cad/gcs/constr/X.java
new file mode 100644
index 00000000..5a069450
--- /dev/null
+++ b/src/cad/gcs/constr/X.java
@@ -0,0 +1,163 @@
+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 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;
+  }
+
+
+  @Override
+  public void step(double alpha) {
+
+    System.out.println(angle());
+    
+    step(l1p1x, l1p1y, params[l2p1x].get() - params[l2p2x].get(), params[l2p1y].get() - params[l2p2y].get(), alpha);
+    step(l1p2x, l1p2y, -(params[l2p1x].get() - params[l2p2x].get()), -(params[l2p1y].get() - params[l2p2y].get()), alpha);
+
+    step(l2p1x, l2p1y, (params[l1p1x].get() - params[l1p2x].get()), (params[l1p1y].get() - params[l1p2y].get()), alpha);
+    step(l2p2x, l2p2y, -(params[l1p1x].get() - params[l1p2x].get()), -(params[l1p1y].get() - params[l1p2y].get()), alpha);
+  }
+
+  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]);
+  }
+
+}
diff --git a/src/cad/gcs/constr/XY.java b/src/cad/gcs/constr/XY.java
new file mode 100644
index 00000000..94661a58
--- /dev/null
+++ b/src/cad/gcs/constr/XY.java
@@ -0,0 +1,45 @@
+package cad.gcs.constr;
+
+import cad.math.Vector;
+
+import java.util.Collections;
+import java.util.List;
+
+/**
+ * Created by verastov
+ */
+public class XY implements Constraint2 {
+
+  private final Vector point;
+  private final Vector lock;
+
+  public XY(Vector point, Vector lock) {
+    this.point = point;
+    this.lock = lock;
+  }
+
+  public Vector diff() {
+    return lock.minus(point);
+  }
+
+  @Override
+  public double error() {
+    Vector diff = diff();
+    return diff.x * diff.x + diff.y * diff.y;
+  }
+
+  @Override
+  public List<Vector> params() {
+    return Collections.singletonList(point);
+  }
+
+  @Override
+  public List<Vector> gradient() {
+    return Collections.singletonList(diff());
+  }
+
+  @Override
+  public Object debug() {
+    return point;
+  }
+}
diff --git a/src/cad/gl/Scene.java b/src/cad/gl/Scene.java
index 90c4eed4..ab0ee97a 100644
--- a/src/cad/gl/Scene.java
+++ b/src/cad/gl/Scene.java
@@ -1,11 +1,9 @@
 package cad.gl;
 
-import cad.fx.Polygon;
 import cad.math.Vector;
 import com.jogamp.newt.event.MouseEvent;
 import com.jogamp.newt.event.MouseListener;
 import com.jogamp.newt.opengl.GLWindow;
-import com.sun.javafx.geom.*;
 import javafx.scene.input.PickResult;
 
 import javax.media.opengl.GL2;
@@ -154,7 +152,7 @@ public class Scene implements GLEventListener, MouseListener {
 //    Vector dir = pickRay[1].minus(pickRay[0]);
     
 //    pickRay[1] = pickRay[1].minus(pickRay[0]).scale(700);//.normalize().scale(55);
-    pickRay[1] = pickRay[1].minus(pickRay[0]).normalize().scale(30);
+    pickRay[1] = pickRay[1].minus(pickRay[0]).normalize().multi(30);
   }
 
   public static float[] fixW(float[] v) {
diff --git a/src/cad/math/Vector.java b/src/cad/math/Vector.java
index 8eb7e8ac..23d87584 100644
--- a/src/cad/math/Vector.java
+++ b/src/cad/math/Vector.java
@@ -53,11 +53,11 @@ public class Vector {
     this.z = data[2];
   }
 
-  public Vector scale(double factor) {
-    return scale(factor, factor, factor);
+  public Vector multi(double factor) {
+    return multi(factor, factor, factor);
   }
 
-  public Vector scale(double dx, double dy, double dz) {
+  public Vector multi(double dx, double dy, double dz) {
     return new Vector(x * dx, y * dy, z * dz);
   }
 
@@ -83,7 +83,14 @@ public class Vector {
   }
 
   public Vector plus(Vector vector) {
-    return new Vector(x + vector.x, y + vector.y, z + vector.z);
+    return new Vector(this)._plus(vector);
+  }
+
+  public Vector _plus(Vector vector) {
+    x += vector.x;
+    y += vector.y;
+    z += vector.z;
+    return this;
   }
 
   public Vector plus(double dx, double dy, double dz) {
@@ -118,7 +125,7 @@ public class Vector {
   }
 
   public Vector negate() {
-    return scale(-1);
+    return multi(-1);
   }
 
 }