source: branches/2994-AutoDiffForIntervals/HeuristicLab.Problems.DataAnalysis.Symbolic/3.4/Interpreter/Interpreter.cs @ 16694

using System;
using System.Collections.Generic;
using System.Linq;
using HeuristicLab.Common;
using HeuristicLab.Encodings.SymbolicExpressionTreeEncoding;

namespace HeuristicLab.Problems.DataAnalysis.Symbolic {
  public abstract class Interpreter<T> where T : IAlgebraicType<T> {
    public struct Instruction {
      public byte opcode;
      public ushort narg;
      public int childIndex;
      public double dblVal;
      public object data; // any kind of data you want to store in instructions
      public T value;
    }

    public T Evaluate(Instruction[] code) {
      for (int i = code.Length - 1; i >= 0; --i) {
        var instr = code[i];
        var c = instr.childIndex;
        var n = instr.narg;

        switch (instr.opcode) {
          case OpCodes.Variable: {
              LoadVariable(instr);
              break;
            }
          case OpCodes.Constant: { break; }  // we initialize constants in Compile. The value never changes afterwards
          case OpCodes.Add: {
              instr.value.Assign(code[c].value);
              for (int j = 1; j < n; ++j) {
                instr.value.Add(code[c + j].value);
              }
              break;
            }

          case OpCodes.Sub: {
              if (n == 1) {
                instr.value.AssignNeg(code[c].value);
              } else {
                instr.value.Assign(code[c].value);
                for (int j = 1; j < n; ++j) {
                  instr.value.Sub(code[c + j].value);
                }
              }
              break;
            }

          case OpCodes.Mul: {
              instr.value.Assign(code[c].value);
              for (int j = 1; j < n; ++j) {
                instr.value.Mul(code[c + j].value);
              }
              break;
            }

          case OpCodes.Div: {
              if (n == 1) {
                instr.value.AssignInv(code[c].value);
              } else {
                instr.value.Assign(code[c].value);
                for (int j = 1; j < n; ++j) {
                  instr.value.Div(code[c + j].value);
                }
              }
              break;
            }
          case OpCodes.Square: {
              instr.value.AssignIntPower(code[c].value, 2);
              break;
            }
          case OpCodes.Exp: {
              instr.value.AssignExp(code[c].value);
              break;
            }

          case OpCodes.Log: {
              instr.value.AssignLog(code[c].value);
              break;
            }
          default: throw new ArgumentException($"Unknown opcode {code[c].opcode}");
        }
      }
      return code[0].value;
    }

    protected Instruction[] Compile(ISymbolicExpressionTree tree) {
      var root = tree.Root.GetSubtree(0).GetSubtree(0);
      var code = new Instruction[root.GetLength()];
      if (root.SubtreeCount > ushort.MaxValue) throw new ArgumentException("Number of subtrees is too big (>65.535)");
      int c = 1, i = 0;
      foreach (var node in root.IterateNodesBreadth()) {
        if (node.SubtreeCount > ushort.MaxValue) throw new ArgumentException("Number of subtrees is too big (>65.535)");
        code[i] = new Instruction {
          opcode = OpCodes.MapSymbolToOpCode(node),
          narg = (ushort)node.SubtreeCount,
          childIndex = c
        };
        if (node is VariableTreeNode variable) {
          InitializeTerminalInstruction(ref code[i], variable);
        } else if (node is ConstantTreeNode constant) {
          InitializeTerminalInstruction(ref code[i], constant);
        } else {
          InitializeInternalInstruction(ref code[i], node);
        }
        c += node.SubtreeCount;
        ++i;
      }
      return code;
    }

    protected abstract void InitializeTerminalInstruction(ref Instruction instruction, ConstantTreeNode constant);
    protected abstract void InitializeTerminalInstruction(ref Instruction instruction, VariableTreeNode variable);
    protected abstract void InitializeInternalInstruction(ref Instruction instruction, ISymbolicExpressionTreeNode node);

    protected abstract void LoadVariable(Instruction a);

  }


  public sealed class VectorEvaluator : Interpreter<DoubleVector> {
    private const int BATCHSIZE = 128;
    [ThreadStatic]
    private Dictionary<string, double[]> cachedData;

    [ThreadStatic]
    private IDataset dataset;

    [ThreadStatic]
    private int rowIndex;

    [ThreadStatic]
    private int[] rows;

    private void InitCache(IDataset dataset) {
      this.dataset = dataset;
      cachedData = new Dictionary<string, double[]>();
      foreach (var v in dataset.DoubleVariables) {
        cachedData[v] = dataset.GetReadOnlyDoubleValues(v).ToArray();
      }
    }

    public double[] Evaluate(ISymbolicExpressionTree tree, IDataset dataset, int[] rows) {
      if (cachedData == null || this.dataset != dataset) {
        InitCache(dataset);
      }

      this.rows = rows;
      var code = Compile(tree);
      var remainingRows = rows.Length % BATCHSIZE;
      var roundedTotal = rows.Length - remainingRows;

      var result = new double[rows.Length];

      for (rowIndex = 0; rowIndex < roundedTotal; rowIndex += BATCHSIZE) {
        Evaluate(code);
        code[0].value.CopyTo(result, rowIndex, BATCHSIZE);
      }

      if (remainingRows > 0) {
        Evaluate(code);
        code[0].value.CopyTo(result, roundedTotal, remainingRows);
      }

      return result;
    }

    protected override void InitializeTerminalInstruction(ref Instruction instruction, ConstantTreeNode constant) {
      instruction.dblVal = constant.Value;
      instruction.value = new DoubleVector(BATCHSIZE);
      instruction.value.AssignConstant(instruction.dblVal);
    }

    protected override void InitializeTerminalInstruction(ref Instruction instruction, VariableTreeNode variable) {
      instruction.dblVal = variable.Weight;
      instruction.value = new DoubleVector(BATCHSIZE);
      if (cachedData.ContainsKey(variable.VariableName)) {
        instruction.data = cachedData[variable.VariableName];
      } else {
        instruction.data = dataset.GetDoubleValues(variable.VariableName).ToArray();
        cachedData[variable.VariableName] = (double[])instruction.data;
      }
    }

    protected override void InitializeInternalInstruction(ref Instruction instruction, ISymbolicExpressionTreeNode node) {
      instruction.value = new DoubleVector(BATCHSIZE);
    }

    protected override void LoadVariable(Instruction a) {
      var data = (double[])a.data;
      for (int i = rowIndex; i < rows.Length && i - rowIndex < BATCHSIZE; i++) a.value[i - rowIndex] = data[rows[i]];
      a.value.Scale(a.dblVal);
    }
  }

  public sealed class VectorAutoDiffEvaluator : Interpreter<MultivariateDual<DoubleVector>> {
    private const int BATCHSIZE = 128;
    [ThreadStatic]
    private Dictionary<string, double[]> cachedData;

    [ThreadStatic]
    private IDataset dataset;

    [ThreadStatic]
    private int rowIndex;

    [ThreadStatic]
    private int[] rows;

    [ThreadStatic]
    private Dictionary<ISymbolicExpressionTreeNode, int> node2paramIdx;

    private void InitCache(IDataset dataset) {
      this.dataset = dataset;
      cachedData = new Dictionary<string, double[]>();
      foreach (var v in dataset.DoubleVariables) {
        cachedData[v] = dataset.GetDoubleValues(v).ToArray();
      }
    }

    public void Evaluate(ISymbolicExpressionTree tree, IDataset dataset, int[] rows, ISymbolicExpressionTreeNode[] parameterNodes, out double[] fi, out double[,] jac) {
      if (cachedData == null || this.dataset != dataset) {
        InitCache(dataset);
      }

      int nParams = parameterNodes.Length;
      node2paramIdx = new Dictionary<ISymbolicExpressionTreeNode, int>();
      for (int i = 0; i < parameterNodes.Length; i++) node2paramIdx.Add(parameterNodes[i], i);

      var code = Compile(tree);

      var remainingRows = rows.Length % BATCHSIZE;
      var roundedTotal = rows.Length - remainingRows;

      fi = new double[rows.Length];
      jac = new double[rows.Length, nParams];

      this.rows = rows;

      for (rowIndex = 0; rowIndex < roundedTotal; rowIndex += BATCHSIZE) {
        Evaluate(code);
        code[0].value.Value.CopyTo(fi, rowIndex, BATCHSIZE);

        // TRANSPOSE into JAC
        var g = code[0].value.Gradient;
        for (int j = 0; j < nParams; ++j) {
          g.Elements[j].CopyColumnTo(jac, j, rowIndex, BATCHSIZE);
        }
      }

      if (remainingRows > 0) {
        Evaluate(code);
        code[0].value.Value.CopyTo(fi, roundedTotal, remainingRows);

        var g = code[0].value.Gradient;
        for (int j = 0; j < nParams; ++j)
          g.Elements[j].CopyColumnTo(jac, j, roundedTotal, remainingRows);
      }
    }

    protected override void InitializeInternalInstruction(ref Instruction instruction, ISymbolicExpressionTreeNode node) {
      var zero = new DoubleVector(BATCHSIZE);
      instruction.value = new MultivariateDual<DoubleVector>(zero);
    }

    protected override void InitializeTerminalInstruction(ref Instruction instruction, ConstantTreeNode constant) {
      var g_arr = new double[BATCHSIZE];
      if (node2paramIdx.TryGetValue(constant, out var paramIdx)) {
        for (int i = 0; i < BATCHSIZE; i++) g_arr[i] = 1.0;
        var g = new DoubleVector(g_arr);
        instruction.value = new MultivariateDual<DoubleVector>(new DoubleVector(BATCHSIZE), paramIdx, g); // only a single column for the gradient
      } else {
        instruction.value = new MultivariateDual<DoubleVector>(new DoubleVector(BATCHSIZE));
      }

      instruction.dblVal = constant.Value;
      instruction.value.Value.AssignConstant(instruction.dblVal);
    }

    protected override void InitializeTerminalInstruction(ref Instruction instruction, VariableTreeNode variable) {
      double[] data;
      if (cachedData.ContainsKey(variable.VariableName)) {
        data = cachedData[variable.VariableName];
      } else {
        data = dataset.GetReadOnlyDoubleValues(variable.VariableName).ToArray();
        cachedData[variable.VariableName] = (double[])instruction.data;
      }

      var paramIdx = -1;
      if (node2paramIdx.ContainsKey(variable)) {
        paramIdx = node2paramIdx[variable];
        var f = new DoubleVector(BATCHSIZE);
        var g = new DoubleVector(BATCHSIZE);
        instruction.value = new MultivariateDual<DoubleVector>(f, paramIdx, g);
      } else {
        var f = new DoubleVector(BATCHSIZE);
        instruction.value = new MultivariateDual<DoubleVector>(f);
      }

      instruction.dblVal = variable.Weight;
      instruction.data = new object[] { data, paramIdx };
    }

    protected override void LoadVariable(Instruction a) {
      var paramIdx = (int)((object[])a.data)[1];
      var data = (double[])((object[])a.data)[0];

      for (int i = rowIndex; i < rows.Length && i - rowIndex < BATCHSIZE; i++) a.value.Value[i - rowIndex] = data[rows[i]];
      a.value.Scale(a.dblVal);

      if (paramIdx >= 0) {
        // update gradient with variable values
        var g = a.value.Gradient.Elements[paramIdx];
        for (int i = rowIndex; i < rows.Length && i - rowIndex < BATCHSIZE; i++) {
          g[i] = data[rows[i]];
        }
      }
    }
  }


  public sealed class IntervalEvaluator : Interpreter<AlgebraicInterval> {
    [ThreadStatic]
    private Dictionary<string, Interval> intervals;

    public Interval Evaluate(ISymbolicExpressionTree tree, Dictionary<string, Interval> intervals) {
      this.intervals = intervals;
      var code = Compile(tree);
      Evaluate(code);
      return new Interval(code[0].value.LowerBound.Value.Value, code[0].value.UpperBound.Value.Value);
    }

    public Interval Evaluate(ISymbolicExpressionTree tree, Dictionary<string, Interval> intervals, ISymbolicExpressionTreeNode[] paramNodes, out double[] lowerGradient, out double[] upperGradient) {
      this.intervals = intervals;
      var code = Compile(tree);
      Evaluate(code);
      lowerGradient = new double[paramNodes.Length];
      upperGradient = new double[paramNodes.Length];
      var l = code[0].value.LowerBound;
      var u = code[0].value.UpperBound;
      for (int i = 0; i < paramNodes.Length; ++i) {
        if (paramNodes[i] == null) continue;
        lowerGradient[i] = l.Gradient.Elements[paramNodes[i]];
        upperGradient[i] = u.Gradient.Elements[paramNodes[i]];
      }
      return new Interval(code[0].value.LowerBound.Value.Value, code[0].value.UpperBound.Value.Value);
    }

    protected override void InitializeInternalInstruction(ref Instruction instruction, ISymbolicExpressionTreeNode node) {
      instruction.value = new AlgebraicInterval(0, 0);
    }


    protected override void InitializeTerminalInstruction(ref Instruction instruction, ConstantTreeNode constant) {
      instruction.dblVal = constant.Value;
      instruction.value = new AlgebraicInterval(
        new MultivariateDual<AlgebraicDouble>(constant.Value, constant, 1.0),
        new MultivariateDual<AlgebraicDouble>(constant.Value, constant, 1.0) // use node as key
        );
    }

    protected override void InitializeTerminalInstruction(ref Instruction instruction, VariableTreeNode variable) {
      instruction.dblVal = variable.Weight;
      instruction.value = new AlgebraicInterval(
        low: new MultivariateDual<AlgebraicDouble>(intervals[variable.VariableName].LowerBound, variable, intervals[variable.VariableName].LowerBound),  // bounds change by variable value d/dc (c I(var)) = I(var)
        high: new MultivariateDual<AlgebraicDouble>(intervals[variable.VariableName].UpperBound, variable, intervals[variable.VariableName].UpperBound)
        );
    }

    protected override void LoadVariable(Instruction a) {
      // nothing to do
    }
  }

  public interface IAlgebraicType<T> {
    T Zero { get; }

    T AssignAbs(T a); // set this to assign abs(a)
    T Assign(T a); // assign this to same value as a (copy!)
    T AssignNeg(T a); // set this to negative(a)
    T AssignInv(T a); // set this to inv(a);
    T Scale(double s); // scale this with s
    T Add(T a); // add a to this
    T Sub(T a); // subtract a from this
    T Mul(T a); // multiply this with a
    T Div(T a); // divide this by a
    T AssignLog(T a); // set this to log a
    T AssignExp(T a); // set this to exp(a)
    T AssignSin(T a); // set this to sin(a)
    T AssignCos(T a); // set this to cos(a)
    T AssignIntPower(T a, int p);
    T AssignIntRoot(T a, int r);
    T AssignSgn(T a); // set this to sign(a)
    T Clone();
  }

  public static class Algebraic {
    public static T Abs<T>(this T a) where T : IAlgebraicType<T> { a.AssignAbs(a); return a; }
    public static T Neg<T>(this T a) where T : IAlgebraicType<T> { a.AssignNeg(a); return a; }
    public static T Inv<T>(this T a) where T : IAlgebraicType<T> { a.AssignInv(a); return a; }
    public static T Log<T>(this T a) where T : IAlgebraicType<T> { a.AssignLog(a); return a; }
    public static T Exp<T>(this T a) where T : IAlgebraicType<T> { a.AssignExp(a); return a; }
    public static T Sin<T>(this T a) where T : IAlgebraicType<T> { a.AssignSin(a); return a; }
    public static T Cos<T>(this T a) where T : IAlgebraicType<T> { a.AssignCos(a); return a; }
    public static T Sgn<T>(this T a) where T : IAlgebraicType<T> { a.AssignSgn(a); return a; }
    public static T IntPower<T>(this T a, int p) where T : IAlgebraicType<T> { a.AssignIntPower(a, p); return a; }
    public static T IntRoot<T>(this T a, int r) where T : IAlgebraicType<T> { a.AssignIntRoot(a, r); return a; }

    public static T Max<T>(T a, T b) where T : IAlgebraicType<T> {
      // ((a + b) + abs(b - a)) / 2
      return a.Clone().Add(b).Add(b.Clone().Sub(a).Abs()).Scale(1.0 / 2.0);
    }
    public static T Min<T>(T a, T b) where T : IAlgebraicType<T> {
      // ((a + b) - abs(a - b)) / 2
      return a.Clone().Add(b).Sub(a.Clone().Sub(b).Abs()).Scale(1.0 / 2.0);
    }
  }


  // algebraic type wrapper for a double value
  public sealed class AlgebraicDouble : IAlgebraicType<AlgebraicDouble> {
    public static implicit operator AlgebraicDouble(double value) { return new AlgebraicDouble(value); }
    public static implicit operator double(AlgebraicDouble value) { return value.Value; }
    public double Value;

    public AlgebraicDouble Zero => new AlgebraicDouble(0.0);
    public AlgebraicDouble() { }
    public AlgebraicDouble(double value) { this.Value = value; }
    public AlgebraicDouble Assign(AlgebraicDouble a) { Value = a.Value; return this; }
    public AlgebraicDouble Add(AlgebraicDouble a) { Value += a.Value; return this; }
    public AlgebraicDouble Sub(AlgebraicDouble a) { Value -= a.Value; return this; }
    public AlgebraicDouble Mul(AlgebraicDouble a) { Value *= a.Value; return this; }
    public AlgebraicDouble Div(AlgebraicDouble a) { Value /= a.Value; return this; }
    public AlgebraicDouble Scale(double s) { Value *= s; return this; }
    public AlgebraicDouble AssignInv(AlgebraicDouble a) { Value = 1.0 / a.Value; return this; }
    public AlgebraicDouble AssignNeg(AlgebraicDouble a) { Value = -a.Value; return this; }
    public AlgebraicDouble AssignSin(AlgebraicDouble a) { Value = Math.Sin(a.Value); return this; }
    public AlgebraicDouble AssignCos(AlgebraicDouble a) { Value = Math.Cos(a.Value); return this; }
    public AlgebraicDouble AssignLog(AlgebraicDouble a) { Value = Math.Log(a.Value); return this; }
    public AlgebraicDouble AssignExp(AlgebraicDouble a) { Value = Math.Exp(a.Value); return this; }
    public AlgebraicDouble AssignIntPower(AlgebraicDouble a, int p) { Value = Math.Pow(a.Value, p); return this; }
    public AlgebraicDouble AssignIntRoot(AlgebraicDouble a, int r) { Value = Math.Pow(a.Value, 1.0 / r); return this; }
    public AlgebraicDouble AssignAbs(AlgebraicDouble a) { Value = Math.Abs(a.Value); return this; }
    public AlgebraicDouble AssignSgn(AlgebraicDouble a) { Value = Math.Sign(a.Value); return this; }
    public AlgebraicDouble Clone() { return new AlgebraicDouble(Value); }
  }

  // a simple vector as an algebraic type
  public class DoubleVector : IAlgebraicType<DoubleVector> {
    private double[] arr;


    public double this[int idx] { get { return arr[idx]; } set { arr[idx] = value; } }

    public int Length => arr.Length;

    public DoubleVector(int length) {
      arr = new double[length];
    }

    public DoubleVector() { }

    /// <summary>
    /// 
    /// </summary>
    /// <param name="arr">array is not copied</param>
    public DoubleVector(double[] arr) {
      this.arr = arr;
    }

    public DoubleVector Zero => new DoubleVector(new double[this.Length]); // must return vector of same length as this (therefore Zero is not static)
    public DoubleVector Assign(DoubleVector a) { for (int i = 0; i < arr.Length; ++i) { arr[i] = a.arr[i]; } return this; }
    public DoubleVector Add(DoubleVector a) { for (int i = 0; i < arr.Length; ++i) { arr[i] += a.arr[i]; } return this; }
    public DoubleVector Sub(DoubleVector a) { for (int i = 0; i < arr.Length; ++i) { arr[i] -= a.arr[i]; } return this; }
    public DoubleVector Mul(DoubleVector a) { for (int i = 0; i < arr.Length; ++i) { arr[i] *= a.arr[i]; } return this; }
    public DoubleVector Div(DoubleVector a) { for (int i = 0; i < arr.Length; ++i) { arr[i] /= a.arr[i]; } return this; }
    public DoubleVector AssignNeg(DoubleVector a) { for (int i = 0; i < arr.Length; ++i) { arr[i] = -a.arr[i]; } return this; }
    public DoubleVector AssignInv(DoubleVector a) { for (int i = 0; i < arr.Length; ++i) { arr[i] = 1.0 / a.arr[i]; } return this; }
    public DoubleVector Scale(double s) { for (int i = 0; i < arr.Length; ++i) { arr[i] *= s; } return this; }
    public DoubleVector AssignLog(DoubleVector a) { for (int i = 0; i < arr.Length; ++i) { arr[i] = Math.Log(a.arr[i]); } return this; }
    public DoubleVector AssignSin(DoubleVector a) { for (int i = 0; i < arr.Length; ++i) { arr[i] = Math.Sin(a.arr[i]); } return this; }
    public DoubleVector AssignExp(DoubleVector a) { for (int i = 0; i < arr.Length; ++i) { arr[i] = Math.Exp(a.arr[i]); } return this; }
    public DoubleVector AssignCos(DoubleVector a) { for (int i = 0; i < arr.Length; ++i) { arr[i] = Math.Cos(a.arr[i]); } return this; }
    public DoubleVector AssignIntPower(DoubleVector a, int p) { for (int i = 0; i < arr.Length; ++i) { arr[i] = Math.Pow(a.arr[i], p); } return this; }
    public DoubleVector AssignIntRoot(DoubleVector a, int r) { for (int i = 0; i < arr.Length; ++i) { arr[i] = Math.Pow(a.arr[i], 1.0 / r); } return this; }
    public DoubleVector AssignAbs(DoubleVector a) { for (int i = 0; i < arr.Length; ++i) { arr[i] = Math.Abs(a.arr[i]); } return this; }
    public DoubleVector AssignSgn(DoubleVector a) { for (int i = 0; i < arr.Length; ++i) { arr[i] = Math.Sign(a.arr[i]); } return this; }

    public DoubleVector Clone() {
      var v = new DoubleVector(this.arr.Length);
      Array.Copy(arr, v.arr, v.arr.Length);
      return v;
    }

    public void AssignConstant(double constantValue) {
      for (int i = 0; i < arr.Length; ++i) {
        arr[i] = constantValue;
      }
    }

    public void CopyTo(double[] dest, int idx, int length) {
      Array.Copy(arr, 0, dest, idx, length);
    }

    public void CopyFrom(double[] data, int rowIndex) {
      Array.Copy(data, rowIndex, arr, 0, Math.Min(arr.Length, data.Length - rowIndex));
    }
    public void CopyRowTo(double[,] dest, int row) {
      for (int j = 0; j < arr.Length; ++j) dest[row, j] = arr[j];
    }

    internal void CopyColumnTo(double[,] dest, int column, int row, int len) {
      for (int j = 0; j < len; ++j) dest[row + j, column] = arr[j];
    }
  }

  // vectors of algebraic types
  public sealed class Vector<T> : IAlgebraicType<Vector<T>> where T : IAlgebraicType<T> {
    private T[] elems;

    public T this[int idx] { get { return elems[idx]; } set { elems[idx] = value; } }

    public int Length => elems.Length;


    private Vector() { }

    public Vector(int len) {
      elems = new T[len];
    }

    /// <summary>
    /// 
    /// </summary>
    /// <param name="elems">The array is copied (element-wise clone)</param>
    public Vector(T[] elems) {
      this.elems = new T[elems.Length];
      for (int i = 0; i < elems.Length; ++i) { this.elems[i] = elems[i].Clone(); }
    }

    /// <summary>
    /// 
    /// </summary>
    /// <param name="elems">Array is not copied!</param>
    /// <returns></returns>
    public Vector<T> FromArray(T[] elems) {
      var v = new Vector<T>();
      v.elems = elems;
      return v;
    }

    public void CopyTo(T[] dest) {
      if (dest.Length != elems.Length) throw new InvalidOperationException("arr lengths do not match in Vector<T>.Copy");
      Array.Copy(elems, dest, dest.Length);
    }

    public Vector<T> Clone() {
      return new Vector<T>(elems);
    }

    public Vector<T> Concat(Vector<T> other) {
      var oldLen = Length;
      Array.Resize(ref this.elems, oldLen + other.Length);
      for (int i = oldLen; i < Length; i++) {
        elems[i] = other.elems[i - oldLen].Clone();
      }
      return this;
    }

    public Vector<T> Zero => new Vector<T>(Length);
    public Vector<T> Assign(Vector<T> a) { for (int i = 0; i < elems.Length; ++i) { elems[i].Assign(a.elems[i]); } return this; }
    public Vector<T> Add(Vector<T> a) { for (int i = 0; i < elems.Length; ++i) { elems[i].Add(a.elems[i]); } return this; }
    public Vector<T> Sub(Vector<T> a) { for (int i = 0; i < elems.Length; ++i) { elems[i].Sub(a.elems[i]); } return this; }
    public Vector<T> Mul(Vector<T> a) { for (int i = 0; i < elems.Length; ++i) { elems[i].Mul(a.elems[i]); } return this; }
    public Vector<T> Div(Vector<T> a) { for (int i = 0; i < elems.Length; ++i) { elems[i].Div(a.elems[i]); } return this; }
    public Vector<T> AssignNeg(Vector<T> a) { for (int i = 0; i < elems.Length; ++i) { elems[i].AssignNeg(a.elems[i]); } return this; }
    public Vector<T> Scale(double s) { for (int i = 0; i < elems.Length; ++i) { elems[i].Scale(s); } return this; }
    public Vector<T> Scale(T s) { for (int i = 0; i < elems.Length; ++i) { elems[i].Mul(s); } return this; }
    public Vector<T> AssignInv(Vector<T> a) { for (int i = 0; i < elems.Length; ++i) { elems[i].AssignInv(a.elems[i]); } return this; }
    public Vector<T> AssignLog(Vector<T> a) { for (int i = 0; i < elems.Length; ++i) { elems[i].AssignLog(a.elems[i]); } return this; }
    public Vector<T> AssignExp(Vector<T> a) { for (int i = 0; i < elems.Length; ++i) { elems[i].AssignExp(a.elems[i]); } return this; }
    public Vector<T> AssignSin(Vector<T> a) { for (int i = 0; i < elems.Length; ++i) { elems[i].AssignSin(a.elems[i]); } return this; }
    public Vector<T> AssignCos(Vector<T> a) { for (int i = 0; i < elems.Length; ++i) { elems[i].AssignCos(a.elems[i]); } return this; }
    public Vector<T> AssignIntPower(Vector<T> a, int p) { for (int i = 0; i < elems.Length; ++i) { elems[i].AssignIntPower(a.elems[i], p); } return this; }
    public Vector<T> AssignIntRoot(Vector<T> a, int r) { for (int i = 0; i < elems.Length; ++i) { elems[i].AssignIntRoot(a.elems[i], r); } return this; }
    public Vector<T> AssignAbs(Vector<T> a) { for (int i = 0; i < elems.Length; ++i) { elems[i].AssignAbs(a.elems[i]); } return this; }
    public Vector<T> AssignSgn(Vector<T> a) { for (int i = 0; i < elems.Length; ++i) { elems[i].AssignSgn(a.elems[i]); } return this; }
  }


  /// <summary>
  /// A sparse vector of algebraic types. Elements are accessed via a key of type K
  /// </summary>
  /// <typeparam name="K">Key type</typeparam>
  /// <typeparam name="T">Element type</typeparam>
  public sealed class SparseVector<K, T> : IAlgebraicType<SparseVector<K, T>> where T : IAlgebraicType<T> {

    private Dictionary<K, T> elems;
    public IReadOnlyDictionary<K, T> Elements => elems;


    public SparseVector(SparseVector<K, T> original) {
      elems = original.elems.ToDictionary(kvp => kvp.Key, kvp => kvp.Value.Clone());
    }

    /// <summary>
    /// 
    /// </summary>
    /// <param name="keys"></param>
    /// <param name="values">values are cloned</param>
    public SparseVector(K[] keys, T[] values) {
      if (keys.Length != values.Length) throw new ArgumentException("lengths of keys and values doesn't match in SparseVector");
      elems = new Dictionary<K, T>(keys.Length);
      for (int i = 0; i < keys.Length; ++i) {
        elems.Add(keys[i], values[i].Clone());
      }
    }

    public SparseVector() {
      this.elems = new Dictionary<K, T>();
    }



    private void AssignTransformed(SparseVector<K, T> a, Func<T, T, T> mapAssign) {
      foreach (var kvp in a.elems) {
        if (elems.TryGetValue(kvp.Key, out T value))
          mapAssign(kvp.Value, value);
        else {
          var newValue = kvp.Value.Zero;
          elems.Add(kvp.Key, newValue);
          mapAssign(kvp.Value, newValue);
        }
      }
    }

    public SparseVector<K, T> Zero => new SparseVector<K, T>();

    public SparseVector<K, T> Scale(T s) { foreach (var kvp in elems) { kvp.Value.Mul(s); } return this; }
    public SparseVector<K, T> Scale(double s) { foreach (var kvp in elems) { kvp.Value.Scale(s); } return this; }

    public SparseVector<K, T> Assign(SparseVector<K, T> a) { AssignTransformed(a, (src, dest) => dest.Assign(src)); return this; }
    public SparseVector<K, T> Add(SparseVector<K, T> a) { AssignTransformed(a, (src, dest) => dest.Add(src)); return this; }
    public SparseVector<K, T> Mul(SparseVector<K, T> a) { AssignTransformed(a, (src, dest) => dest.Mul(src)); return this; }
    public SparseVector<K, T> Sub(SparseVector<K, T> a) { AssignTransformed(a, (src, dest) => dest.Sub(src)); return this; }
    public SparseVector<K, T> Div(SparseVector<K, T> a) { AssignTransformed(a, (src, dest) => dest.Div(src)); return this; }
    public SparseVector<K, T> AssignInv(SparseVector<K, T> a) { AssignTransformed(a, (src, dest) => dest.AssignInv(src)); return this; }
    public SparseVector<K, T> AssignNeg(SparseVector<K, T> a) { AssignTransformed(a, (src, dest) => dest.AssignNeg(src)); return this; }
    public SparseVector<K, T> AssignLog(SparseVector<K, T> a) { AssignTransformed(a, (src, dest) => dest.AssignLog(src)); return this; }
    public SparseVector<K, T> AssignExp(SparseVector<K, T> a) { AssignTransformed(a, (src, dest) => dest.AssignExp(src)); return this; }
    public SparseVector<K, T> AssignIntPower(SparseVector<K, T> a, int p) { AssignTransformed(a, (src, dest) => dest.AssignIntPower(src, p)); return this; }
    public SparseVector<K, T> AssignIntRoot(SparseVector<K, T> a, int r) { AssignTransformed(a, (src, dest) => dest.AssignIntRoot(src, r)); return this; }
    public SparseVector<K, T> AssignSin(SparseVector<K, T> a) { AssignTransformed(a, (src, dest) => dest.AssignSin(src)); return this; }
    public SparseVector<K, T> AssignCos(SparseVector<K, T> a) { AssignTransformed(a, (src, dest) => dest.AssignCos(src)); return this; }
    public SparseVector<K, T> AssignAbs(SparseVector<K, T> a) { AssignTransformed(a, (src, dest) => dest.AssignAbs(src)); return this; }
    public SparseVector<K, T> AssignSgn(SparseVector<K, T> a) { AssignTransformed(a, (src, dest) => dest.AssignSgn(src)); return this; }

    public SparseVector<K, T> Clone() {
      return new SparseVector<K, T>(this);
    }
  }

  public class AlgebraicInterval : IAlgebraicType<AlgebraicInterval> {
    private MultivariateDual<AlgebraicDouble> low;
    private MultivariateDual<AlgebraicDouble> high;

    public MultivariateDual<AlgebraicDouble> LowerBound => low.Clone();
    public MultivariateDual<AlgebraicDouble> UpperBound => high.Clone();


    public AlgebraicInterval() : this(double.NegativeInfinity, double.PositiveInfinity) { }

    public AlgebraicInterval(MultivariateDual<AlgebraicDouble> low, MultivariateDual<AlgebraicDouble> high) {
      this.low = low.Clone();
      this.high = high.Clone();
    }

    public AlgebraicInterval(double low, double high) {
      this.low = new MultivariateDual<AlgebraicDouble>(new AlgebraicDouble(low));
      this.high = new MultivariateDual<AlgebraicDouble>(new AlgebraicDouble(high));
    }

    public AlgebraicInterval Zero => new AlgebraicInterval(0.0, 0.0);
    public AlgebraicInterval Add(AlgebraicInterval a) {
      low.Add(a.low);
      high.Add(a.high);
      return this;
    }

    public AlgebraicInterval Mul(AlgebraicInterval a) {
      var v1 = low.Clone().Mul(a.low);
      var v2 = low.Clone().Mul(a.high);
      var v3 = high.Clone().Mul(a.low);
      var v4 = high.Clone().Mul(a.high);

      low = Algebraic.Min(Algebraic.Min(v1, v2), Algebraic.Min(v3, v4));
      high = Algebraic.Max(Algebraic.Max(v1, v2), Algebraic.Max(v3, v4));
      return this;
    }

    public AlgebraicInterval Assign(AlgebraicInterval a) {
      low = a.low;
      high = a.high;
      return this;
    }

    public AlgebraicInterval AssignCos(AlgebraicInterval a) {
      return AssignSin(a.Clone().Sub(new AlgebraicInterval(Math.PI / 2, Math.PI / 2)));
    }

    public AlgebraicInterval Div(AlgebraicInterval a) {
      if (a.Contains(0.0)) {
        if (a.low.Value.Value.IsAlmost(0.0) && a.high.Value.Value.IsAlmost(0.0)) {
          low = new MultivariateDual<AlgebraicDouble>(double.NegativeInfinity);
          high = new MultivariateDual<AlgebraicDouble>(double.PositiveInfinity);
        } else if (a.low.Value.Value.IsAlmost(0.0))
          Mul(new AlgebraicInterval(a.Clone().high.Inv(), new MultivariateDual<AlgebraicDouble>(double.PositiveInfinity)));
        else
          Mul(new AlgebraicInterval(new MultivariateDual<AlgebraicDouble>(double.NegativeInfinity), a.low.Clone().Inv()));
      } else {
        Mul(new AlgebraicInterval(a.high.Clone().Inv(), a.low.Clone().Inv())); // inverting leads to inverse roles of high and low
      }
      return this;
    }

    public AlgebraicInterval AssignExp(AlgebraicInterval a) {
      low.AssignExp(a.low);
      high.AssignExp(a.high);
      return this;
    }

    public AlgebraicInterval AssignIntPower(AlgebraicInterval a, int p) {
      if (p == 0) {
        // => 1
        low = new MultivariateDual<AlgebraicDouble>(1.0);
        high = new MultivariateDual<AlgebraicDouble>(1.0);
        return this;
      }
      if (p == 1) return this;

      if (p < 0) {  // x^-3 == 1/(x^3)
        AssignIntPower(a, -p);
        return AssignInv(this);
      } else {
        // p is even => interval must be positive
        if (p % 2 == 0) {
          if (a.Contains(0.0)) {
            low = new MultivariateDual<AlgebraicDouble>(0.0);
            high = Algebraic.Max(low.Clone().IntPower(p), high.Clone().IntPower(p));
          } else {
            var lowPower = low.Clone().IntPower(p);
            var highPower = high.Clone().IntPower(p);
            low = Algebraic.Min(lowPower, highPower);
            high = Algebraic.Max(lowPower, highPower);
          }
        } else {
          // p is uneven
          var lowPower = low.Clone().IntPower(p);
          var highPower = high.Clone().IntPower(p);
          low = Algebraic.Min(lowPower, highPower);
          high = Algebraic.Max(lowPower, highPower);
        }
        return this;
      }
    }

    public AlgebraicInterval AssignIntRoot(AlgebraicInterval a, int r) {
      if (r == 0) { low = new MultivariateDual<AlgebraicDouble>(double.NaN); high = new MultivariateDual<AlgebraicDouble>(double.NaN); return this; }
      if (r == 1) return this;
      if (r < 0) {
        // x^ (-1/2) = 1 / (x^(1/2))
        AssignIntRoot(a, -r);
        return AssignInv(this);
      } else {
        // root only defined for positive arguments
        if (a.LowerBound.Value.Value < 0) {
          low = new MultivariateDual<AlgebraicDouble>(double.NaN);
          high = new MultivariateDual<AlgebraicDouble>(double.NaN);
          return this;
        } else {
          low.AssignIntRoot(a.low, r);
          high.AssignIntRoot(a.high, r);
          return this;
        }
      }
    }

    public AlgebraicInterval AssignInv(AlgebraicInterval a) {
      low = new MultivariateDual<AlgebraicDouble>(1.0);
      high = new MultivariateDual<AlgebraicDouble>(1.0);
      return Div(a);
    }

    public AlgebraicInterval AssignLog(AlgebraicInterval a) {
      low.AssignLog(a.low);
      high.AssignLog(a.high);
      return this;
    }

    public AlgebraicInterval AssignNeg(AlgebraicInterval a) {
      low.AssignNeg(a.high);
      high.AssignNeg(a.low);
      return this;
    }

    public AlgebraicInterval Scale(double s) {
      low.Scale(s);
      high.Scale(s);
      if (s < 0) {
        var t = low;
        low = high;
        high = t;
      }
      return this;
    }

    public AlgebraicInterval AssignSin(AlgebraicInterval a) {
      if (Math.Abs(a.UpperBound.Value.Value - a.LowerBound.Value.Value) >= Math.PI * 2) {
        low = new MultivariateDual<AlgebraicDouble>(-1.0);
        high = new MultivariateDual<AlgebraicDouble>(1.0);
      }

      //divide the interval by PI/2 so that the optima lie at x element of N (0,1,2,3,4,...)
      double Pihalf = Math.PI / 2;
      var scaled = this.Clone().Scale(1.0 / Pihalf);
      //move to positive scale
      if (scaled.LowerBound.Value.Value < 0) {
        int periodsToMove = Math.Abs((int)scaled.LowerBound.Value.Value / 4) + 1;
        scaled.Add(new AlgebraicInterval(periodsToMove * 4, periodsToMove * 4));
      }

      double scaledLowerBound = scaled.LowerBound.Value.Value % 4.0;
      double scaledUpperBound = scaled.UpperBound.Value.Value % 4.0;
      if (scaledUpperBound < scaledLowerBound) scaledUpperBound += 4.0;
      List<double> sinValues = new List<double>();
      sinValues.Add(Math.Sin(scaledLowerBound * Pihalf));
      sinValues.Add(Math.Sin(scaledUpperBound * Pihalf));

      int startValue = (int)Math.Ceiling(scaledLowerBound);
      while (startValue < scaledUpperBound) {
        sinValues.Add(Math.Sin(startValue * Pihalf));
        startValue += 1;
      }

      low = new MultivariateDual<AlgebraicDouble>(sinValues.Min());
      high = new MultivariateDual<AlgebraicDouble>(sinValues.Max());
      return this;
    }

    public AlgebraicInterval Sub(AlgebraicInterval a) {
      // [x1,x2] − [y1,y2] = [x1 − y2,x2 − y1]
      low.Sub(a.high);
      high.Sub(a.low);
      return this;
    }

    public AlgebraicInterval Clone() {
      return new AlgebraicInterval(low, high);
    }

    public bool Contains(double val) {
      return LowerBound.Value.Value <= val && val <= UpperBound.Value.Value;
    }

    public AlgebraicInterval AssignAbs(AlgebraicInterval a) {
      if (a.Contains(0.0)) {
        var abslow = a.low.Clone().Abs();
        var abshigh = a.high.Clone().Abs();
        a.high.Assign(Algebraic.Max(abslow, abshigh));
        a.low.Assign(new MultivariateDual<AlgebraicDouble>(0.0)); // lost gradient for lower bound
      } else {
        var abslow = a.low.Clone().Abs();
        var abshigh = a.high.Clone().Abs();
        a.low.Assign(Algebraic.Min(abslow, abshigh));
        a.high.Assign(Algebraic.Max(abslow, abshigh));
      }
      return this;
    }

    public AlgebraicInterval AssignSgn(AlgebraicInterval a) {
      low.AssignSgn(a.low);
      high.AssignSgn(a.high);
      return this;
    }
  }


  public class Dual<V> : IAlgebraicType<Dual<V>>
    where V : IAlgebraicType<V> {
    private V v;
    public V Value => v;

    private V dv;
    public V Derivative => dv;

    public Dual(V v, V dv) { this.v = v; this.dv = dv; }

    public Dual<V> Zero => new Dual<V>(Value.Zero, Derivative.Zero);

    public Dual<V> Assign(Dual<V> a) { v.Assign(a.v); dv.Assign(a.dv); return this; }
    public Dual<V> Scale(double s) { v.Scale(s); dv.Scale(s); return this; }
    public Dual<V> Add(Dual<V> a) { v.Add(a.v); dv.Add(a.dv); return this; }
    public Dual<V> Sub(Dual<V> a) { v.Sub(a.v); dv.Sub(a.dv); return this; }
    public Dual<V> AssignNeg(Dual<V> a) { v.AssignNeg(a.v); dv.AssignNeg(a.dv); return this; }
    public Dual<V> AssignInv(Dual<V> a) { v.AssignInv(a.v); dv.AssignNeg(a.dv).Mul(v).Mul(v); return this; } // (1/f(x))' = - f(x)' / f(x)^2

    // (a(x) * b(x))' = b(x)*a(x)' + b(x)'*a(x);
    public Dual<V> Mul(Dual<V> a) {
      var t1 = a.dv.Clone().Mul(v);
      var t2 = dv.Clone().Mul(a.v);
      dv.Assign(t1).Add(t2);

      v.Mul(a.v);
      return this;
    }
    public Dual<V> Div(Dual<V> a) { Mul(a.Inv()); return this; }

    public Dual<V> AssignExp(Dual<V> a) { v.AssignExp(a.v); dv.Assign(a.dv).Mul(v); return this; } // exp(f(x)) = exp(f(x))*f(x)'
    public Dual<V> AssignLog(Dual<V> a) { v.AssignLog(a.v); dv.Assign(a.dv).Div(a.v); return this; }     // log(x)' = 1/f(x) * f(x)' 

    public Dual<V> AssignIntPower(Dual<V> a, int p) { v.AssignIntPower(a.v, p); dv.Assign(a.dv).Scale(p).Mul(a.v.Clone().IntPower(p - 1)); return this; }
    public Dual<V> AssignIntRoot(Dual<V> a, int r) { v.AssignIntRoot(a.v, r); dv.Assign(a.dv).Scale(1.0 / r).Mul(a.v.IntRoot(r - 1)); return this; }

    public Dual<V> AssignSin(Dual<V> a) { v.AssignSin(a.v); dv.Assign(a.dv).Mul(a.v.Clone().Cos()); return this; }
    public Dual<V> AssignCos(Dual<V> a) { v.AssignCos(a.v); dv.AssignNeg(a.dv).Mul(a.v.Clone().Sin()); return this; }

    public Dual<V> AssignAbs(Dual<V> a) { v.AssignAbs(a.v); dv.Assign(a.dv).Mul(a.v.Clone().Sgn()); return this; }       // abs(f(x))' = f(x)*f'(x) / |f(x)|      
    public Dual<V> AssignSgn(Dual<V> a) { v.AssignSgn(a.v); dv.Assign(a.dv.Zero); return this; }

    public Dual<V> Clone() { return new Dual<V>(v.Clone(), dv.Clone()); }

  }

  /// <summary>
  /// An algebraic type which has a value as well as the partial derivatives of the value over multiple variables.
  /// </summary>
  /// <typeparam name="V"></typeparam>
  public class MultivariateDual<V> : IAlgebraicType<MultivariateDual<V>> where V : IAlgebraicType<V>, new() {
    private V v;
    public V Value => v;

    private SparseVector<object, V> dv;
    public SparseVector<object, V> Gradient => dv; // <key,value> partial derivative identified via the key

    private MultivariateDual(MultivariateDual<V> orig) { this.v = orig.v.Clone(); this.dv = orig.dv.Clone(); }

    /// <summary>
    /// Constructor without partial derivative
    /// </summary>
    /// <param name="v"></param>
    public MultivariateDual(V v) { this.v = v.Clone(); this.dv = new SparseVector<object, V>(); }

    /// <summary>
    /// Constructor for multiple partial derivatives
    /// </summary>
    /// <param name="v"></param>
    /// <param name="keys"></param>
    /// <param name="dv"></param>
    public MultivariateDual(V v, object[] keys, V[] dv) { this.v = v.Clone(); this.dv = new SparseVector<object, V>(keys, dv); }

    /// <summary>
    /// Constructor for a single partial derivative 
    /// </summary>
    /// <param name="v"></param>
    /// <param name="key"></param>
    /// <param name="dv"></param>
    public MultivariateDual(V v, object key, V dv) { this.v = v.Clone(); this.dv = new SparseVector<object, V>(new[] { key }, new[] { dv }); }

    /// <summary>
    /// Constructor with a given value and gradient. For internal use.
    /// </summary>
    /// <param name="v">The value (not cloned).</param>
    /// <param name="gradient">The gradient (not cloned).</param>
    internal MultivariateDual(V v, SparseVector<object, V> gradient) { this.v = v; this.dv = gradient; }

    public MultivariateDual<V> Clone() { return new MultivariateDual<V>(this); }

    public MultivariateDual<V> Zero => new MultivariateDual<V>(Value.Zero, Gradient.Zero);

    public MultivariateDual<V> Scale(double s) { v.Scale(s); dv.Scale(s); return this; }

    public MultivariateDual<V> Add(MultivariateDual<V> a) { v.Add(a.v); dv.Add(a.dv); return this; }
    public MultivariateDual<V> Sub(MultivariateDual<V> a) { v.Sub(a.v); dv.Sub(a.dv); return this; }
    public MultivariateDual<V> Assign(MultivariateDual<V> a) { v.Assign(a.v); dv.Assign(a.dv); return this; }
    public MultivariateDual<V> Mul(MultivariateDual<V> a) {
      // (a(x) * b(x))' = b(x)*a(x)' + b(x)'*a(x);
      var t1 = a.dv.Clone().Scale(v);
      var t2 = dv.Clone().Scale(a.v);
      dv.Assign(t1).Add(t2);

      v.Mul(a.v);
      return this;
    }
    public MultivariateDual<V> Div(MultivariateDual<V> a) { v.Div(a.v); dv.Mul(a.dv.Inv()); return this; }
    public MultivariateDual<V> AssignNeg(MultivariateDual<V> a) { v.AssignNeg(a.v); dv.AssignNeg(a.dv); return this; }
    public MultivariateDual<V> AssignInv(MultivariateDual<V> a) { v.AssignInv(a.v); dv.AssignNeg(a.dv).Scale(v).Scale(v); return this; }   // (1/f(x))' = - f(x)' / f(x)^2

    public MultivariateDual<V> AssignSin(MultivariateDual<V> a) { v.AssignSin(a.v); dv.Assign(a.dv).Scale(a.v.Clone().Cos()); return this; }
    public MultivariateDual<V> AssignCos(MultivariateDual<V> a) { v.AssignCos(a.v); dv.AssignNeg(a.dv).Scale(a.v.Clone().Sin()); return this; }

    public MultivariateDual<V> AssignIntPower(MultivariateDual<V> a, int p) { v.AssignIntPower(a.v, p); dv.Assign(a.dv).Scale(p).Scale(a.v.Clone().IntPower(p - 1)); return this; }
    public MultivariateDual<V> AssignIntRoot(MultivariateDual<V> a, int r) { v.AssignIntRoot(a.v, r); dv.Assign(a.dv).Scale(1.0 / r).Scale(a.v.IntRoot(r - 1)); return this; }

    public MultivariateDual<V> AssignExp(MultivariateDual<V> a) { v.AssignExp(a.v); dv.Assign(a.dv).Scale(v); return this; } // exp(f(x)) = exp(f(x))*f(x)'     
    public MultivariateDual<V> AssignLog(MultivariateDual<V> a) { v.AssignLog(a.v); dv.Assign(a.dv).Scale(a.v.Clone().Inv()); return this; }     // log(x)' = 1/f(x) * f(x)' 

    public MultivariateDual<V> AssignAbs(MultivariateDual<V> a) { v.AssignAbs(a.v); dv.Assign(a.dv).Scale(a.v.Clone().Sgn()); return this; }      // abs(f(x))' = f(x)*f'(x) / |f(x)|  doesn't work for intervals      
    public MultivariateDual<V> AssignSgn(MultivariateDual<V> a) { v.AssignSgn(a.v); dv = a.dv.Zero; return this; } // sign(f(x))' = 0;     
  }
}