source: branches/2994-AutoDiffForIntervals/HeuristicLab.Problems.DataAnalysis.Regression.Symbolic.Extensions/ConstrainedNLSInternal.cs @ 17203

using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Runtime.InteropServices;
using HeuristicLab.Common;
using HeuristicLab.Encodings.SymbolicExpressionTreeEncoding;

namespace HeuristicLab.Problems.DataAnalysis.Symbolic.Regression.Extensions {
  internal class ConstrainedNLSInternal {
    private readonly int maxIterations;
    public int MaxIterations => maxIterations;

    private readonly string solver;
    public string Solver => solver;

    private readonly ISymbolicExpressionTree expr;
    public ISymbolicExpressionTree Expr => expr;

    private readonly IRegressionProblemData problemData;

    public IRegressionProblemData ProblemData => problemData;


    public event Action FunctionEvaluated;
    public event Action<int, double> ConstraintEvaluated;

    private double bestError = double.MaxValue;
    public double BestError => bestError;

    private double curError = double.MaxValue;
    public double CurError => curError;

    private double[] bestSolution;
    public double[] BestSolution => bestSolution;

    private ISymbolicExpressionTree bestTree;
    public ISymbolicExpressionTree BestTree => bestTree;

    // begin internal state
    private IntPtr nlopt;
    private SymbolicDataAnalysisExpressionTreeLinearInterpreter interpreter;
    private readonly NLOpt.nlopt_func calculateObjectiveDelegate; // must hold the delegate to prevent GC
    private readonly IntPtr[] constraintDataPtr; // must hold the objects to prevent GC
    private readonly NLOpt.nlopt_func[] calculateConstraintDelegates; // must hold the delegates to prevent GC
    private readonly List<double> thetaValues;
    private readonly IDictionary<string, Interval> dataIntervals;
    private readonly int[] trainingRows;
    private readonly double[] target;
    private readonly ISymbolicExpressionTree preparedTree;
    private readonly ISymbolicExpressionTreeNode[] preparedTreeParameterNodes;
    private readonly List<ConstantTreeNode>[] allThetaNodes;
    private readonly double[] fi_eval;
    private readonly double[,] jac_eval;
    private readonly ISymbolicExpressionTree scaledTree;

    // end internal state


    // for data exchange to/from optimizer in native code
    [StructLayout(LayoutKind.Sequential)]
    private struct ConstraintData {
      public int Idx;
      public ISymbolicExpressionTree Tree;
      public ISymbolicExpressionTreeNode[] ParameterNodes;
    }

    internal ConstrainedNLSInternal(string solver, ISymbolicExpressionTree expr, int maxIterations, IRegressionProblemData problemData, double ftol_rel = 0, double ftol_abs = 0, double maxTime = 0) {
      this.solver = solver;
      this.expr = expr;
      this.maxIterations = maxIterations;
      this.problemData = problemData;
      this.interpreter = new SymbolicDataAnalysisExpressionTreeLinearInterpreter();


      var intervalConstraints = problemData.IntervalConstraints;
      dataIntervals = problemData.VariableRanges.GetIntervals();
      trainingRows = problemData.TrainingIndices.ToArray();
      // buffers 
      target = problemData.TargetVariableTrainingValues.ToArray();
      var targetStDev = target.StandardDeviationPop();
      var targetVariance = targetStDev * targetStDev;
      var targetMean = target.Average();
      var pred = interpreter.GetSymbolicExpressionTreeValues(expr, problemData.Dataset, trainingRows).ToArray();

      if (pred.Any(pi => double.IsInfinity(pi) || double.IsNaN(pi))) throw new ArgumentException("The expression produces NaN or infinite values.");

      #region linear scaling
      var predStDev = pred.StandardDeviationPop();
      if (predStDev == 0) throw new ArgumentException("The expression is constant.");
      var predMean = pred.Average();

      var scalingFactor = targetStDev / predStDev;
      var offset = targetMean - predMean * scalingFactor;

      scaledTree = CopyAndScaleTree(expr, scalingFactor, offset);
      #endregion

      // convert constants to variables named theta...
      var treeForDerivation = ReplaceConstWithVar(scaledTree, out List<string> thetaNames, out thetaValues); // copies the tree

      // create trees for relevant derivatives
      Dictionary<string, ISymbolicExpressionTree> derivatives = new Dictionary<string, ISymbolicExpressionTree>();
      allThetaNodes = thetaNames.Select(_ => new List<ConstantTreeNode>()).ToArray();
      var constraintTrees = new List<ISymbolicExpressionTree>();
      foreach (var constraint in intervalConstraints.Constraints) {
        if (constraint.IsDerivation) {
          if (!problemData.AllowedInputVariables.Contains(constraint.Variable))
            throw new ArgumentException($"Invalid constraint: the variable {constraint.Variable} does not exist in the dataset.");
          var df = DerivativeCalculator.Derive(treeForDerivation, constraint.Variable);

          // NLOpt requires constraint expressions of the form c(x) <= 0
          // -> we make two expressions, one for the lower bound and one for the upper bound

          if (constraint.Interval.UpperBound < double.PositiveInfinity) {
            var df_smaller_upper = Subtract((ISymbolicExpressionTree)df.Clone(), CreateConstant(constraint.Interval.UpperBound));
            // convert variables named theta back to constants
            var df_prepared = ReplaceVarWithConst(df_smaller_upper, thetaNames, thetaValues, allThetaNodes);
            constraintTrees.Add(df_prepared);
          }
          if (constraint.Interval.LowerBound > double.NegativeInfinity) {
            var df_larger_lower = Subtract(CreateConstant(constraint.Interval.LowerBound), (ISymbolicExpressionTree)df.Clone());
            // convert variables named theta back to constants
            var df_prepared = ReplaceVarWithConst(df_larger_lower, thetaNames, thetaValues, allThetaNodes);
            constraintTrees.Add(df_prepared);
          }
        } else {
          if (constraint.Interval.UpperBound < double.PositiveInfinity) {
            var f_smaller_upper = Subtract((ISymbolicExpressionTree)treeForDerivation.Clone(), CreateConstant(constraint.Interval.UpperBound));
            // convert variables named theta back to constants
            var df_prepared = ReplaceVarWithConst(f_smaller_upper, thetaNames, thetaValues, allThetaNodes);
            constraintTrees.Add(df_prepared);
          }
          if (constraint.Interval.LowerBound > double.NegativeInfinity) {
            var f_larger_lower = Subtract(CreateConstant(constraint.Interval.LowerBound), (ISymbolicExpressionTree)treeForDerivation.Clone());
            // convert variables named theta back to constants
            var df_prepared = ReplaceVarWithConst(f_larger_lower, thetaNames, thetaValues, allThetaNodes);
            constraintTrees.Add(df_prepared);
          }
        }
      }

      preparedTree = ReplaceVarWithConst(treeForDerivation, thetaNames, thetaValues, allThetaNodes);
      preparedTreeParameterNodes = GetParameterNodes(preparedTree, allThetaNodes);

      var dim = thetaValues.Count;
      fi_eval = new double[target.Length]; // init buffer;
      jac_eval = new double[target.Length, dim]; // init buffer


      var minVal = Math.Min(-1000.0, thetaValues.Min());
      var maxVal = Math.Max(1000.0, thetaValues.Max());
      var lb = Enumerable.Repeat(minVal, thetaValues.Count).ToArray();
      var up = Enumerable.Repeat(maxVal, thetaValues.Count).ToArray();
      nlopt = NLOpt.nlopt_create(GetSolver(solver), (uint)dim);

      NLOpt.nlopt_set_lower_bounds(nlopt, lb);
      NLOpt.nlopt_set_upper_bounds(nlopt, up);
      calculateObjectiveDelegate = new NLOpt.nlopt_func(CalculateObjective); // keep a reference to the delegate (see below)
      NLOpt.nlopt_set_min_objective(nlopt, calculateObjectiveDelegate, IntPtr.Zero);


      constraintDataPtr = new IntPtr[constraintTrees.Count];
      calculateConstraintDelegates = new NLOpt.nlopt_func[constraintTrees.Count]; // make sure we keep a reference to the delegates (otherwise GC will free delegate objects see https://stackoverflow.com/questions/7302045/callback-delegates-being-collected#7302258)
      for (int i = 0; i < constraintTrees.Count; i++) {
        var constraintData = new ConstraintData() { Idx = i, Tree = constraintTrees[i], ParameterNodes = GetParameterNodes(constraintTrees[i], allThetaNodes) };
        constraintDataPtr[i] = Marshal.AllocHGlobal(Marshal.SizeOf<ConstraintData>());
        Marshal.StructureToPtr(constraintData, constraintDataPtr[i], fDeleteOld: false);
        calculateConstraintDelegates[i] = new NLOpt.nlopt_func(CalculateConstraint);
        NLOpt.nlopt_add_inequality_constraint(nlopt, calculateConstraintDelegates[i], constraintDataPtr[i], 1e-8);
      }

      NLOpt.nlopt_set_ftol_rel(nlopt, ftol_rel);
      NLOpt.nlopt_set_ftol_abs(nlopt, ftol_abs);
      NLOpt.nlopt_set_maxtime(nlopt, maxTime);
      NLOpt.nlopt_set_maxeval(nlopt, maxIterations);
    }

    ~ConstrainedNLSInternal() {
      if (nlopt != IntPtr.Zero)
        NLOpt.nlopt_destroy(nlopt);
      if (constraintDataPtr != null) {
        for (int i = 0; i < constraintDataPtr.Length; i++)
          if (constraintDataPtr[i] != IntPtr.Zero)
            Marshal.FreeHGlobal(constraintDataPtr[i]);
      }
    }


    internal void Optimize() {
      var x = thetaValues.ToArray();  /* initial guess */
      double minf = double.MaxValue; /* minimum objective value upon return */
      var res = NLOpt.nlopt_optimize(nlopt, x, ref minf);
      bestSolution = x;
      bestError = minf;

      if (res < 0) {
        throw new InvalidOperationException($"NLOpt failed {res} {NLOpt.nlopt_get_errmsg(nlopt)}");
      } else {
        // update parameters in tree
        var pIdx = 0;
        // here we lose the two last parameters (for linear scaling)
        foreach (var node in scaledTree.IterateNodesPostfix()) {
          if (node is ConstantTreeNode constTreeNode) {
            constTreeNode.Value = x[pIdx++];
          } else if (node is VariableTreeNode varTreeNode) {
            varTreeNode.Weight = x[pIdx++];
          }
        }
        if (pIdx != x.Length) throw new InvalidProgramException();
      }
      bestTree = scaledTree;
    }

    double CalculateObjective(uint dim, double[] curX, double[] grad, IntPtr data) {
      UpdateThetaValues(curX);
      var sse = 0.0;

      if (grad != null) {
        var autoDiffEval = new VectorAutoDiffEvaluator();
        autoDiffEval.Evaluate(preparedTree, problemData.Dataset, trainingRows,
          preparedTreeParameterNodes, fi_eval, jac_eval);

        // calc sum of squared errors and gradient
        for (int j = 0; j < grad.Length; j++) grad[j] = 0;
        for (int i = 0; i < target.Length; i++) {
          var r = target[i] - fi_eval[i];
          sse += r * r;
          for (int j = 0; j < grad.Length; j++) {
            grad[j] -= 2 * r * jac_eval[i, j];
          }
        }
        // average
        for (int j = 0; j < grad.Length; j++) { grad[j] /= target.Length; }

        #region check gradient
        var eval = new VectorEvaluator();
        for (int i = 0; i < dim; i++) {
          // make two additional evaluations
          var xForNumDiff = (double[])curX.Clone();
          double delta = Math.Abs(xForNumDiff[i] * 1e-5);
          xForNumDiff[i] += delta;
          UpdateThetaValues(xForNumDiff);
          var evalHigh = eval.Evaluate(preparedTree, problemData.Dataset, trainingRows);
          var mseHigh = MSE(target, evalHigh);
          xForNumDiff[i] = curX[i] - delta;
          UpdateThetaValues(xForNumDiff);
          var evalLow = eval.Evaluate(preparedTree, problemData.Dataset, trainingRows);
          var mseLow = MSE(target, evalLow);

          var numericDiff = (mseHigh - mseLow) / (2 * delta);
          var autoDiff = grad[i];
          if ((Math.Abs(autoDiff) < 1e-10 && Math.Abs(numericDiff) > 1e-2)
            || (Math.Abs(autoDiff) >= 1e-10 && Math.Abs((numericDiff - autoDiff) / numericDiff) > 1e-2))
            throw new InvalidProgramException();
        }
        #endregion
      } else {
        var eval = new VectorEvaluator();
        var prediction = eval.Evaluate(preparedTree, problemData.Dataset, trainingRows);

        // calc sum of squared errors
        sse = 0.0;
        for (int i = 0; i < target.Length; i++) {
          var r = target[i] - prediction[i];
          sse += r * r;
        }
      }

      UpdateBestSolution(sse / target.Length, curX);
      RaiseFunctionEvaluated();

      if (double.IsNaN(sse)) return double.MaxValue;
      return sse / target.Length;
    }

    private double MSE(double[] a, double[] b) {
      Trace.Assert(a.Length == b.Length);
      var sse = 0.0;
      for (int i = 0; i < a.Length; i++) sse += (a[i] - b[i]) * (a[i] - b[i]);
      return sse / a.Length;
    }

    private void UpdateBestSolution(double curF, double[] curX) {
      if (double.IsNaN(curF) || double.IsInfinity(curF)) return;
      else if (curF < bestError) {
        bestError = curF;
        bestSolution = (double[])curX.Clone();
      }
    }

    private void UpdateConstraintViolations(int constraintIdx, double value) {
      if (double.IsNaN(value) || double.IsInfinity(value)) return;
      RaiseConstraintEvaluated(constraintIdx, value);
      // else if (curF < bestError) {
      //   bestError = curF;
      //   bestSolution = (double[])curX.Clone();
      // }
    }

    double CalculateConstraint(uint dim, double[] curX, double[] grad, IntPtr data) {
      UpdateThetaValues(curX);
      var intervalEvaluator = new IntervalEvaluator();
      var constraintData = Marshal.PtrToStructure<ConstraintData>(data);

      if (grad != null) for (int j = 0; j < grad.Length; j++) grad[j] = 0; // clear grad

      var interval = intervalEvaluator.Evaluate(constraintData.Tree, dataIntervals, constraintData.ParameterNodes,
        out double[] lowerGradient, out double[] upperGradient);

      // we transformed this to a constraint c(x) <= 0, so only the upper bound is relevant for us
      if (grad != null) for (int j = 0; j < grad.Length; j++) { grad[j] = upperGradient[j]; }

      #region check gradient
      for (int i = 0; i < dim; i++) {
        // make two additional evaluations
        var xForNumDiff = (double[])curX.Clone();
        double delta = Math.Abs(xForNumDiff[i] * 1e-5);
        xForNumDiff[i] += delta;
        UpdateThetaValues(xForNumDiff);
        var evalHigh = intervalEvaluator.Evaluate(constraintData.Tree, dataIntervals, constraintData.ParameterNodes,
          out double[] unusedLowerGradientHigh, out double[] unusedUpperGradientHigh);

        xForNumDiff[i] = curX[i] - delta;
        UpdateThetaValues(xForNumDiff);
        var evalLow = intervalEvaluator.Evaluate(constraintData.Tree, dataIntervals, constraintData.ParameterNodes,
          out double[] unusedLowerGradientLow, out double[] unusedUpperGradientLow);
        
        var numericDiff = (evalHigh.UpperBound - evalLow.UpperBound) / (2 * delta);
        var autoDiff = grad[i];
        // XXX GRADIENT FOR UPPER BOUND FOR FUNCTION IS ZERO FOR THE FIRST SET OF VARIABLES? GRADIENT FOR ADDITION INCORRECT?!
        if ((Math.Abs(autoDiff) < 1e-10 && Math.Abs(numericDiff) > 1e-2)
          || (Math.Abs(autoDiff) >= 1e-10 && Math.Abs((numericDiff - autoDiff) / numericDiff) > 1e-2))
          throw new InvalidProgramException();
      }
      #endregion


      UpdateConstraintViolations(constraintData.Idx, interval.UpperBound);
      if (double.IsNaN(interval.UpperBound)) return double.MaxValue;
      else return interval.UpperBound;
    }


    void UpdateThetaValues(double[] theta) {
      for (int i = 0; i < theta.Length; ++i) {
        foreach (var constNode in allThetaNodes[i]) constNode.Value = theta[i];
      }
    }

    internal void RequestStop() {
      NLOpt.nlopt_set_force_stop(nlopt, 1); // hopefully NLOpt is thread safe  , val must be <> 0 otherwise no effect
    }

    private void RaiseFunctionEvaluated() {
      FunctionEvaluated?.Invoke();
    }

    private void RaiseConstraintEvaluated(int idx, double value) {
      ConstraintEvaluated?.Invoke(idx, value);
    }


    #region helper

    private static ISymbolicExpressionTree CopyAndScaleTree(ISymbolicExpressionTree tree, double scalingFactor, double offset) {
      var m = (ISymbolicExpressionTree)tree.Clone();

      var add = MakeNode<Addition>(MakeNode<Multiplication>(m.Root.GetSubtree(0).GetSubtree(0), CreateConstant(scalingFactor)), CreateConstant(offset));
      m.Root.GetSubtree(0).RemoveSubtree(0);
      m.Root.GetSubtree(0).AddSubtree(add);
      return m;
    }

    private static void UpdateConstants(ISymbolicExpressionTreeNode[] nodes, double[] constants) {
      if (nodes.Length != constants.Length) throw new InvalidOperationException();
      for (int i = 0; i < nodes.Length; i++) {
        if (nodes[i] is VariableTreeNode varNode) varNode.Weight = constants[i];
        else if (nodes[i] is ConstantTreeNode constNode) constNode.Value = constants[i];
      }
    }

    private NLOpt.nlopt_algorithm GetSolver(string solver) {
      if (solver.Contains("MMA")) return NLOpt.nlopt_algorithm.NLOPT_LD_MMA;
      if (solver.Contains("COBYLA")) return NLOpt.nlopt_algorithm.NLOPT_LN_COBYLA;
      if (solver.Contains("CCSAQ")) return NLOpt.nlopt_algorithm.NLOPT_LD_CCSAQ;
      if (solver.Contains("ISRES")) return NLOpt.nlopt_algorithm.NLOPT_GN_ISRES;
      throw new ArgumentException($"Unknown solver {solver}");
    }

    private static ISymbolicExpressionTreeNode[] GetParameterNodes(ISymbolicExpressionTree tree, List<ConstantTreeNode>[] allNodes) {
      // TODO better solution necessary
      var treeConstNodes = tree.IterateNodesPostfix().OfType<ConstantTreeNode>().ToArray();
      var paramNodes = new ISymbolicExpressionTreeNode[allNodes.Length];
      for (int i = 0; i < paramNodes.Length; i++) {
        paramNodes[i] = allNodes[i].SingleOrDefault(n => treeConstNodes.Contains(n));
      }
      return paramNodes;
    }

    private static ISymbolicExpressionTree ReplaceVarWithConst(ISymbolicExpressionTree tree, List<string> thetaNames, List<double> thetaValues, List<ConstantTreeNode>[] thetaNodes) {
      var copy = (ISymbolicExpressionTree)tree.Clone();
      var nodes = copy.IterateNodesPostfix().ToList();
      for (int i = 0; i < nodes.Count; i++) {
        var n = nodes[i] as VariableTreeNode;
        if (n != null) {
          var thetaIdx = thetaNames.IndexOf(n.VariableName);
          if (thetaIdx >= 0) {
            var parent = n.Parent;
            if (thetaNodes[thetaIdx].Any()) {
              // HACK: REUSE CONSTANT TREE NODE IN SEVERAL TREES
              // we use this trick to allow autodiff over thetas when thetas occurr multiple times in the tree (e.g. in derived trees)
              var constNode = thetaNodes[thetaIdx].First();
              var childIdx = parent.IndexOfSubtree(n);
              parent.RemoveSubtree(childIdx);
              parent.InsertSubtree(childIdx, constNode);
            } else {
              var constNode = (ConstantTreeNode)CreateConstant(thetaValues[thetaIdx]);
              var childIdx = parent.IndexOfSubtree(n);
              parent.RemoveSubtree(childIdx);
              parent.InsertSubtree(childIdx, constNode);
              thetaNodes[thetaIdx].Add(constNode);
            }
          }
        }
      }
      return copy;
    }

    private static ISymbolicExpressionTree ReplaceConstWithVar(ISymbolicExpressionTree tree, out List<string> thetaNames, out List<double> thetaValues) {
      thetaNames = new List<string>();
      thetaValues = new List<double>();
      var copy = (ISymbolicExpressionTree)tree.Clone();
      var nodes = copy.IterateNodesPostfix().ToList();

      int n = 1;
      for (int i = 0; i < nodes.Count; ++i) {
        var node = nodes[i];
        if (node is ConstantTreeNode constantTreeNode) {
          var thetaVar = (VariableTreeNode)new Problems.DataAnalysis.Symbolic.Variable().CreateTreeNode();
          thetaVar.Weight = 1;
          thetaVar.VariableName = $"θ{n++}";

          thetaNames.Add(thetaVar.VariableName);
          thetaValues.Add(constantTreeNode.Value);

          var parent = constantTreeNode.Parent;
          if (parent != null) {
            var index = constantTreeNode.Parent.IndexOfSubtree(constantTreeNode);
            parent.RemoveSubtree(index);
            parent.InsertSubtree(index, thetaVar);
          }
        }
        if (node is VariableTreeNode varTreeNode) {
          var thetaVar = (VariableTreeNode)new Problems.DataAnalysis.Symbolic.Variable().CreateTreeNode();
          thetaVar.Weight = 1;
          thetaVar.VariableName = $"θ{n++}";

          thetaNames.Add(thetaVar.VariableName);
          thetaValues.Add(varTreeNode.Weight);

          var parent = varTreeNode.Parent;
          if (parent != null) {
            var index = varTreeNode.Parent.IndexOfSubtree(varTreeNode);
            parent.RemoveSubtree(index);
            var prodNode = MakeNode<Multiplication>();
            varTreeNode.Weight = 1.0;
            prodNode.AddSubtree(varTreeNode);
            prodNode.AddSubtree(thetaVar);
            parent.InsertSubtree(index, prodNode);
          }
        }
      }
      return copy;
    }

    private static ISymbolicExpressionTreeNode CreateConstant(double value) {
      var constantNode = (ConstantTreeNode)new Constant().CreateTreeNode();
      constantNode.Value = value;
      return constantNode;
    }

    private static ISymbolicExpressionTree Subtract(ISymbolicExpressionTree t, ISymbolicExpressionTreeNode b) {
      var sub = MakeNode<Subtraction>(t.Root.GetSubtree(0).GetSubtree(0), b);
      t.Root.GetSubtree(0).RemoveSubtree(0);
      t.Root.GetSubtree(0).InsertSubtree(0, sub);
      return t;
    }
    private static ISymbolicExpressionTree Subtract(ISymbolicExpressionTreeNode b, ISymbolicExpressionTree t) {
      var sub = MakeNode<Subtraction>(b, t.Root.GetSubtree(0).GetSubtree(0));
      t.Root.GetSubtree(0).RemoveSubtree(0);
      t.Root.GetSubtree(0).InsertSubtree(0, sub);
      return t;
    }

    private static ISymbolicExpressionTreeNode MakeNode<T>(params ISymbolicExpressionTreeNode[] fs) where T : ISymbol, new() {
      var node = new T().CreateTreeNode();
      foreach (var f in fs) node.AddSubtree(f);
      return node;
    }
    #endregion
  }
}