From 4f914c19c72d70bd4b477450415fb732243fe02a Mon Sep 17 00:00:00 2001
From: Val Erastov <xibyte@gmail.com>
Date: Wed, 1 Oct 2014 00:18:52 -0700
Subject: [PATCH] splitup constraints for subsystems

---
 src/cad/gcs/GlobalSolver.java | 22 +++++++++++++-
 src/cad/gcs/Solver.java       | 55 +++++++++++++++++++++++++++++++----
 2 files changed, 71 insertions(+), 6 deletions(-)

diff --git a/src/cad/gcs/GlobalSolver.java b/src/cad/gcs/GlobalSolver.java
index 317c619c..fab1b085 100644
--- a/src/cad/gcs/GlobalSolver.java
+++ b/src/cad/gcs/GlobalSolver.java
@@ -14,10 +14,11 @@ import org.apache.commons.math3.optim.nonlinear.vector.jacobian.LevenbergMarquar
 import java.lang.*;
 import java.util.ArrayList;
 import java.util.Arrays;
+import java.util.List;
 
 public class GlobalSolver {
 
-  public static void globalSolve(Solver.SubSystem subSystem, Runnable linearSolvedCallback) {
+  public static void globalSolve2(Solver.SubSystem subSystem, Runnable linearSolvedCallback) {
 
 //    for (Constraint c : subSystem.constraints) {
 //      if (c instanceof Reconcilable) {
@@ -43,6 +44,25 @@ public class GlobalSolver {
     }
   }
 
+  public static void globalSolve(Solver.SubSystem subSystem, Runnable linearSolvedCallback) {
+
+
+    double eps = 0.0001;
+    java.lang.System.out.println("Solve system with error: " + subSystem.value());
+    int count = 0;
+
+    List<Solver.SubSystem> subSystems = subSystem.splitUp();
+    for (Solver.SubSystem system : subSystems) {
+      java.lang.System.out.println("Solve subsystem: " + subSystem.value());
+      solveLM_COMMONS(system);
+      Solver.solve_BFGS(subSystem, false);
+      Solver.solve_LM(subSystem);
+      java.lang.System.out.println("Subsystem solved: " + subSystem.value());
+      linearSolvedCallback.run();
+    }
+    linearSolvedCallback.run();
+  }
+
 
   public static void solveLM_COMMONS(final Solver.SubSystem subSystem) {
     double eps = 1e-10, eps1 = 1e-80;
diff --git a/src/cad/gcs/Solver.java b/src/cad/gcs/Solver.java
index 915a170b..b1134a44 100644
--- a/src/cad/gcs/Solver.java
+++ b/src/cad/gcs/Solver.java
@@ -16,9 +16,13 @@ import org.apache.commons.math3.linear.RealMatrix;
 import org.apache.commons.math3.linear.SingularMatrixException;
 
 import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.HashSet;
 import java.util.Iterator;
 import java.util.LinkedHashMap;
+import java.util.LinkedHashSet;
 import java.util.List;
+import java.util.Set;
 
 public class Solver {
 
@@ -718,8 +722,8 @@ public class Solver {
 
   static class ParamInfo {
 
-    final int id;
-    final List<Constraint> constraints = new ArrayList<>();
+    int id;
+    Set<Constraint> constraints = new LinkedHashSet<>();
 
     ParamInfo(int id) {
       this.id = id;
@@ -732,7 +736,7 @@ public class Solver {
     private final LinkedHashMap<Param, ParamInfo> params = new LinkedHashMap<>();
 
     public SubSystem(List<Constraint> constraints) {
-      this.constraints = constraints;
+      this.constraints = new ArrayList<>(constraints);
       for (Constraint c : constraints) {
 
         for (Param p : c.getParams()) {
@@ -835,7 +839,9 @@ public class Solver {
 
         for (int p = 0; p < cParams.length; p++) {
           Param param = cParams[p];
-          int j = params.get(param).id;
+          ParamInfo pi = params.get(param);
+          if (pi == null) continue;
+          int j = pi.id;
           jacobi.setEntry(i,j, param.isLocked() ? 0 : grad[p]);
         }
       }
@@ -872,7 +878,9 @@ public class Solver {
 
         Param[] localParams = c.getParams();
         for (int i = 0; i < localParams.length; i++) {
-          grad[params.get(localParams[i]).id] += error * localGrad[i];
+          ParamInfo pi = params.get(localParams[i]);
+          if (pi == null) continue;
+          grad[pi.id] += error * localGrad[i];
         }
       }
       return grad;
@@ -887,6 +895,43 @@ public class Solver {
     }
 
 
+    public List<SubSystem> splitUp () {
+      List<SubSystem> subSystems = new ArrayList<>();
+      for (Constraint constraint : constraints) {
+        Set<Param> params = new HashSet<>(Arrays.asList(constraint.getParams()));
+        SubSystem subSystem = new SubSystem(constraints);
+        subSystems.add(subSystem);
+        Iterator<Param> it = subSystem.params.keySet().iterator();
+        while (it.hasNext()) {
+          Param param = it.next();
+          if (!params.contains(param)) {
+            it.remove();
+          }
+        }
+        int i = 0;
+        for (ParamInfo pi : subSystem.params.values()) {
+          pi.id = i ++;
+        }
+        Iterator<Constraint> cit = subSystem.constraints.iterator();
+        while (cit.hasNext()) {
+          Constraint c = cit.next();
+          boolean remove = true;
+          for (ParamInfo pi : subSystem.params.values()) {
+            if (pi.constraints.contains(c)) {
+              remove = false;
+              break;
+            }
+          }
+          if (remove) {
+            cit.remove();
+          }
+      }
+
+      }
+
+      return subSystems;
+    }
+
   }
 
   private static double dotProduct(RealMatrix m1, RealMatrix m2) {