#include <NTL/FFT.h>

#include <NTL/new.h>

NTL_START_IMPL

long NumFFTPrimes = 0;

long *FFTPrime = 0;
long **RootTable = 0;
long **RootInvTable = 0;
long **TwoInvTable = 0;
double *FFTPrimeInv = 0;

static
long IsFFTPrime(long n, long& w)
{
   long  m, x, y, z;
   long j, k;

   if (n % 3 == 0) return 0;

   if (n % 5 == 0) return 0;

   if (n % 7 == 0) return 0;

   m = n - 1;
   k = 0;
   while ((m & 1) == 0) {
      m = m >> 1;
      k++;
   }

   for (;;) {
      x = RandomBnd(n);

      if (x == 0) continue;
      z = PowerMod(x, m, n);
      if (z == 1) continue;

      x = z;
      j = 0;
      do {
         y = z;
         z = MulMod(y, y, n);
         j++;
      } while (j != k && z != 1);

      if (z != 1 || y !=  n-1) return 0;

      if (j == k)
         break;
   }

   /* x^{2^k} = 1 mod n, x^{2^{k-1}} = -1 mod n */

   long TrialBound;

   TrialBound = m >> k;
   if (TrialBound > 0) {
      if (!ProbPrime(n, 5)) return 0;

      /* we have to do trial division by special numbers */

      TrialBound = SqrRoot(TrialBound);

      long a, b;

      for (a = 1; a <= TrialBound; a++) {
         b = (a << k) + 1;
         if (n % b == 0) return 0;
      }
   }

   /* n is an FFT prime */

   for (j = NTL_FFTMaxRoot; j < k; j++)
      x = MulMod(x, x, n);

   w = x;
   return 1;
}

static
void NextFFTPrime(long& q, long& w)
{
   static long m = NTL_FFTMaxRootBnd + 1;
   static long k = 0;

   long t, cand;

   for (;;) {
      if (k == 0) {
         m--;
         if (m < 5) Error("ran out of FFT primes");
         k = 1L << (NTL_SP_NBITS-m-2);
      }

      k--;

      cand = (1L << (NTL_SP_NBITS-1)) + (k << (m+1)) + (1L << m) + 1;

      if (!IsFFTPrime(cand, t)) continue;
      q = cand;
      w = t;
      return;
   }
}

long CalcMaxRoot(long p)
{
   p = p-1;
   long k = 0;
   while ((p & 1) == 0) {
      p = p >> 1;
      k++;
   }

   if (k > NTL_FFTMaxRoot)
      return NTL_FFTMaxRoot;
   else
      return k;
}

void UseFFTPrime(long index)
{
   if (index < 0 || index > NumFFTPrimes)
      Error("invalid FFT prime index");

   if (index < NumFFTPrimes) return;

   long q, w;

   NextFFTPrime(q, w);

   long mr = CalcMaxRoot(q);

   // tables are allocated in increments of 100

   if (index == 0) {
      FFTPrime = (long *) NTL_MALLOC(100, sizeof(long), 0);
      RootTable = (long **) NTL_MALLOC(100, sizeof(long *), 0);
      RootInvTable = (long **) NTL_MALLOC(100, sizeof(long *), 0);
      TwoInvTable = (long **) NTL_MALLOC(100, sizeof(long *), 0);
      FFTPrimeInv = (double *) NTL_MALLOC(100, sizeof(double), 0);
   }
   else if ((index % 100) == 0) {
      FFTPrime = (long *) NTL_REALLOC(FFTPrime, index+100, sizeof(long), 0);
      RootTable = (long **)
                  NTL_REALLOC(RootTable, index+100, sizeof(long *), 0);
      RootInvTable = (long **)
                     NTL_REALLOC(RootInvTable, index+100, sizeof(long *), 0);
      TwoInvTable = (long **)
                    NTL_REALLOC(TwoInvTable, index+100, sizeof(long *), 0);
      FFTPrimeInv = (double *)
                    NTL_REALLOC(FFTPrimeInv, index+100, sizeof(double), 0);
   }

   if (!FFTPrime || !RootTable || !RootInvTable || !TwoInvTable ||
       !FFTPrimeInv)
      Error("out of space");

   FFTPrime[index] = q;

   long *rt, *rit, *tit;

   if (!(rt = RootTable[index] = (long*) NTL_MALLOC(mr+1, sizeof(long), 0)))
      Error("out of space");
   if (!(rit = RootInvTable[index] = (long*) NTL_MALLOC(mr+1, sizeof(long), 0)))
      Error("out of space");
   if (!(tit = TwoInvTable[index] = (long*) NTL_MALLOC(mr+1, sizeof(long), 0)))
      Error("out of space");

   long j;
   long t;

   rt[mr] = w;
   for (j = mr-1; j >= 0; j--)
      rt[j] = MulMod(rt[j+1], rt[j+1], q);

   rit[mr] = InvMod(w, q);
   for (j = mr-1; j >= 0; j--)
      rit[j] = MulMod(rit[j+1], rit[j+1], q);

   t = InvMod(2, q);
   tit[0] = 1;
   for (j = 1; j <= mr; j++)
      tit[j] = MulMod(tit[j-1], t, q);

   FFTPrimeInv[index] = 1/double(q);

   NumFFTPrimes++;
}

static
long RevInc(long a, long k)
{
   long j, m;

   j = k;
   m = 1L << (k-1);

   while (j && (m & a)) {
      a ^= m;
      m >>= 1;
      j--;
   }
   if (j) a ^= m;
   return a;
}

static
void BitReverseCopy(long *A, const long *a, long k)
{
   static long* mem[NTL_FFTMaxRoot+1];

   long n = 1L << k;
   long* rev;
   long i, j;

   rev = mem[k];
   if (!rev) {
      rev = mem[k] = (long *) NTL_MALLOC(n, sizeof(long), 0);
      if (!rev) Error("out of memory in BitReverseCopy");
      for (i = 0, j = 0; i < n; i++, j = RevInc(j, k))
         rev[i] = j;
   }

   for (i = 0; i < n; i++)
      A[rev[i]] = a[i];
}

/*
 * Our FFT is based on the routine in Cormen, Leiserson, Rivest, and Stein.
 * For very large inputs, it should be relatively cache friendly.
 * The inner loop has been unrolled and pipelined, to exploit any
 * low-level parallelism in the machine.
 */

void FFT(long* A, const long* a, long k, long q, const long* root)

// performs a 2^k-point convolution modulo q

{
   if (k <= 1) {
      if (k == 0) {
	 A[0] = a[0];
	 return;
      }
      if (k == 1) {
	 A[0] = AddMod(a[0], a[1], q);
	 A[1] = SubMod(a[0], a[1], q);
	 return;
      }
   }

   // assume k > 1

   static long tab_size = 0;
   static long *wtab = 0;
   static mulmod_precon_t *wqinvtab = 0;

   if (!tab_size) {
      tab_size = k;

      wtab = (long *) NTL_MALLOC(1L << (k-2), sizeof(long), 0);
      wqinvtab = (mulmod_precon_t *)
                 NTL_MALLOC(1L << (k-2), sizeof(mulmod_precon_t), 0);
      if (!wtab || !wqinvtab) Error("out of space");
   }
   else if (tab_size < k) {
      tab_size = k;

      wtab = (long *) NTL_REALLOC(wtab, 1L << (k-2), sizeof(long), 0);
      wqinvtab = (mulmod_precon_t *)
                 NTL_REALLOC(wqinvtab, 1L << (k-2), sizeof(mulmod_precon_t), 0);
      if (!wtab || !wqinvtab) Error("out of space");
   }

   double qinv = 1/((double) q);

   wtab[0] = 1;
   wqinvtab[0] = PrepMulModPrecon(1, q, qinv);

   BitReverseCopy(A, a, k);

   long n = 1L << k;

   long s, m, m_half, m_fourth, i, j, t, u, t1, u1, uu, uu1, tt, tt1;

   long w;
   mulmod_precon_t wqinv;

   // s = 1

   for (i = 0; i < n; i += 2) {
      t = A[i + 1];
      u = A[i];
      A[i] = AddMod(u, t, q);
      A[i+1] = SubMod(u, t, q);
   }

   for (s = 2; s < k; s++) {
      m = 1L << s;
      m_half = 1L << (s-1);
      m_fourth = 1L << (s-2);

      // prepare wtab...

      w = root[s];
      wqinv = PrepMulModPrecon(w, q, qinv);

      for (i = m_half-1, j = m_fourth-1; i >= 0; i -= 2, j--) {
         wtab[i-1] = wtab[j];
         wqinvtab[i-1] = wqinvtab[j];
         wtab[i] = MulModPrecon(wtab[i-1], w, q, wqinv);
         wqinvtab[i] = PrepMulModPrecon(wtab[i], q, qinv);
      }

      for (i = 0; i < n; i+= m) {

         t = A[i + m_half];
         u = A[i];
         t1 = MulModPrecon(A[i + 1+ m_half], w, q, wqinv);
         u1 = A[i+1];

         for (j = 0; j < m_half-2; j += 2) {
            tt = MulModPrecon(A[i + j + 2 + m_half], wtab[j+2], q, wqinvtab[j+2]);
            uu = A[i + j + 2];

            tt1 = MulModPrecon(A[i + j + 3+ m_half], wtab[j+3], q, wqinvtab[j+3]);
            uu1 = A[i + j + 3];

            A[i + j] = AddMod(u, t, q);
            A[i + j + m_half] = SubMod(u, t, q);
            A[i + j + 1] = AddMod(u1, t1, q);
            A[i + j + 1 + m_half] = SubMod(u1, t1, q);
            t = tt;
            t1 = tt1;
            u = uu;
            u1 = uu1;
         }

         A[i + j] = AddMod(u, t, q);
         A[i + j + m_half] = SubMod(u, t, q);
         A[i + j + 1] = AddMod(u1, t1, q);
         A[i + j + 1 + m_half] = SubMod(u1, t1, q);

      }
   }

   // s == k...special case

   m = 1L << s;
   m_half = 1L << (s-1);
   m_fourth = 1L << (s-2);

   w = root[s];
   wqinv = PrepMulModPrecon(w, q, qinv);

   // j = 0, 1

   t = A[m_half];
   u = A[0];
   t1 = MulModPrecon(A[1+ m_half], w, q, wqinv);
   u1 = A[1];

   A[0] = AddMod(u, t, q);
   A[m_half] = SubMod(u, t, q);
   A[1] = AddMod(u1, t1, q);
   A[1 + m_half] = SubMod(u1, t1, q);

   for (j = 2; j < m_half; j += 2) {
      t = MulModPrecon(A[j + m_half], wtab[j >> 1], q, wqinvtab[j >> 1]);
      u = A[j];
      t1 = MulModPrecon(A[j + 1+ m_half], wtab[j >> 1], q,
                        wqinvtab[j >> 1]);
      t1 = MulModPrecon(t1, w, q, wqinv);
      u1 = A[j + 1];

      A[j] = AddMod(u, t, q);
      A[j + m_half] = SubMod(u, t, q);
      A[j + 1] = AddMod(u1, t1, q);
      A[j + 1 + m_half] = SubMod(u1, t1, q);

   }
}

NTL_END_IMPL


/*********************                                                        */
/*! \file sets_translate.cpp
 ** \verbatim
 ** Original author: Kshitij Bansal
 ** Major contributors: none
 ** Minor contributors (to current version): none
 ** This file is part of the CVC4 project.
 ** Copyright (c) 2009-2014  New York University and The University of Iowa
 ** See the file COPYING in the top-level source directory for licensing
 ** information.\endverbatim
 **
 ** \brief [[ Add one-line brief description here ]]
 **
 ** [[ Add lengthier description here ]]
 ** \todo document this file
 **/

#include <string>
#include <iostream>
#include <typeinfo>
#include <cassert>
#include <vector>
#include <boost/algorithm/string.hpp> // include Boost, a C++ library

#include "options/options.h"
#include "expr/expr.h"
#include "theory/logic_info.h"
#include "expr/command.h"
#include "parser/parser.h"
#include "parser/parser_builder.h"

using namespace std;
using namespace CVC4;
using namespace CVC4::parser;
using namespace CVC4::options;

bool nonsense(char c) { return !isalnum(c); }

#ifdef ENABLE_AXIOMS
const bool enableAxioms = true;
#else
const bool enableAxioms = false;
#endif

string setaxioms[] = {
  "(declare-fun memberHOLDA (HOLDB (Set HOLDB)) Bool)",
  "",
  "(declare-fun unionHOLDA ((Set HOLDB) (Set HOLDB)) (Set HOLDB))",
  "(assert (forall ((?X (Set HOLDB)) (?Y (Set HOLDB)) (?x HOLDB))",
  "                (= (memberHOLDA ?x (unionHOLDA ?X ?Y))",
  "                     (or (memberHOLDA ?x ?X) (memberHOLDA ?x ?Y))",
  "                ) ) )",
  "",
  "",
  "(declare-fun intersectionHOLDA ((Set HOLDB) (Set HOLDB)) (Set HOLDB))",
  "(assert (forall ((?X (Set HOLDB)) (?Y (Set HOLDB)) (?x HOLDB))",
  "                (= (memberHOLDA ?x (intersectionHOLDA ?X ?Y))",
  "                     (and (memberHOLDA ?x ?X) (memberHOLDA ?x ?Y))",
  "                ) ) )",
  "",
  "(declare-fun setminusHOLDA ((Set HOLDB) (Set HOLDB)) (Set HOLDB))",
  "(assert (forall ((?X (Set HOLDB)) (?Y (Set HOLDB)) (?x HOLDB))",
  "                (= (memberHOLDA ?x (setminusHOLDA ?X ?Y))",
  "                     (and (memberHOLDA ?x ?X) (not (memberHOLDA ?x ?Y)))",
  "                ) ) )",
  "",
  "(declare-fun singletonHOLDA (HOLDB) (Set HOLDB))",
  "(assert (forall ((?x HOLDB) (?y HOLDB))",
  "                (= (memberHOLDA ?x (singletonHOLDA ?y))",
  "                     (= ?x ?y)",
  "                ) ) )",
  "",
  "(declare-fun emptysetHOLDA () (Set HOLDB))",
  "(assert (forall ((?x HOLDB)) (not (memberHOLDA ?x emptysetHOLDA)) ) )",
  "",
  "(define-fun subsetHOLDA ((X (Set HOLDB)) (Y (Set HOLDB))) Bool (= (unionHOLDA X Y) Y))",
  ""
};

class Mapper {
  set< Type > setTypes;
  map< Type, Type > mapTypes;
  map< pair<Type, Kind>, Expr > setoperators;
  hash_map< Expr, Expr, ExprHashFunction > substitutions;
  ostringstream sout;
  ExprManager* em;
  int depth;

  Expr add(SetType t, Expr e) {

    if(setTypes.find(t) == setTypes.end() ) {
      // mark as processed
      setTypes.insert(t);

      Type elementType = t.getElementType();
      string elementTypeAsString = elementType.toString();
      remove_if(elementTypeAsString.begin(), elementTypeAsString.end(), nonsense);

      // define-sort
      ostringstream oss_name;
      oss_name << Expr::setlanguage(language::output::LANG_SMTLIB_V2)
               << "(Set " << elementType << ")";
      string name = oss_name.str();
      Type newt = em->mkArrayType(t.getElementType(), em->booleanType());
      mapTypes[t] = newt;

      // diffent types
      vector<Type> t_t;
      t_t.push_back(t);
      t_t.push_back(t);
      vector<Type> elet_t;
      elet_t.push_back(elementType);
      elet_t.push_back(t);

      if(!enableAxioms)
        sout << "(define-fun emptyset" << elementTypeAsString << "    "
             << " ()"
             << " " << name
             << " ( (as const " << name << ") false ) )" << endl;
      setoperators[ make_pair(t, kind::EMPTYSET) ] =
        em->mkVar( std::string("emptyset") + elementTypeAsString,
                   t);

      if(!enableAxioms)
        sout << "(define-fun singleton" << elementTypeAsString << "     "
             << " ( (x " << elementType << ") )"
             << " " << name << ""
             << " (store emptyset" << elementTypeAsString << " x true) )" << endl;
      setoperators[ make_pair(t, kind::SINGLETON) ] =
        em->mkVar( std::string("singleton") + elementTypeAsString,
                   em->mkFunctionType( elementType, t ) );

      if(!enableAxioms)
        sout << "(define-fun union" << elementTypeAsString << "       "
             << " ( (s1 " << name << ") (s2 " << name << ") )"
             << " " << name << ""
             << " ((_ map or) s1 s2))" << endl;
      setoperators[ make_pair(t, kind::UNION) ] =
        em->mkVar( std::string("union") + elementTypeAsString,
                   em->mkFunctionType( t_t, t ) );

      if(!enableAxioms)
        sout << "(define-fun intersection" << elementTypeAsString << ""
             << " ( (s1 " << name << ") (s2 " << name << ") )"
             << " " << name << ""
             << " ((_ map and) s1 s2))" << endl;
      setoperators[ make_pair(t, kind::INTERSECTION) ] =
        em->mkVar( std::string("intersection") + elementTypeAsString,
                   em->mkFunctionType( t_t, t ) );

      if(!enableAxioms)
        sout << "(define-fun setminus" << elementTypeAsString << "    "
             << " ( (s1 " << name << ") (s2 " << name << ") )"
             << " " << name << ""
             << " (intersection" << elementTypeAsString << " s1 ((_ map not) s2)))" << endl;
      setoperators[ make_pair(t, kind::SETMINUS) ] =
        em->mkVar( std::string("setminus") + elementTypeAsString,
                   em->mkFunctionType( t_t, t ) );

      if(!enableAxioms)
        sout << "(define-fun in" << elementTypeAsString << "          "
             << " ( (x " << elementType << ")" << " (s " << name << "))"
             << " Bool"
             << " (select s x) )" << endl;
      setoperators[ make_pair(t, kind::MEMBER) ] =
        em->mkVar( std::string("member") + elementTypeAsString,
                   em->mkPredicateType( elet_t ) );

      if(!enableAxioms)
        sout << "(define-fun subset" << elementTypeAsString << "    "
             << " ( (s1 " << name << ") (s2 " << name << ") )"
             << " Bool"
             <<" (= emptyset" << elementTypeAsString << " (setminus" << elementTypeAsString << " s1 s2)) )" << endl;
      setoperators[ make_pair(t, kind::SUBSET) ] =
        em->mkVar( std::string("subset") + elementTypeAsString,
                   em->mkPredicateType( t_t ) );

      if(enableAxioms) {
        int N = sizeof(setaxioms) / sizeof(setaxioms[0]);
        for(int i = 0; i < N; ++i) {
          string s = setaxioms[i];
          ostringstream oss; oss << Expr::setlanguage(language::output::LANG_SMTLIB_V2) << elementType;
          boost::replace_all(s, "HOLDA", elementTypeAsString);
          boost::replace_all(s, "HOLDB", oss.str());
          if( s == "" ) continue;
          sout << s << endl;
        }
      }

    }
    Expr ret;
    if(e.getKind() == kind::EMPTYSET) {
      ret = setoperators[ make_pair(t, e.getKind()) ];
    } else {
      vector<Expr> children = e.getChildren();
      children.insert(children.begin(), setoperators[ make_pair(t, e.getKind()) ]);
      ret = em->mkExpr(kind::APPLY, children);
    }
    // cout << "returning " << ret  << endl;
    return ret;
  }

public:
  Mapper(ExprManager* e) : em(e),depth(0) {
    sout << Expr::setlanguage(language::output::LANG_SMTLIB_V2);
  }

  void defineSetSort() {
    if(setTypes.empty()) {
      cout << "(define-sort Set (X) (Array X Bool) )" << endl;
    }
  }

  Expr collectSortsExpr(Expr e)
  {
    if(substitutions.find(e) != substitutions.end()) {
      return substitutions[e];
    }
    ++depth;
    Expr old_e = e;
    for(unsigned i = 0; i < e.getNumChildren(); ++i) {
      collectSortsExpr(e[i]);
    }
    e = e.substitute(substitutions);
    // cout << "[debug] " << e << " " << e.getKind() << " " << theory::kindToTheoryId(e.getKind()) << endl;
    if(theory::kindToTheoryId(e.getKind()) == theory::THEORY_SETS) {
      SetType t = SetType(e.getType().isBoolean() ? e[1].getType() : e.getType());
      substitutions[e] = add(t, e);
      e = e.substitute(substitutions);
    }
    substitutions[old_e] = e;
    // cout << ";"; for(int i = 0; i < depth; ++i) cout << " "; cout << old_e << " => " << e << endl;
    --depth;
    return e;
  }

  void dump() {
    cout << sout.str();
  }
};

int main(int argc, char* argv[])
{

  try {

    // Get the filename
    string input;
    if(argc > 1) input = string(argv[1]);
    else input = "<stdin>";

    // Create the expression manager
    Options options;
    options.set(inputLanguage, language::input::LANG_SMTLIB_V2);
    cout << Expr::setlanguage(language::output::LANG_SMTLIB_V2);
    // cout << Expr::dag(0);
    ExprManager exprManager(options);

    Mapper m(&exprManager);

    // Create the parser
    ParserBuilder parserBuilder(&exprManager, input, options);
    if(input == "<stdin>") parserBuilder.withStreamInput(cin);
    Parser* parser = parserBuilder.build();

    // Variables and assertions
    vector<string> variables;
    vector<string> info_tags;
    vector<string> info_data;
    vector<Expr> assertions;

    Command* cmd = NULL;
    CommandSequence commandsSequence;
    bool logicisset = false;

    while ((cmd = parser->nextCommand())) {

      // till logic is set, don't do any modifications
      if(!parser->logicIsSet()) {
        cout << (*cmd) << endl;
        delete cmd;
        continue;
      }

      // transform set-logic command, if there is one
      SetBenchmarkLogicCommand* setlogic = dynamic_cast<SetBenchmarkLogicCommand*>(cmd);
      if(setlogic) {
	LogicInfo logicinfo(setlogic->getLogic());
	if(!logicinfo.isTheoryEnabled(theory::THEORY_SETS)) {
	  cerr << "Sets theory not enabled. Stopping translation." << endl;
	  return 0;
	}
        logicinfo = logicinfo.getUnlockedCopy();
        if(enableAxioms) {
          logicinfo.enableQuantifiers();
          logicinfo.lock();
          if(!logicinfo.hasEverything()) {
            (logicinfo = logicinfo.getUnlockedCopy()).disableTheory(theory::THEORY_SETS);
            logicinfo.lock();
            cout << SetBenchmarkLogicCommand(logicinfo.getLogicString()) << endl;
          }
        } else {
          logicinfo.enableTheory(theory::THEORY_ARRAY);
          // we print logic string only for Quantifiers, for Z3 stuff
          // we don't set the logic
        }

        delete cmd;
        continue;
      }

      // if we reach here, logic is set by now, so can define our sort
      if( !logicisset ) {
        logicisset = true;
        m.defineSetSort();
      }

      // declare/define-sort commands are printed immediately
      DeclareTypeCommand* declaresort = dynamic_cast<DeclareTypeCommand*>(cmd);
      DefineTypeCommand* definesort = dynamic_cast<DefineTypeCommand*>(cmd);
      if(declaresort || definesort) {
        cout << *cmd << endl;
        delete cmd;
        continue;
      }

      // other commands are queued up, while replacing with new function symbols
      AssertCommand* assert = dynamic_cast<AssertCommand*>(cmd);
      DeclareFunctionCommand* declarefun = dynamic_cast<DeclareFunctionCommand*>(cmd);
      DefineFunctionCommand* definefun = dynamic_cast<DefineFunctionCommand*>(cmd);

      Command* new_cmd = NULL;
      if(assert) {
        Expr newexpr = m.collectSortsExpr(assert->getExpr());
        new_cmd = new AssertCommand(newexpr);
      } else if(declarefun) {
        Expr newfunc = m.collectSortsExpr(declarefun->getFunction());
        new_cmd = new DeclareFunctionCommand(declarefun->getSymbol(), newfunc, declarefun->getType());
      } else if(definefun) {
        Expr newfunc = m.collectSortsExpr(definefun->getFunction());
        Expr newformula = m.collectSortsExpr(definefun->getFormula());
        new_cmd = new DefineFunctionCommand(definefun->getSymbol(), newfunc, definefun->getFormals(), newformula);
      }

      if(new_cmd == NULL) {
        commandsSequence.addCommand(cmd);
      } else {
        commandsSequence.addCommand(new_cmd);
        delete cmd;
      }

    }

    m.dump();
    cout << commandsSequence;

    // Get rid of the parser
    //delete parser;
  } catch (Exception& e) {
    cerr << e << endl;
  }
}


#include <Engine/MeshEdit/MinSurf.h>

#include <Engine/Primitive/TriMesh.h>

#include <Eigen/Sparse>
#include <Eigen/SparseQR>

using namespace Ubpa;

using namespace std;
using namespace Eigen;

MinSurf::MinSurf(Ptr<TriMesh> triMesh)
	: heMesh(make_shared<HEMesh<V>>())
{
	Init(triMesh);
}

void MinSurf::Clear() {
	heMesh->Clear();
	triMesh = nullptr;
}

bool MinSurf::Init(Ptr<TriMesh> triMesh) {
	Clear();

	if (triMesh == nullptr)
		return true;

	if (triMesh->GetType() == TriMesh::INVALID) {
		printf("ERROR::MinSurf::Init:\n"
			"\t""trimesh is invalid\n");
		return false;
	}

	// init half-edge structure
	size_t nV = triMesh->GetPositions().size();
	vector<vector<size_t>> triangles;
	triangles.reserve(triMesh->GetTriangles().size());
	for (auto triangle : triMesh->GetTriangles())
		triangles.push_back({ triangle->idx[0], triangle->idx[1], triangle->idx[2] });
	heMesh->Reserve(nV);
	heMesh->Init(triangles);

	if (!heMesh->IsTriMesh() || !heMesh->HaveBoundary()) {
		printf("ERROR::MinSurf::Init:\n"
			"\t""trimesh is not a triangle mesh or hasn't a boundaries\n");
		heMesh->Clear();
		return false;
	}

	// triangle mesh's positions ->  half-edge structure's positions
	for (int i = 0; i < nV; i++) {
		auto v = heMesh->Vertices().at(i);
		v->pos = triMesh->GetPositions()[i].cast_to<vecf3>();
	}

	this->triMesh = triMesh;
	return true;
}

bool MinSurf::Run() {
	if (heMesh->IsEmpty() || !triMesh) {
		printf("ERROR::MinSurf::Run\n"
			"\t""heMesh->IsEmpty() || !triMesh\n");
		return false;
	}

	Minimize();

	// half-edge structure -> triangle mesh
	size_t nV = heMesh->NumVertices();
	size_t nF = heMesh->NumPolygons();
	vector<pointf3> positions;
	vector<unsigned> indice;
	positions.reserve(nV);
	indice.reserve(3 * nF);
	for (auto v : heMesh->Vertices())
		positions.push_back(v->pos.cast_to<pointf3>());
	for (auto f : heMesh->Polygons()) { // f is triangle
		for (auto v : f->BoundaryVertice()) // vertices of the triangle
			indice.push_back(static_cast<unsigned>(heMesh->Index(v)));
	}

	triMesh->Init(indice, positions);

	return true;
}

void MinSurf::Minimize() {
	// First, detect and fix boundary
	random_set<V*> boundary_points;
	random_set<V*> inner_points;

	auto boundaries = this->heMesh->Boundaries();
	if (boundaries.size() != 1) {
		cout << "ERROR::MinSurf::Minimize:" << endl
			 << "\t" << "got boundaries = " << boundaries.size()
			 << " (expect 1)" << endl;
		return;
	}

	for (auto v: boundaries[0]) {
		boundary_points.insert(v->Origin());
	}

	for (auto v: heMesh->Vertices()) {
		if (!boundary_points.contains(v)) {
			inner_points.insert(v);
		}
	}
	// Build sparse matrix
	size_t n = inner_points.size();
	SparseMatrix<float> coeff_mat(n, n);
	coeff_mat.setZero();
	VectorXf b_vec_x = VectorXf::Zero(n);
	VectorXf b_vec_y = VectorXf::Zero(n);
	VectorXf b_vec_z = VectorXf::Zero(n);

	cout << "coeff mat build start" << endl;

	int current_row = 0;
	for (auto v: inner_points) {
		// vidx CERTAINLY follows order (and it's redundant)
		size_t vidx = inner_points.idx(v);
		auto adj = v->AdjVertices();
		size_t degree = v->Degree();
		for (auto adjv : adj) {
			// check type
			if (boundary_points.contains(adjv)) { // this set is usually smaller
				b_vec_x(current_row) += (1.0f / degree) * adjv->pos[0];
				b_vec_y(current_row) += (1.0f / degree) * adjv->pos[1];
				b_vec_z(current_row) += (1.0f / degree) * adjv->pos[2];
			} else { // inner
				assert(inner_points.contains(adjv));
				size_t adjidx = inner_points.idx(adjv);
				// todo add assert = 0
				coeff_mat.insert(current_row, adjidx) = - 1.0f / degree;
			}
		}

		// add itself
		// todo add assert
		coeff_mat.insert(current_row, vidx) = 1;
		current_row++;
	}

	cout << "coeff mat build complete" << endl;

	// Solve
	SparseQR<SparseMatrix<float>, COLAMDOrdering<int>> solver;

	cout << "begin makeCompressed()" << endl;
	coeff_mat.makeCompressed();

	cout << "begin compute()" << endl;
	solver.compute(coeff_mat);
	if (solver.info() != Eigen::Success) {
		cout << "solver: decomposition was not successfull." << endl;
		return;
	}

	cout << "begin solve() for x" << endl;
	VectorXf res_x = solver.solve(b_vec_x);

	cout << "begin solve() for y" << endl;
	VectorXf res_y = solver.solve(b_vec_y);

	cout << "begin solve() for z" << endl;
	VectorXf res_z = solver.solve(b_vec_z);

	// Update vertex coordinates
	for (int i = 0; i < n; i++) {
		// find the corresponding point
		auto v = inner_points[i];
		vecf3 new_pos = { res_x(i), res_y(i), res_z(i) }; // works?

		//cout << new_pos << endl;
		v->pos = new_pos;
	}

}


//  (C) Copyright Matt Borland 2022.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/math/ccmath/isgreaterequal.hpp>
#include "test_compile_result.hpp"

void compile_and_link_test()
{
   check_result<float>(boost::math::ccmath::isgreaterequal(1.0f, 1.0f));
   check_result<double>(boost::math::ccmath::isgreaterequal(1.0, 1.0));
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   check_result<long double>(boost::math::ccmath::isgreaterequal(1.0l, 1.0l));
#endif
}


#pragma once

#include <armadillo>

namespace utils
{
    class LinearInterpolator
    {
        // contains methods for finding interpolated points between given points
        // used for finding height and other properties when setting up the grid

    public:
        LinearInterpolator(
                const arma::vec& positions,
                const arma::vec& values,
                const arma::uword interpolationMethod = 0 // default is no interpolation, only height uses linear interpolation
                );

        double getValueAtPoint(const double position);
        arma::vec getValuesAtPoints(const arma::vec& points);
        static arma::vec getValuesAtPoints(
                const arma::vec& referencePoints,
                const arma::vec& referenceValues,
                const arma::vec& points,
                const arma::uword interpolationMethod = 0);

    private:
        const size_t m_N;
        arma::vec m_positions;
        arma::vec m_values;
        arma::vec m_gradients;
        arma::uword m_interpolationMethod; // 0 means constant, 1 means linear

        bool m_printOutsideRangeWarning = false;
    };
}


/*    This file is part of the Gudhi Library - https://gudhi.inria.fr/ - which is released under MIT.
 *    See file LICENSE or go to https://gudhi.inria.fr/licensing/ for full license details.
 *    Author(s):       Pawel Dlotko
 *
 *    Copyright (C) 2016 Inria
 *
 *    Modification(s):
 *      - YYYY/MM Author: Description of the modification
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "Persistence_landscapes_test"
#include <boost/test/unit_test.hpp>
#include <gudhi/reader_utils.h>
#include <gudhi/Persistence_landscape.h>
#include <gudhi/Unitary_tests_utils.h>

#include <iostream>
#include <limits>

using namespace Gudhi;
using namespace Gudhi::Persistence_representations;

double epsilon = 0.0005;

BOOST_AUTO_TEST_CASE(check_construction_of_landscape) {
  std::vector<std::pair<double, double> > diag =
      read_persistence_intervals_in_one_dimension_from_file("data/file_with_diagram");
  Persistence_landscape p(diag);
  Persistence_landscape q;
  q.load_landscape_from_file("data/file_with_landscape_from_file_with_diagram");
  BOOST_CHECK(p == q);
}

BOOST_AUTO_TEST_CASE(check_construction_of_landscape_form_gudhi_style_file) {
  Persistence_landscape p("data/persistence_file_with_four_entries_per_line", 1);
  // p.print_to_file("persistence_file_with_four_entries_per_line_landscape");
  Persistence_landscape q;
  q.load_landscape_from_file("data/persistence_file_with_four_entries_per_line_landscape");
  BOOST_CHECK(p == q);
}

BOOST_AUTO_TEST_CASE(check_computations_of_integrals) {
  std::vector<std::pair<double, double> > diag =
      read_persistence_intervals_in_one_dimension_from_file("data/file_with_diagram");
  Persistence_landscape p(diag);
  GUDHI_TEST_FLOAT_EQUALITY_CHECK(p.compute_integral_of_landscape(), 2.34992, epsilon);
}

BOOST_AUTO_TEST_CASE(check_computations_of_integrals_for_each_level_separatelly) {
  std::vector<std::pair<double, double> > diag =
      read_persistence_intervals_in_one_dimension_from_file("data/file_with_diagram");
  Persistence_landscape p(diag);

  std::vector<double> integrals_for_different_levels;
  integrals_for_different_levels.push_back(0.216432);
  integrals_for_different_levels.push_back(0.204763);
  integrals_for_different_levels.push_back(0.188793);
  integrals_for_different_levels.push_back(0.178856);
  integrals_for_different_levels.push_back(0.163142);
  integrals_for_different_levels.push_back(0.155015);
  integrals_for_different_levels.push_back(0.143046);
  integrals_for_different_levels.push_back(0.133765);
  integrals_for_different_levels.push_back(0.123531);
  integrals_for_different_levels.push_back(0.117393);
  integrals_for_different_levels.push_back(0.111269);
  integrals_for_different_levels.push_back(0.104283);
  integrals_for_different_levels.push_back(0.0941308);
  integrals_for_different_levels.push_back(0.0811208);
  integrals_for_different_levels.push_back(0.0679001);
  integrals_for_different_levels.push_back(0.0580801);
  integrals_for_different_levels.push_back(0.0489647);
  integrals_for_different_levels.push_back(0.0407936);
  integrals_for_different_levels.push_back(0.0342599);
  integrals_for_different_levels.push_back(0.02896);
  integrals_for_different_levels.push_back(0.0239881);
  integrals_for_different_levels.push_back(0.0171792);
  integrals_for_different_levels.push_back(0.0071511);
  integrals_for_different_levels.push_back(0.00462067);
  integrals_for_different_levels.push_back(0.00229033);
  integrals_for_different_levels.push_back(0.000195296);

  for (size_t level = 0; level != p.size(); ++level) {
    GUDHI_TEST_FLOAT_EQUALITY_CHECK(p.compute_integral_of_a_level_of_a_landscape(level),
                                    integrals_for_different_levels[level], epsilon);
  }
}

BOOST_AUTO_TEST_CASE(check_computations_of_integrals_of_powers_of_landscape) {
  std::vector<std::pair<double, double> > diag =
      read_persistence_intervals_in_one_dimension_from_file("data/file_with_diagram");
  Persistence_landscape p(diag);

  std::vector<double> integrals_for_different_powers;
  integrals_for_different_powers.push_back(17.1692);
  integrals_for_different_powers.push_back(2.34992);
  integrals_for_different_powers.push_back(0.49857);
  integrals_for_different_powers.push_back(0.126405);
  integrals_for_different_powers.push_back(0.0355235);

  for (size_t power = 0; power != 5; ++power) {
    GUDHI_TEST_FLOAT_EQUALITY_CHECK(p.compute_integral_of_landscape((double)power),
                                    integrals_for_different_powers[power], epsilon);
  }
}

BOOST_AUTO_TEST_CASE(check_computations_of_values_on_different_points) {
  std::vector<std::pair<double, double> > diag =
      read_persistence_intervals_in_one_dimension_from_file("data/file_with_diagram");
  Persistence_landscape p(diag);

  GUDHI_TEST_FLOAT_EQUALITY_CHECK(p.compute_value_at_a_given_point(1, 0.0), 0., epsilon);
  GUDHI_TEST_FLOAT_EQUALITY_CHECK(p.compute_value_at_a_given_point(1, 0.1), 0.0692324, epsilon);
  GUDHI_TEST_FLOAT_EQUALITY_CHECK(p.compute_value_at_a_given_point(1, 0.2), 0.163369, epsilon);
  GUDHI_TEST_FLOAT_EQUALITY_CHECK(p.compute_value_at_a_given_point(1, 0.3), 0.217115, epsilon);
  GUDHI_TEST_FLOAT_EQUALITY_CHECK(p.compute_value_at_a_given_point(2, 0.0), 0., epsilon);
  GUDHI_TEST_FLOAT_EQUALITY_CHECK(p.compute_value_at_a_given_point(2, 0.1), 0.0633688, epsilon);
  GUDHI_TEST_FLOAT_EQUALITY_CHECK(p.compute_value_at_a_given_point(2, 0.2), 0.122361, epsilon);
  GUDHI_TEST_FLOAT_EQUALITY_CHECK(p.compute_value_at_a_given_point(2, 0.3), 0.195401, epsilon);
  GUDHI_TEST_FLOAT_EQUALITY_CHECK(p.compute_value_at_a_given_point(3, 0.0), 0., epsilon);
  GUDHI_TEST_FLOAT_EQUALITY_CHECK(p.compute_value_at_a_given_point(3, 0.1), 0.0455386, epsilon);
  GUDHI_TEST_FLOAT_EQUALITY_CHECK(p.compute_value_at_a_given_point(3, 0.2), 0.0954012, epsilon);
  GUDHI_TEST_FLOAT_EQUALITY_CHECK(p.compute_value_at_a_given_point(3, 0.3), 0.185282, epsilon);
}

BOOST_AUTO_TEST_CASE(check_computations_sum_differences_and_multiplications) {
  std::vector<std::pair<double, double> > diag =
      read_persistence_intervals_in_one_dimension_from_file("data/file_with_diagram");
  Persistence_landscape p(diag);
  Persistence_landscape second;
  second.load_landscape_from_file("data/file_with_landscape_from_file_with_diagram_1");

  Persistence_landscape sum = p + second;
  Persistence_landscape difference = p - second;
  Persistence_landscape multiply_by_scalar = 10 * p;

  Persistence_landscape template_sum;
  template_sum.load_landscape_from_file("data/sum");

  Persistence_landscape template_difference;
  template_difference.load_landscape_from_file("data/difference");

  Persistence_landscape template_multiply_by_scalar;
  template_multiply_by_scalar.load_landscape_from_file("data/multiply_by_scalar");

  BOOST_CHECK(sum == template_sum);
  BOOST_CHECK(difference == template_difference);
  BOOST_CHECK(multiply_by_scalar == template_multiply_by_scalar);
}

BOOST_AUTO_TEST_CASE(check_computations_of_maxima_and_norms) {
  std::vector<std::pair<double, double> > diag =
      read_persistence_intervals_in_one_dimension_from_file("data/file_with_diagram");
  Persistence_landscape p(diag);
  Persistence_landscape second;
  second.load_landscape_from_file("data/file_with_landscape_from_file_with_diagram_1");
  Persistence_landscape sum = p + second;

  GUDHI_TEST_FLOAT_EQUALITY_CHECK(p.compute_maximum(), 0.431313, epsilon);
  GUDHI_TEST_FLOAT_EQUALITY_CHECK(p.compute_norm_of_landscape(1), 2.34992, epsilon);
  GUDHI_TEST_FLOAT_EQUALITY_CHECK(p.compute_norm_of_landscape(2), 0.706095, epsilon);
  GUDHI_TEST_FLOAT_EQUALITY_CHECK(p.compute_norm_of_landscape(3), 0.501867, epsilon);
  GUDHI_TEST_FLOAT_EQUALITY_CHECK(compute_distance_of_landscapes(p, sum, 1), 27.9323, epsilon);
  GUDHI_TEST_FLOAT_EQUALITY_CHECK(compute_distance_of_landscapes(p, sum, 2), 2.35199, epsilon);
  GUDHI_TEST_FLOAT_EQUALITY_CHECK(compute_distance_of_landscapes(p, sum, std::numeric_limits<double>::max()), 0.464478,
                                  epsilon);
}

BOOST_AUTO_TEST_CASE(check_default_parameters_of_distances) {
  std::vector<std::pair<double, double> > diag =
      read_persistence_intervals_in_one_dimension_from_file("data/file_with_diagram");
  Persistence_landscape p(diag);

  std::vector<std::pair<double, double> > diag1 =
      read_persistence_intervals_in_one_dimension_from_file("data/file_with_diagram_1");
  Persistence_landscape q(diag1);

  double dist_numeric_limit_max = p.distance(q, std::numeric_limits<double>::max());
  double dist_infinity = p.distance(q, std::numeric_limits<double>::infinity());

  GUDHI_TEST_FLOAT_EQUALITY_CHECK(dist_numeric_limit_max, dist_infinity);
}

BOOST_AUTO_TEST_CASE(check_computations_of_averages) {
  std::vector<std::pair<double, double> > diag =
      read_persistence_intervals_in_one_dimension_from_file("data/file_with_diagram");
  Persistence_landscape p(diag);
  std::vector<std::pair<double, double> > diag2 =
      read_persistence_intervals_in_one_dimension_from_file("data/file_with_diagram_1");
  Persistence_landscape q(diag2);
  Persistence_landscape av;
  av.compute_average({&p, &q});

  Persistence_landscape template_average;
  template_average.load_landscape_from_file("data/average");
  BOOST_CHECK(template_average == av);
}

BOOST_AUTO_TEST_CASE(check_computations_of_distances) {
  std::vector<std::pair<double, double> > diag =
      read_persistence_intervals_in_one_dimension_from_file("data/file_with_diagram");
  Persistence_landscape p(diag);
  std::vector<std::pair<double, double> > diag2 =
      read_persistence_intervals_in_one_dimension_from_file("data/file_with_diagram_1");
  Persistence_landscape q(diag2);
  GUDHI_TEST_FLOAT_EQUALITY_CHECK(p.distance(q), 25.5824, epsilon);
  GUDHI_TEST_FLOAT_EQUALITY_CHECK(p.distance(q, 2), 2.1264, epsilon);
  GUDHI_TEST_FLOAT_EQUALITY_CHECK(p.distance(q, std::numeric_limits<double>::max()), 0.359068, epsilon);
}

BOOST_AUTO_TEST_CASE(check_computations_of_scalar_product) {
  std::vector<std::pair<double, double> > diag =
      read_persistence_intervals_in_one_dimension_from_file("data/file_with_diagram");
  Persistence_landscape p(diag);
  std::vector<std::pair<double, double> > diag2 =
      read_persistence_intervals_in_one_dimension_from_file("data/file_with_diagram_1");
  Persistence_landscape q(diag2);
  GUDHI_TEST_FLOAT_EQUALITY_CHECK(p.compute_scalar_product(q), 0.754498, epsilon);
}

// Below I am storing the code used to generate tests for that functionality.
/*
if ( argc != 2 )
        {
                std::cerr << "To run this program, please provide a name of a file with persistence landscape \n";
                //return 1;
        }
        Persistence_landscape p("data/file_with_diagram");

        Persistence_landscape q;
        q.load_landscape_from_file( "file_with_landscape_from_file_with_diagram" );

        if ( p != q )
        {
                cout << "Not equal \n";
        }

        double integral = p.compute_integral_of_landscape();
        cout << "integral : " << integral <<endl;

        //compute integral for each level separatelly
        for ( size_t level = 0 ; level != p.size() ; ++level )
        {
                cout << p.compute_integral_of_landscape( level ) << endl;
        }

        //compute integral of p-th power of landscspe
        for ( size_t power = 0 ; power != 5 ; ++power )
        {
                cout << p.compute_integral_of_landscape( power ) << endl;
        }

        cout << "Value of level 1 at 0 : " <<  p.compute_value_at_a_given_point(1,0.0) << endl;
        cout << "Value of level 1  at 1 : " <<  p.compute_value_at_a_given_point(1,0.1) << endl;
        cout << "Value of level 1  at 2 : " <<  p.compute_value_at_a_given_point(1,0.2) << endl;
        cout << "Value of level 1  at 3 : " <<  p.compute_value_at_a_given_point(1,0.3) << endl;

        cout << "Value of level 2 at 0 : " <<  p.compute_value_at_a_given_point(2,0.0) << endl;
        cout << "Value of level 2  at 1 : " <<  p.compute_value_at_a_given_point(2,0.1) << endl;
        cout << "Value of level 2  at 2 : " <<  p.compute_value_at_a_given_point(2,0.2) << endl;
        cout << "Value of level 2  at 3 : " <<  p.compute_value_at_a_given_point(2,0.3) << endl;

        cout << "Value of level 3 at 0 : " <<  p.compute_value_at_a_given_point(3,0.0) << endl;
        cout << "Value of level 3  at 1 : " <<  p.compute_value_at_a_given_point(3,0.1) << endl;
        cout << "Value of level 3  at 2 : " <<  p.compute_value_at_a_given_point(3,0.2) << endl;
        cout << "Value of level 3  at 3 : " <<  p.compute_value_at_a_given_point(3,0.3) << endl;

        Persistence_landscape second;
        second.load_landscape_from_file("file_with_landscape_from_file_with_diagram_1" );

        Persistence_landscape sum = p + second;
        Persistence_landscape difference = p - second;
        Persistence_landscape multiply_by_scalar = 10*p;

        //sum.print_to_file("sum");
        //difference.print_to_file("difference");
        //multiply_by_scalar.print_to_file("multiply_by_scalar");

        Persistence_landscape template_sum;
        template_sum.load_landscape_from_file( "sum" );
        Persistence_landscape template_difference;
        template_difference.load_landscape_from_file( "difference" );
        Persistence_landscape template_multiply_by_scalar;
        template_multiply_by_scalar.load_landscape_from_file( "multiply_by_scalar" );

        if ( sum != template_sum )
        {
                cerr << "Problem with sums \n";
        }
        if ( difference != template_difference )
        {
                cerr << "Problem with differences \n";
        }
        if ( multiply_by_scalar != template_multiply_by_scalar )
        {
                cerr << "Problem with multiplication by scalar \n";
        }

        cout << "Maximum : " << p.compute_maximum() << endl;

        cout << "L^1 norm : " << p.compute_norm_of_landscape(1) << endl;
        cout << "L^2 norm : " << p.compute_norm_of_landscape(2) << endl;
        cout << "L^3 norm : " << p.compute_norm_of_landscape(3) << endl;

        cout << "L^1 distance : " << compute_distance_of_landscapes(p,sum,1) << endl;
        cout << "L^2 distance : " << compute_distance_of_landscapes(p,sum,2) << endl;
        cout << "L^infty distance : " << compute_distance_of_landscapes(p,sum,std::numeric_limits<double>::max() ) <<
endl;

        {
                Persistence_landscape p( "data/file_with_diagram" );
                Persistence_landscape q( "data/file_with_diagram_1" );
                Persistence_landscape av;
                av.compute_average( {&p,&q} );

                Persistence_landscape template_average;
                template_average.load_landscape_from_file( "average" );
                if ( template_average != av )
                {
                        cerr << "We have a problem with average \n";
                }
        }

        {
                Persistence_landscape p( "data/file_with_diagram" );
                Persistence_landscape q( "data/file_with_diagram_1" );
                cout << "L^1 distance : " <<  p.distance( &q ) << endl;
                cout << "L^2 distance : " <<  p.distance( &q , 2) << endl;
                cout << "L^infty distance : " <<  p.distance( &q , std::numeric_limits<double>::max() ) << endl;
        }

        {
                Persistence_landscape p( "data/file_with_diagram" );
                Persistence_landscape q( "data/file_with_diagram_1" );
                cout << "Scalar product : " <<  p.compute_scalar_product( &q ) << endl;
        }
*/


#include <string>

#include <CGAL/Simple_cartesian.h>
#include <CGAL/Surface_mesh.h>

#include <boost/foreach.hpp>

typedef CGAL::Simple_cartesian<double> K;
typedef CGAL::Surface_mesh<K::Point_3> Mesh;
typedef Mesh::Vertex_index vertex_descriptor;
typedef Mesh::Face_index face_descriptor;

int main()
{

  Mesh m;
  vertex_descriptor v0 = m.add_vertex(K::Point_3(0,2,0));
  vertex_descriptor v1 = m.add_vertex(K::Point_3(2,2,0));
  vertex_descriptor v2 = m.add_vertex(K::Point_3(0,0,0));
  vertex_descriptor v3 = m.add_vertex(K::Point_3(2,0,0));
  vertex_descriptor v4 = m.add_vertex(K::Point_3(1,1,0));
  m.add_face(v3, v1, v4);
  m.add_face(v0, v4, v1);
  m.add_face(v0, v2, v4);
  m.add_face(v2, v3, v4);

  // give each vertex a name, the default is empty
  Mesh::Property_map<vertex_descriptor,std::string> name;
  bool created;
  boost::tie(name, created) = m.add_property_map<vertex_descriptor,std::string>("v:name","");
  assert(created);
  // add some names to the vertices
  name[v0] = "hello";
  name[v2] = "world";

  {
    // You get an existing property, and created will be false
    Mesh::Property_map<vertex_descriptor,std::string> name;
    bool created;
    boost::tie(name, created) = m.add_property_map<vertex_descriptor,std::string>("v:name", "");
    assert(! created);
  }

  //  You can't get a property that does not exist
  Mesh::Property_map<face_descriptor,std::string> gnus;
  bool found;
  boost::tie(gnus, found) = m.property_map<face_descriptor,std::string>("v:gnus");
  assert(! found);

  // retrieve the point property for which exists a convenience function
  Mesh::Property_map<vertex_descriptor, K::Point_3> location = m.points();
  BOOST_FOREACH( vertex_descriptor vd, m.vertices()) {
    std::cout << name[vd] << " @ " << location[vd] << std::endl;
  }

  // delete the string property again
  m.remove_property_map(name);

  return 0;
}


#ifndef KALMAN_FILTER_H_
#define KALMAN_FILTER_H_

#include <Eigen/Dense>

template<typename T, class System>
class KalmanFilter
{
typedef Eigen::Matrix<T, Eigen::Dynamic, 1> VectorXt;    //列向量
typedef Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> MatrixXt;
public:

    KalmanFilter(const System& _system, int _state_dim, int _input_dim, int _measurement_dim,
                 const MatrixXt& _process_noise, const MatrixXt& _measurement_noise,
                 const VectorXt& _mean, const MatrixXt& _cov):
        system(_system),
        state_dim(_state_dim),
        input_dim(_input_dim),
        measurement_dim(_measurement_dim),
        N(_state_dim),
        M(_measurement_dim),
        mean(_mean),
        cov(_cov),
        process_noise(_process_noise),
        measurement_noise(_measurement_noise)
    {

    }

    ~KalmanFilter()
    {
    }

    virtual void predict(const VectorXt& control) = 0;
    virtual void correct(const VectorXt& measurement) = 0;

    virtual const VectorXt& getMean() const { return mean; }
    virtual const MatrixXt& getCov() const { return cov; }

    virtual System& getSystem() { return system; }
    virtual const System& getSystem() const { return system; }
    virtual const MatrixXt& getProcessNoiseCov() const { return process_noise; }
    virtual const MatrixXt& getMeasurementNoiseCov() const { return measurement_noise; }
    virtual const MatrixXt& getKalmanGain() const { return kalman_gain; }

    /*			setter			*/
    virtual void setMean(const VectorXt& m) { mean = m;}
    virtual void setCov(const MatrixXt& s) { cov = s;}

    virtual void setProcessNoiseCov(const MatrixXt& p) { process_noise = p;}
    virtual void setMeasurementNoiseCov(const MatrixXt& m) { measurement_noise = m;}

    /**
    * @brief make covariance matrix positive finite
    * @param cov  covariance matrix
    */
    virtual void ensurePositiveFinite(MatrixXt& cov)
    {
        return;
        const double eps = 1e-9;

        Eigen::EigenSolver<MatrixXt> solver(cov);
        MatrixXt D = solver.pseudoEigenvalueMatrix();
        MatrixXt V = solver.pseudoEigenvectors();
        for (int i = 0; i < D.rows(); i++)
        {
            if (D(i, i) < eps)
            D(i, i) = eps;
        }

        cov = V * D * V.inverse();
    }

public:
    const int state_dim,       N; //状态向量维度
    const int input_dim;
    const int measurement_dim, M; //测量向量维度

    VectorXt mean;                //均值
    MatrixXt cov;                 //协方差

    System system;                //控制系统
    MatrixXt process_noise;		  //过程噪声 Q
    MatrixXt measurement_noise;	  //测量噪声 R

    MatrixXt kalman_gain;         //卡尔曼增益K

};

#endif


#pragma once
#include <Eigen/Core>
#include <map>
#include <optional>
#include <tuple>

#include "geohash/geometry.hpp"
#include "geohash/math.hpp"

namespace geohash::int64 {

// Returns the precision in longitude/latitude and degrees for the given
// precision
[[nodiscard]] inline auto constexpr error_with_precision(
    const uint32_t precision) -> std::tuple<double, double> {
  auto lat_bits = static_cast<int32_t>(precision >> 1U);
  auto lng_bits = static_cast<int32_t>(precision - lat_bits);

  return std::make_tuple(360 * power2(-lng_bits), 180 * power2(-lat_bits));
}

// Encode a point into geohash with the given precision
[[nodiscard]] auto encode(const Point& point, uint32_t precision) -> uint64_t;

// Encode points into geohash with the given precision
[[nodiscard]] inline auto encode(
    const Eigen::Ref<const Eigen::Matrix<Point, -1, 1>>& points,
    uint32_t precision) -> Eigen::Matrix<uint64_t, -1, 1> {
  auto result = Eigen::Matrix<uint64_t, -1, 1>(points.size());
  for (Eigen::Index ix = 0; ix < points.size(); ++ix) {
    result(ix) = encode(points(ix), precision);
  }
  return result;
}

// Returns the region encoded by the integer geohash with the specified
// precision.
[[nodiscard]] auto bounding_box(uint64_t hash, uint32_t precision) -> Box;

// Decode a hash into a spherical equatorial point with the given precision.
// If round is true, the coordinates of the points will be rounded to the
// accuracy defined by the GeoHash.
[[nodiscard]] inline auto decode(const uint64_t hash, const uint32_t precision,
                                 const bool round) -> Point {
  auto bbox = bounding_box(hash, precision);
  return round ? bbox.round() : bbox.center();
}

// Decode hashs into a spherical equatorial points with the given bit depth.
// If round is true, the coordinates of the points will be rounded to the
// accuracy defined by the GeoHash.
[[nodiscard]] inline auto decode(
    const Eigen::Ref<const Eigen::Matrix<uint64_t, -1, 1>>& hashs,
    const uint32_t precision, const bool center)
    -> Eigen::Matrix<Point, -1, 1> {
  auto result = Eigen::Matrix<Point, -1, 1>(hashs.size());
  for (Eigen::Index ix = 0; ix < hashs.size(); ++ix) {
    result(ix) = decode(hashs(ix), precision, center);
  }
  return result;
}

// Returns all neighbors hash clockwise from north around northwest at the given
// precision.
// 7 0 1
// 6 x 2
// 5 4 3
[[nodiscard]] auto neighbors(const uint64_t hash, const uint32_t precision)
    -> Eigen::Matrix<uint64_t, 8, 1>;

// Returns the property of the grid covering the given box: geohash of the
// minimum corner point, number of boxes in longitudes and latitudes.
[[nodiscard]] auto grid_properties(const Box& box, uint32_t precision)
    -> std::tuple<uint64_t, size_t, size_t>;

// Returns all the GeoHash codes within the box.
[[nodiscard]] auto bounding_boxes(const std::optional<Box>& box, uint32_t chars)
    -> Eigen::Matrix<uint64_t, -1, 1>;

// Returns all the GeoHash codes within the polygon.
[[nodiscard]] inline auto bounding_boxes(const Polygon& polygon, uint32_t chars)
    -> Eigen::Matrix<uint64_t, -1, 1> {
  auto box = Box();
  boost::geometry::envelope<Polygon, Box>(polygon, box);
  return bounding_boxes(box, chars);
}

// Returns the start and end indexes of the different GeoHash boxes.
[[nodiscard]] auto where(
    const Eigen::Ref<const Eigen::Matrix<uint64_t, -1, -1>>& hashs)
    -> std::map<uint64_t, std::tuple<std::tuple<int64_t, int64_t>,
                                     std::tuple<int64_t, int64_t>>>;

}  // namespace geohash::int64


#include <math.h>
#include <vector>
#include <limits>
using namespace std;

#include <armadillo>
using namespace arma;

#include "optimize_state_reconstructor.h"
#include "rate_model.h"
#include "state_reconstructor.h"

#include <gsl/gsl_multimin.h>
#include <gsl/gsl_vector.h>

OptimizeStateReconstructor::OptimizeStateReconstructor(RateModel * _rm,StateReconstructor * _sr, mat * _free_mask, int _nfree):
    rm(_rm),sr(_sr),free_variables(_free_mask),nfree_variables(_nfree),maxiterations(10000),stoppingprecision(0.001) {}

double OptimizeStateReconstructor::GetLikelihoodWithOptimized(const gsl_vector * variables) {
    for (unsigned int i=0; i < free_variables->n_rows; i++) {
        for (unsigned int j=0; j < free_variables->n_cols; j++) {
            if (i != j) {
                rm->set_Q_cell(i,j,gsl_vector_get(variables,(*free_variables)(i, j)));
                if (rm->get_Q()(i, j) < 0 || rm->get_Q()(i, j) >= 1000) {
                    return 100000000;
                }
            }
        }
    }
    double like;
    rm->set_Q_diag();
    like = sr->eval_likelihood();
    //cout << like << endl;
    if (like < 0 || like == std::numeric_limits<double>::infinity()) {
        like = 100000000;
    }
    return like;
}

double OptimizeStateReconstructor::GetLikelihoodWithOptimized_gsl(const gsl_vector * variables, void *obj) {
    double temp;
    temp= ((OptimizeStateReconstructor*)obj)->GetLikelihoodWithOptimized(variables);
    return temp;
}

/*
 * USES THE SIMPLEX ALGORITHM
 *
 */
mat OptimizeStateReconstructor::optimize() {
    const gsl_multimin_fminimizer_type *T = gsl_multimin_fminimizer_nmsimplex2;
    gsl_multimin_fminimizer *s = NULL;
    gsl_vector *ss, *x;
    size_t np = nfree_variables;
    size_t iter = 0, i;
    int status;
    double size;
    /* Initial vertex size vector */
    ss = gsl_vector_alloc (np);
    /* Set all step sizes to .01 */ //Note that it was originally 1
    gsl_vector_set_all (ss, .2);
    /* Starting point */
    //cout<<"Now in OPtimizaRateWithGivenTipVariance in OptimizationFn"<<endl;
    x = gsl_vector_alloc (np);
    for (unsigned int i=0; i < np; i++) {
        gsl_vector_set (x, i, 0.1);
    }
    OptimizeStateReconstructor *pt;
    pt=(this);
    double (*F)(const gsl_vector *, void *);
    F = &OptimizeStateReconstructor::GetLikelihoodWithOptimized_gsl;
    /* Initialize method and iterate */
    gsl_multimin_function minex_func;
    minex_func.f =* F;
    minex_func.params = pt;
    minex_func.n = np;
    s = gsl_multimin_fminimizer_alloc (T, np);
    gsl_multimin_fminimizer_set (s, &minex_func, x, ss);
    do {
        //cout<<"Now on iteration "<<iter<<endl;
        iter++;
        status = gsl_multimin_fminimizer_iterate(s);
        if (status != 0) { //0 Means it's a success
        //    printf ("error: %s\n", gsl_strerror (status));
            break;
        }
        size = gsl_multimin_fminimizer_size (s);
        //status = gsl_multimin_test_size (size, 1e-2);
        status = gsl_multimin_test_size (size, stoppingprecision); //since we want more precision
        if (status == GSL_SUCCESS) {
        //    printf ("converged to minimum at\n");
        }
        //printf ("%5d ", iter);
        for (i = 0; i < np; i++) {
        //    printf ("%10.3e ", gsl_vector_get (s->x, i));
        }
        //printf ("f() = %7.3f size = %.3f\n", s->fval, size);
    }
    while (status == GSL_CONTINUE && iter < maxiterations);
    mat results (free_variables->n_rows, free_variables->n_cols); results.fill(0);
    for (unsigned int i=0; i < results.n_rows; i++) {
        for (unsigned int j=0; j < results.n_cols; j++) {
            if (i != j) {
                results(i, j) = (gsl_vector_get(s->x, (*free_variables)(i, j)));
            }
        }
    }
    gsl_vector_free(x);
    gsl_vector_free(ss);
    gsl_multimin_fminimizer_free (s);
    return results;
}


#define BOOST_TEST_MODULE SRIM_PDF
#include <boost/test/included/unit_test.hpp>

#include <limits>
#include <tuple>

typedef std::tuple<float, double, long double> test_types;

#include <triumf/srim/pdf.hpp>

//
BOOST_AUTO_TEST_CASE_TEMPLATE(modified_beta, T, test_types) {
  T alpha = 2.0;
  T beta = 3.0;
  T z_max = 100.0;

  BOOST_TEST(triumf::srim::pdf::modified_beta<T>(-1.0, alpha, beta, z_max) ==
             static_cast<T>(0.0));
  BOOST_TEST(triumf::srim::pdf::modified_beta<T>(z_max + 1.0, alpha, beta,
                                                 z_max) == static_cast<T>(0.0));
}


#pragma once
#include "rust/cxx.h"
#include <memory>
#include <cstdint>
#include <NTL/GF2X.h>
#include <NTL/GF2E.h>

namespace poly
{
    struct PolyI64Pair;
    class Poly
    {
    public:
        // internal representation of polynomial
        NTL::GF2X int_pol;
        Poly();
        // copy
        Poly(const Poly &b);
        // move
        Poly(Poly &&b);
        void add_to(const Poly &b);
        void mul_to(const Poly &b);
        void div_to(const Poly &b);
        void gcd_to(const Poly &b);
        void rem_to(const Poly &b);
        void sqr();
        bool div_to_checked(const Poly &b);
        bool coeff(int64_t idx) const;
        bool eq(const Poly &b) const;
        bool is_zero() const;
        std::unique_ptr<std::vector<uint8_t>> to_bytes(int64_t min_bytes) const;
    };

    int64_t deg(const Poly &a);
    std::unique_ptr<Poly> new_poly_shifted(rust::Slice<uint8_t> bytes, int64_t shift, bool msb_first);
    std::unique_ptr<Poly> new_poly(rust::Slice<uint8_t> bytes);
    std::unique_ptr<Poly> new_zero();
    std::unique_ptr<Poly> copy_poly(const Poly &p);
    std::unique_ptr<Poly> add(const Poly &b, const Poly &c);
    std::unique_ptr<Poly> mul(const Poly &b, const Poly &c);
    std::unique_ptr<Poly> div(const Poly &b, const Poly &c);
    std::unique_ptr<Poly> gcd(const Poly &b, const Poly &c);
    std::unique_ptr<Poly> xgcd(Poly &x, Poly &y, const Poly &b, const Poly &c);
    std::unique_ptr<Poly> rem(const Poly &b, const Poly &c);
    std::unique_ptr<Poly> power(const Poly &p, int64_t n);
    std::unique_ptr<Poly> shift(const Poly &p, int64_t n);
    std::unique_ptr<std::vector<PolyI64Pair>> factor(const Poly &p, int64_t verbosity);

    class PolyRem
    {
    public:
        NTL::GF2E int_pol;
        // from modulus
        PolyRem(const Poly &p);
        // copy
        PolyRem(const PolyRem &b);
        // move
        PolyRem(PolyRem &&b);
        PolyRem(const NTL::GF2E &p);
        void add_to(const PolyRem &b);
        void mul_to(const PolyRem &b);
        void div_to(const PolyRem &b);
        void sqr();
        std::unique_ptr<Poly> rep() const;
    };
    std::unique_ptr<PolyRem> new_polyrem(const Poly &rem, const Poly &m);
    std::unique_ptr<PolyRem> powermod(const PolyRem &p, int64_t n);
    std::unique_ptr<PolyRem> copy_polyrem(const PolyRem &p);
} // namespace poly


#ifndef QP_SOLVER_H
#define QP_SOLVER_H

#include <Eigen/Dense>
#ifdef QP_SOLVER_USE_SPARSE
#include <Eigen/Sparse>
#endif
#include <cmath>
#include <limits>

#ifndef SOLVER_ASSERT
#define SOLVER_ASSERT(x) eigen_assert(x)
#endif

namespace qp_solver {

#ifdef QP_SOLVER_USE_SPARSE
template <int _n, int _m, typename _Scalar = double>
struct QP {
    using Scalar = _Scalar;
    enum {
        n=_n,
        m=_m
    };
    Eigen::SparseMatrix<Scalar> P;
    Eigen::Matrix<int, n, 1> P_col_nnz;
    Eigen::Matrix<Scalar, n, 1> q;
    Eigen::SparseMatrix<Scalar> A;
    Eigen::Matrix<int, n, 1> A_col_nnz;
    Eigen::Matrix<Scalar, m, 1> l, u;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
#else
template <int _n, int _m, typename _Scalar = double>
struct QP {
    using Scalar = _Scalar;
    enum {
        n=_n,
        m=_m
    };
    Eigen::Matrix<Scalar, n, n> P;
    Eigen::Matrix<Scalar, n, 1> q;
    Eigen::Matrix<Scalar, m, n> A;
    Eigen::Matrix<Scalar, m, 1> l, u;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
#endif

template <typename Scalar>
struct qp_sover_settings_t {
    Scalar rho = 1e-1;          /**< ADMM rho step, 0 < rho */
    Scalar sigma = 1e-6;        /**< ADMM sigma step, 0 < sigma, (small) */
    Scalar alpha = 1.0;         /**< ADMM overrelaxation parameter, 0 < alpha < 2,
                                     values in [1.5, 1.8] give good results (empirically) */
    Scalar eps_rel = 1e-3;      /**< Relative tolerance for termination, 0 < eps_rel */
    Scalar eps_abs = 1e-3;      /**< Absolute tolerance for termination, 0 < eps_abs */
    int max_iter = 1000;        /**< Maximal number of iteration, 0 < max_iter */
    int check_termination = 25; /**< Check termination after every Nth iteration, 0 (disabled) or 0 < check_termination */
    bool warm_start = false;    /**< Warm start solver, reuses previous x,z,y */
    bool adaptive_rho = false;  /**< Adapt rho to optimal estimate */
    Scalar adaptive_rho_tolerance = 5;  /**< Minimal for rho update factor, 1 < adaptive_rho_tolerance */
    int adaptive_rho_interval = 25; /**< change rho every Nth iteration, 0 < adaptive_rho_interval,
                                         set equal to check_termination to save computation  */
    bool verbose = false;

#ifdef QP_SOLVER_PRINTING
    void print() const
    {
        printf("ADMM settings:\n");
        printf("  sigma %.2e\n", sigma);
        printf("  rho %.2e\n", rho);
        printf("  alpha %.2f\n", alpha);
        printf("  eps_rel %.1e\n", eps_rel);
        printf("  eps_abs %.1e\n", eps_abs);
        printf("  max_iter %d\n", max_iter);
        printf("  adaptive_rho %d\n", adaptive_rho);
        printf("  warm_start %d\n", warm_start);
    }
#endif
};

typedef enum {
    SOLVED,
    MAX_ITER_EXCEEDED,
    UNSOLVED,
    UNINITIALIZED
} status_t;

template <typename Scalar>
struct qp_solver_info_t {
    status_t status = UNINITIALIZED; /**< Solver status */
    int iter = 0;               /**< Number of iterations */
    int rho_updates = 0;        /**< Number of rho updates (factorizations) */
    Scalar rho_estimate = 0;    /**< Last rho estimate */
    Scalar res_prim = 0;        /**< Primal residual */
    Scalar res_dual = 0;        /**< Dual residual */

#ifdef QP_SOLVER_PRINTING
    void print() const
    {
        printf("ADMM info:\n");
        printf("  status ");
        switch (status) {
        case SOLVED:
            printf("SOLVED\n");
            break;
        case MAX_ITER_EXCEEDED:
            printf("MAX_ITER_EXCEEDED\n");
            break;
        case UNSOLVED:
            printf("UNSOLVED\n");
            break;
        default:
            printf("UNINITIALIZED\n");
        };
        printf("  iter %d\n", iter);
        printf("  rho_updates %d\n", rho_updates);
        printf("  rho_estimate %f\n", rho_estimate);
        printf("  res_prim %f\n", res_prim);
        printf("  res_dual %f\n", res_dual);
    }
#endif
};

/**
 *  minimize        0.5 x' P x + q' x
 *  subject to      l <= A x <= u
 *
 *  with:
 *    x element of R^n
 *    Ax element of R^m
 */
template <typename QPType,
#ifdef QP_SOLVER_USE_SPARSE
          template <typename, int, typename... Args> class LinearSolver = Eigen::SimplicialLDLT,
#else
          template <typename, int, typename... Args> class LinearSolver = Eigen::LDLT,
#endif
          int LinearSolver_UpLo = Eigen::Lower>
class QPSolver {
public:
    enum {
        n=QPType::n,
        m=QPType::m
    };

    using qp_t = QPType;
    using Scalar = typename QPType::Scalar;
    using var_t = Eigen::Matrix<Scalar, n, 1>;
    using constraint_t = Eigen::Matrix<Scalar, m, 1>;
    using dual_t = Eigen::Matrix<Scalar, m, 1>;
    using kkt_vec_t = Eigen::Matrix<Scalar, n + m, 1>;
#ifdef QP_SOLVER_USE_SPARSE
    using SpMat = Eigen::SparseMatrix<Scalar, Eigen::ColMajor>;
    using kkt_mat_t = SpMat;
#else
    using kkt_mat_t = Eigen::Matrix<Scalar, n + m, n + m>;
#endif
    using settings_t = qp_sover_settings_t<Scalar>;
    using info_t = qp_solver_info_t<Scalar>;
    using linear_solver_t = LinearSolver<kkt_mat_t, LinearSolver_UpLo>;

    static constexpr Scalar RHO_MIN = 1e-6;
    static constexpr Scalar RHO_MAX = 1e+6;
    static constexpr Scalar RHO_TOL = 1e-4;
    static constexpr Scalar RHO_EQ_FACTOR = 1e+3;
    static constexpr Scalar LOOSE_BOUNDS_THRESH = 1e+16;
    static constexpr Scalar DIV_BY_ZERO_REGUL = std::numeric_limits<Scalar>::epsilon();

    // Solver state variables
    int iter;
    var_t x;
    constraint_t z;
    dual_t y;
    var_t x_tilde;
    constraint_t z_tilde;
    constraint_t z_prev;
    dual_t rho_vec;
    dual_t rho_inv_vec;
    Scalar rho;

    // State
    Scalar res_prim;
    Scalar res_dual;
    Scalar _max_Ax_z_norm;
    Scalar _max_Px_ATy_q_norm;

    enum {
        INEQUALITY_CONSTRAINT,
        EQUALITY_CONSTRAINT,
        LOOSE_BOUNDS
    } constr_type[m]; /**< constraint type classification */

    settings_t _settings;
    info_t _info;

    kkt_mat_t kkt_mat;
    linear_solver_t linear_solver;

    // enforce 16 byte alignment https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    QPSolver() { }

    void setup(const qp_t &qp)
    {
        x.setZero();
        z.setZero();
        y.setZero();

        // Set QP constraint type
        constr_type_init(qp);

        // initialize step size (rho) vector
        rho_vec_update(_settings.rho);

        // construct KKT system and compute decomposition
        construct_KKT_mat(qp);
        compute_KKT();

        _info.status = UNSOLVED;
    }

    void update_qp(const qp_t &qp)
    {
        // Set QP constraint type
        constr_type_init(qp);

        // initialize step size (rho) vector
        rho_vec_update(_settings.rho);

        // update KKT system and do factorization
        update_KKT_mat(qp);
        factorize_KKT();

        _info.status = UNSOLVED;
    }

    void solve(const qp_t &qp)
    {
        kkt_vec_t rhs, x_tilde_nu;
        bool check_termination = false;

        if (_info.status == UNINITIALIZED) {
            SOLVER_ASSERT(_info.status == UNINITIALIZED);
            return;
        }

#ifdef QP_SOLVER_PRINTING
        if (_settings.verbose) {
            _settings.print();
        }
#endif
        if (!_settings.warm_start) {
            x.setZero();
            z.setZero();
            y.setZero();
        }

        for (iter = 1; iter <= _settings.max_iter; iter++) {
            z_prev = z;

            // update x_tilde z_tilde
            form_KKT_rhs(qp, rhs);
            x_tilde_nu = linear_solver.solve(rhs);

            x_tilde = x_tilde_nu.template head<n>();
            z_tilde = z_prev + rho_inv_vec.cwiseProduct(x_tilde_nu.template tail<m>() - y);

            // update x
            x = _settings.alpha * x_tilde + (1 - _settings.alpha) * x;

            // update z
            z = _settings.alpha * z_tilde + (1 - _settings.alpha) * z_prev + rho_inv_vec.cwiseProduct(y);
            box_projection(z, qp.l, qp.u); // euclidean projection

            // update y
            y = y + rho_vec.cwiseProduct(_settings.alpha * z_tilde + (1 - _settings.alpha) * z_prev - z);

            if (_settings.check_termination != 0 && iter % _settings.check_termination == 0) {
                check_termination = true;
            } else {
                check_termination = false;
            }

            if (check_termination) {
                update_state(qp);

#ifdef QP_SOLVER_PRINTING
                if (_settings.verbose) {
                    print_status(qp);
                }
#endif
                if (termination_criteria(qp)) {
                    _info.status = SOLVED;
                    break;
                }
            }

            if (_settings.adaptive_rho && iter % _settings.adaptive_rho_interval == 0) {
                if (!check_termination) {
                    // state was not yet updated
                    update_state(qp);
                }
                Scalar new_rho = rho_estimate(rho, qp);
                new_rho = fmax(RHO_MIN, fmin(new_rho, RHO_MAX));
                _info.rho_estimate = new_rho;

                if (new_rho < rho / _settings.adaptive_rho_tolerance ||
                    new_rho > rho * _settings.adaptive_rho_tolerance) {
                    rho_vec_update(new_rho);
                    update_KKT_rho();
                    /* Note: KKT Sparsity pattern unchanged by rho update. Only factorize. */
                    factorize_KKT();
                }
            }
        }

        if (iter > _settings.max_iter) {
            _info.status = MAX_ITER_EXCEEDED;
        }
        _info.iter = iter;

#ifdef QP_SOLVER_PRINTING
        if (_settings.verbose) {
            _info.print();
        }
#endif
    }

    inline const var_t& primal_solution() const { return x; }
    inline var_t& primal_solution() { return x; }

    inline const dual_t& dual_solution() const { return y; }
    inline dual_t& dual_solution() { return y; }

    inline const settings_t& settings() const { return _settings; }
    inline settings_t& settings() { return _settings; }

    inline const info_t& info() const { return _info; }
    inline info_t& info() { return _info; }

private:
    /* Construct the KKT matrix of the form
     *
     * [[ P + sigma*I,        A' ],
     *  [ A,           -1/rho.*I ]]
     *
     * If LinearSolver_UpLo parameter is Eigen::Lower, then only the lower
     * triangular part is constructed to optimize memory.
     *
     * Note: For Eigen::ConjugateGradient it is advised to set Upper|Lower for
     *       best performance.
     */
    void construct_KKT_mat(const qp_t &qp)
    {
        static_assert(LinearSolver_UpLo == Eigen::Lower ||
                      LinearSolver_UpLo == (Eigen::Upper|Eigen::Lower),
                      "LinearSolver_UpLo must be Lower or Upper|Lower");

#ifdef QP_SOLVER_USE_SPARSE
        kkt_mat.resize(n+m,n+m); // sets all elements to 0
        Eigen::Matrix<int, n+m, 1> nnz_col;

        nnz_col.setConstant(1);
        nnz_col.template head<n>() = qp.P_col_nnz + qp.A_col_nnz;
        if (LinearSolver_UpLo == (Eigen::Upper|Eigen::Lower)) {
            // We don't know nnz of A rows, assume worst case
            nnz_col.template tail<m>().array() += n;
        }
        kkt_mat.reserve(nnz_col);

        // top left:  P + sigma*I
        sparse_insert_at(kkt_mat, 0, 0, qp.P);
        for (int i = 0; i < n; i++) {
            kkt_mat.coeffRef(i,i) += _settings.sigma;
        }

        // bottom left:  A
        sparse_insert_at(kkt_mat, n, 0, qp.A);

        // top right:  A'
        if (LinearSolver_UpLo == (Eigen::Upper|Eigen::Lower)) {
            sparse_insert_at(kkt_mat, 0, n, qp.A.transpose());
        }

        // bottom right:  -1/rho.*I
        for (int i = 0; i < m; i++) {
            kkt_mat.insert(n+i, n+i) = -rho_inv_vec(i);
        }

        kkt_mat.makeCompressed();
#else
        kkt_mat.template topLeftCorner<n, n>() = qp.P + _settings.sigma * qp.P.Identity();
        if (LinearSolver_UpLo == (Eigen::Upper|Eigen::Lower)) {
            kkt_mat.template topRightCorner<n, m>() = qp.A.transpose();
        }
        kkt_mat.template bottomLeftCorner<m, n>() = qp.A;
        kkt_mat.template bottomRightCorner<m, m>() = -1.0 * rho_inv_vec.asDiagonal();
#endif
    }

    /** KKT matrix value update, assumes same sparsity pattern */
    void update_KKT_mat(const qp_t &qp)
    {
#ifdef QP_SOLVER_USE_SPARSE
        // construct_KKT_mat(qp);
        // TODO: more efficient update?
        for (int k = 0; k < kkt_mat.outerSize(); ++k) {
            for (typename SpMat::InnerIterator it(kkt_mat, k); it; ++it) {
                int row, col;
                row = it.row();
                col = it.col();
                if (row < n) {
                    if (col < n) {
                        // top left:  P + sigma*I
                        it.valueRef() = qp.P.coeff(row, col) + _settings.sigma;
                    } else {
                        // top right:  A'
                        it.valueRef() = qp.A.coeff(col, row-n);
                    }
                } else {
                    if (col < n) {
                        // bottom left:  A
                        it.valueRef() = qp.A.coeff(row-n, col);
                    } else {
                        // bottom right:  -1/rho.*I
                        it.valueRef() = -rho_inv_vec(row-n);
                    }
                }
            }
        }
#else
        construct_KKT_mat(qp);
#endif
    }

    void update_KKT_rho()
    {
#ifdef QP_SOLVER_USE_SPARSE
        // Note: optimize by writing kkt_mat.valuePtr(). needs to be compressed and Eigen::Lower.
        for (int i = 0; i < m; i++) {
            kkt_mat.coeffRef(n+i, n+i) = -rho_inv_vec(i);
        }
#else
        kkt_mat.template bottomRightCorner<m, m>() = -1.0 * rho_inv_vec.asDiagonal();
#endif
    }

    void factorize_KKT()
    {
#ifdef QP_SOLVER_USE_SPARSE
        linear_solver.factorize(kkt_mat);
#else
        linear_solver.compute(kkt_mat);
#endif
        SOLVER_ASSERT(linear_solver.info() == Eigen::Success);
    }

    void compute_KKT()
    {
        linear_solver.compute(kkt_mat);
        SOLVER_ASSERT(linear_solver.info() == Eigen::Success);
    }

#ifdef QP_SOLVER_USE_SPARSE
    void sparse_insert_at(SpMat &dst, int row, int col, const SpMat &src) const
    {
        for (int k = 0; k < src.outerSize(); ++k) {
            for (typename SpMat::InnerIterator it(src, k); it; ++it) {
                dst.insert(row + it.row(), col + it.col()) = it.value();
            }
        }
    }
#endif

    void form_KKT_rhs(const qp_t &qp, kkt_vec_t& rhs)
    {
        rhs.template head<n>() = _settings.sigma * x - qp.q;
        rhs.template tail<m>() = z - rho_inv_vec.cwiseProduct(y);
    }

    void box_projection(constraint_t& z, const constraint_t& l, const constraint_t& u)
    {
        z = z.cwiseMax(l).cwiseMin(u);
    }

    void constr_type_init(const qp_t &qp)
    {
        for (int i = 0; i < qp.l.RowsAtCompileTime; i++) {
            if (qp.l[i] < -LOOSE_BOUNDS_THRESH && qp.u[i] > LOOSE_BOUNDS_THRESH) {
                constr_type[i] = LOOSE_BOUNDS;
            } else if (qp.u[i] - qp.l[i] < RHO_TOL) {
                constr_type[i] = EQUALITY_CONSTRAINT;
            } else {
                constr_type[i] = INEQUALITY_CONSTRAINT;
            }
        }
    }

    void rho_vec_update(Scalar rho0)
    {
        for (int i = 0; i < rho_vec.RowsAtCompileTime; i++) {
            switch (constr_type[i]) {
            case LOOSE_BOUNDS:
                rho_vec[i] = RHO_MIN;
                break;
            case EQUALITY_CONSTRAINT:
                rho_vec[i] = RHO_EQ_FACTOR*rho0;
                break;
            case INEQUALITY_CONSTRAINT: /* fall through */
            default:
                rho_vec[i] = rho0;
            };
        }
        rho_inv_vec = rho_vec.cwiseInverse();
        rho = rho0;
        _info.rho_updates += 1;
    }

    void update_state(const qp_t& qp)
    {
        Scalar norm_Ax, norm_z;
        norm_Ax = (qp.A*x).template lpNorm<Eigen::Infinity>();
        norm_z = z.template lpNorm<Eigen::Infinity>();
        _max_Ax_z_norm = fmax(norm_Ax, norm_z);

        Scalar norm_Px, norm_ATy, norm_q;
        norm_Px = (qp.P*x).template lpNorm<Eigen::Infinity>();
        norm_ATy = (qp.A.transpose()*y).template lpNorm<Eigen::Infinity>();
        norm_q = qp.q.template lpNorm<Eigen::Infinity>();
        _max_Px_ATy_q_norm = fmax(norm_Px, fmax(norm_ATy, norm_q));

        _info.res_prim = residual_prim(qp);
        _info.res_dual = residual_dual(qp);
    }

    Scalar rho_estimate(const Scalar rho0, const qp_t &qp) const
    {
        Scalar rp_norm, rd_norm;
        rp_norm = _info.res_prim / (_max_Ax_z_norm + DIV_BY_ZERO_REGUL);
        rd_norm = _info.res_dual / (_max_Px_ATy_q_norm + DIV_BY_ZERO_REGUL);

        Scalar rho_new = rho0 * sqrt(rp_norm/(rd_norm + DIV_BY_ZERO_REGUL));
        return rho_new;
    }

    Scalar eps_prim(const qp_t &qp) const
    {
        return _settings.eps_abs + _settings.eps_rel * _max_Ax_z_norm;
    }

    Scalar eps_dual(const qp_t &qp) const
    {
        return _settings.eps_abs + _settings.eps_rel * _max_Px_ATy_q_norm ;
    }

    Scalar residual_prim(const qp_t &qp) const
    {
        return (qp.A*x - z).template lpNorm<Eigen::Infinity>();
    }

    Scalar residual_dual(const qp_t &qp) const
    {
        return (qp.P*x + qp.q + qp.A.transpose()*y).template lpNorm<Eigen::Infinity>();
    }

    bool termination_criteria(const qp_t &qp)
    {
        // check residual norms to detect optimality
        if (_info.res_prim <= eps_prim(qp) && _info.res_dual <= eps_dual(qp)) {
            return true;
        }

        return false;
    }

#ifdef QP_SOLVER_PRINTING
    void print_status(const qp_t &qp) const
    {
        Scalar obj = 0.5 * x.dot(qp.P*x) + qp.q.dot(x);

        if (iter == _settings.check_termination) {
            printf("iter   obj       rp        rd\n");
        }
        printf("%4d  %.2e  %.2e  %.2e\n", iter, obj, _info.res_prim, _info.res_dual);
    }
#endif
};

} // namespace qp_solver

#endif // QP_SOLVER_H


/**
 * @file
 * @brief Action of a generic n-joint robot.
 * @copyright 2020, Max Planck Gesellschaft. All rights reserved.
 * @license BSD 3-clause
 */
#pragma once

#include <limits>
#include <string>
#include <vector>

#include <Eigen/Eigen>
#include <serialization_utils/cereal_eigen.hpp>

#include <robot_interfaces/loggable.hpp>

namespace robot_interfaces
{
/**
 * @brief Action of a generic n-joint robot.
 *
 * This action type can be used for all n-joint robots that expect torque or
 * position commands on joint-level.
 *
 * @tparam N Number of joints.
 */
template <size_t N>
struct NJointAction : public Loggable
{
    //! @brief Number of joints.
    static constexpr size_t num_joints = N;

    typedef Eigen::Matrix<double, N, 1> Vector;

    //! Desired torque command (in addition to position controller).
    Vector torque;
    //! Desired position.  Set to NaN to disable position controller.
    Vector position;
    //! P-gain for position controller.  If NaN, default is used.
    Vector position_kp;
    //! D-gain for position controller.  If NaN, default is used.
    Vector position_kd;

    template <class Archive>
    void serialize(Archive& archive)
    {
        archive(torque, position, position_kp, position_kd);
    }

    std::vector<std::string> get_name() override
    {
        return {"torque", "position", "position_kp", "position_kd"};
    }

    std::vector<std::vector<double>> get_data() override
    {
        // first map the Eigen vectors to std::vectors
        std::vector<double> torque_;
        torque_.resize(torque.size());
        Vector::Map(&torque_[0], torque.size()) = torque;

        std::vector<double> position_;
        position_.resize(position.size());
        Vector::Map(&position_[0], position.size()) = position;

        std::vector<double> position_kp_;
        position_kp_.resize(position_kp.size());
        Vector::Map(&position_kp_[0], position_kp.size()) = position_kp;

        std::vector<double> position_kd_;
        position_kd_.resize(position_kd.size());
        Vector::Map(&position_kd_[0], position_kd.size()) = position_kd;

        // then return them in a fixed size vector of vectors to avoid
        // copying due to pushing back value of information!
        std::vector<std::vector<double>> result;
        result = {torque_, position_, position_kp_, position_kd_};

        return result;
    }

    /**
     * @brief Create action with desired torque and (optional) position.
     *
     * The resulting torque command sent to the robot is
     *
     *     sent_torque = torque + PD(position)
     *
     * To disable the position controller, set the target position to NaN.
     * The controller is executed joint-wise, so it is possible to run it
     * only for some joints by setting a target position for these joints
     * and setting the others to NaN.
     *
     * The specified torque is always added to the result of the position
     * controller, so if you only want to run the position controller, make
     * sure to set `torque` to zero for all joints.
     *
     * For more explicit code, the static factory methods `Torque`,
     * `Position`, `TorqueAndPosition` and `Zero` should be used instead
     * directly creating actions through this constructor.
     *
     * @param torque  Desired torque.
     * @param position  Desired position.  Set values to NaN to disable
     *     position controller for the corresponding joints
     * @param position_kp  P-gains for the position controller.  Set to NaN
     *     to use default values.
     * @param position_kd  D-gains for the position controller.  Set to NaN
     *     to use default values.
     */
    NJointAction(Vector torque = Vector::Zero(),
                 Vector position = None(),
                 Vector position_kp = None(),
                 Vector position_kd = None())
        : torque(torque),
          position(position),
          position_kp(position_kp),
          position_kd(position_kd)
    {
    }

    /**
     * @brief Create an action that only contains a torque command.
     *
     * @param torque  Desired torque.
     *
     * @return Pure "torque action".
     */
    static NJointAction Torque(Vector torque)
    {
        return NJointAction(torque);
    }

    /**
     * @brief Create an action that only contains a position command.
     *
     * @param position Desired position.
     * @param kp P-gain for position controller.  If not set, default is
     *     used.  Set to NaN for specific joints to use default for this
     *     joint.
     * @param kd D-gain for position controller.  If not set, default is
     *     used.  Set to NaN for specific joints to use default for this
     *     joint.
     *
     * @return Pure "position action".
     */
    static NJointAction Position(Vector position,
                                 Vector kp = None(),
                                 Vector kd = None())
    {
        return NJointAction(Vector::Zero(), position, kp, kd);
    }

    /**
     * @brief Create an action with both torque and position commands.
     *
     * @param torque Desired torque.
     * @param position Desired position.  Set to NaN for specific joints to
     *     disable position control for this joint.
     * @param kp P-gain for position controller.  If not set, default is
     *     used.  Set to NaN for specific joints to use default for this
     *     joint.
     * @param kd D-gain for position controller.  If not set, default is
     *     used.  Set to NaN for specific joints to use default for this
     *     joint.
     *
     * @return Action with both torque and position commands.
     */
    static NJointAction TorqueAndPosition(Vector torque,
                                          Vector position,
                                          Vector position_kp = None(),
                                          Vector position_kd = None())
    {
        return NJointAction(torque, position, position_kp, position_kd);
    }

    /**
     * @brief Create a zero-torque action.
     *
     * @return Zero-torque action with position control disabled.
     */
    static NJointAction Zero()
    {
        return NJointAction();
    }

    /**
     * @brief Create a NaN-Vector.  Helper function to set defaults for
     *     position.
     *
     * @return Vector with all elements set to NaN.
     */
    static Vector None()
    {
        return Vector::Constant(std::numeric_limits<double>::quiet_NaN());
    }
};

}  // namespace robot_interfaces


#include "debug.h"
#include <CGAL/intersections.h>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Mesh_3/Robust_intersection_traits_3.h>

#include <CGAL/Mesh_triangulation_3.h>
#include <CGAL/Mesh_complex_3_in_triangulation_3.h>
#include <CGAL/Mesh_criteria_3.h>

#include <CGAL/Polyhedral_mesh_domain_3.h>
#include <CGAL/make_mesh_3.h>
#include <CGAL/refine_mesh_3.h>

// IO
#include <CGAL/IO/Polyhedron_iostream.h>

#include <boost/program_options.hpp>
namespace po = boost::program_options;

using namespace CGAL::parameters;

// Domain
// (we use exact intersection computation with Robust_intersection_traits_3)
struct K: public CGAL::Exact_predicates_inexact_constructions_kernel {};
typedef CGAL::Mesh_3::Robust_intersection_traits_3<K> Geom_traits;
typedef CGAL::Polyhedron_3<Geom_traits> Polyhedron;
typedef CGAL::Polyhedral_mesh_domain_3<Polyhedron, Geom_traits> Mesh_domain;

// Triangulation
typedef CGAL::Mesh_triangulation_3<Mesh_domain>::type Tr;
typedef CGAL::Mesh_complex_3_in_triangulation_3<Tr> C3t3;

// Mesh Criteria
typedef CGAL::Mesh_criteria_3<Tr> Mesh_criteria;
typedef Mesh_criteria::Facet_criteria Facet_criteria;
typedef Mesh_criteria::Cell_criteria Cell_criteria;

template <typename T>
T set_arg(const std::string& param_name,
          const std::string& param_string,
          const po::variables_map& vm)
{
  if ( vm.count(param_name) )
  {
    T param_value = vm[param_name].as<T>();
    std::cout << param_string << ": " << param_value << "\n";
    return param_value;
  }
  else
  {
    std::cout << param_string << " ignored.\n";
    return T();
  }
}

int main(int argc, char** argv)
{
  po::options_description generic("Generic options");
  generic.add_options() ("help", "Produce help message");
  generic.add_options()("file", po::value<std::string>(), "Mesh polyhedron contained in that file");

  po::options_description mesh("Mesh generation parameters");
  mesh.add_options()("facet_angle", po::value<double>(), "Set facet angle bound")
  ("facet_size", po::value<double>(), "Set facet size bound")
  ("facet_error", po::value<double>(), "Set facet approximation error bound")
  ("tet_shape", po::value<double>(), "Set tet radius-edge bound")
  ("tet_size", po::value<double>(), "Set tet size bound");

  po::options_description desc("Options");
  desc.add_options()
  ("exude", po::value<double>(), "Exude mesh after refinement. arg is time_limit.")
  ("perturb", po::value<double>(), "Perturb (sliver removal) mesh after refinement. arg is time_limit")
  ("lloyd", po::value<int>(), "Lloyd-smoothing after refinement. arg is max_iteration_nb")
  ("odt", po::value<int>(), "ODT-smoothing after refinement. arg is max_iteration_nb")
  ("convergence", po::value<double>()->default_value(0.02), "Convergence ratio for smoothing functions")
  ("min_displacement", po::value<double>()->default_value(0.01), "Minimal displacement ratio for smoothing functions (moves that are below that ratio will not be done)")
  ("time_limit", po::value<double>()->default_value(0), "Max time for smoothing functions")
  ("off_vertices", "Use polyhedron vertices as initialization step")
  ("no_label_rebind", "Don't rebind cell labels in medit output")
  ("show_patches", "Show surface patches in medit output");

  po::options_description cmdline_options("Usage");
  cmdline_options.add(generic).add(mesh).add(desc);

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, cmdline_options), vm);
  po::notify(vm);

  if (vm.count("help") || argc < 2)
  {
    std::cout << cmdline_options << std::endl;
    return 1;
  }

  std::cout << "=========== Params ==========="<< std::endl;

  double facet_angle = set_arg<double>("facet_angle","Facet angle",vm);
  double facet_size = set_arg<double>("facet_size","Facet size",vm);
  double facet_error = set_arg<double>("facet_error","Facet approximation error",vm);

  double tet_shape = set_arg<double>("tet_shape","Tet shape (radius-edge)",vm);
  double tet_size = set_arg<double>("tet_size","Tet size",vm);

  std::cout << std::endl;
  std::string polyhedron_filename = set_arg<std::string>("file", "Filename", vm);

  std::cout << "=============================="<< std::endl;
  std::cout << std::endl;

  if ( polyhedron_filename.empty() )
  {
    std::cout << "No file selected. Exit.\n";
    return 0;
  }

  // Loads polyhedron
  Polyhedron polyhedron;
  std::ifstream input(polyhedron_filename.c_str());
  input >> polyhedron;

  using std::atof;

  // Domain
  Mesh_domain domain(polyhedron);

  // Mesh criteria
  Facet_criteria facet_criteria(facet_angle,
                                facet_size,
                                facet_error); // angle, size, approximation
  Cell_criteria cell_criteria(tet_shape,
                              tet_size); // radius-edge ratio, size
  Mesh_criteria criteria(facet_criteria, cell_criteria);

  // Mesh generation
  C3t3 c3t3;
  if ( !vm.count("off_vertices") )
  {
    c3t3 = CGAL::make_mesh_3<C3t3>(domain, criteria, no_exude(), no_perturb());
  }
  else
  {
    c3t3.insert_surface_points(polyhedron.points_begin(),
                               polyhedron.points_end(),
                               domain.make_surface_index());

    CGAL::refine_mesh_3<C3t3>(c3t3, domain, criteria, no_exude(), no_perturb());
  }

  // Output
  std::ofstream medit_file_before("out_before.mesh");
  c3t3.output_to_medit(medit_file_before,
                       vm.count("no_label_rebind") == 0, vm.count("show_patches") > 0);

  // Odt
  if (  vm.count("odt") )
  {
    CGAL::odt_optimize_mesh_3(c3t3, domain,
                              max_iteration_number=vm["odt"].as<int>(),
                              convergence=vm["convergence"].as<double>(),
                              sliver_bound=vm["min_displacement"].as<double>(),
                              time_limit=vm["time_limit"].as<double>());
  }

  // Lloyd
  if ( vm.count("lloyd") )
  {
    CGAL::lloyd_optimize_mesh_3(c3t3, domain,
                                max_iteration_number=vm["lloyd"].as<int>(),
                                convergence=vm["convergence"].as<double>(),
                                sliver_bound=vm["min_displacement"].as<double>(),
                                time_limit=vm["time_limit"].as<double>());
  }

  // Perturbation
  if ( vm.count("perturb") )
  {
    CGAL::perturb_mesh_3(c3t3, domain, time_limit = vm["perturb"].as<double>() );
  }

  // Exudation
  if ( vm.count("exude") )
  {
    CGAL::exude_mesh_3(c3t3, time_limit = vm["exude"].as<double>());
  }

  double min_angle = 181.;
  for ( C3t3::Cell_iterator cit = c3t3.cells_begin() ;
       cit != c3t3.cells_end() ;
       ++cit )
  {
    min_angle = (std::min)(min_angle,
                           CGAL::to_double(CGAL::Mesh_3::minimum_dihedral_angle(c3t3.triangulation().tetrahedron(cit))));
  }

  std::cerr << "Min angle: " << min_angle << std::endl;

  // Output
  std::ofstream medit_file("out.mesh");
  c3t3.output_to_medit(medit_file, vm.count("no_label_rebind") == 0, vm.count("show_patches") > 0);

  return 0;
}


#include <functional>
#include <future>
#include <iostream>
#include <random>
#include <thread>

#include <catch.hpp>

#include <Eigen/Dense>

#include <spii/spii.h>

extern "C" {
	#include "matrix.h"
	#include "matrix2.h"
	#include "sparse2.h"
	#undef min
	#undef max
	#undef catch
}

template<typename EigenMat>
MAT* Eigen_to_Meschach(const EigenMat& eigen_matrix)
{
	auto m = eigen_matrix.rows();
	auto n = eigen_matrix.cols();

	auto A = m_get(int(m), int(n));
	for (int i = 0; i < m; ++i) {
		for (int j = 0; j < n; ++j) {
			A->me[i][j] = eigen_matrix(i, j);
		}
	}

	return A;
}

TEST_CASE("BKP-dense", "")
{
	using namespace std;
	using namespace Eigen;

	int n = 4;

	VectorXd x(4), b(4);
	b << 1, 2, 3, 4;

	MatrixXd A(4, 4);
	A.row(0) << 1, 2, 3, 1;
	A.row(1) << 2, 6, 1, 8;
	A.row(2) << 3, 1, 7, 6;
	A.row(3) << 1, 8, 6, 6;
	REQUIRE((A - A.transpose()).norm() == 0);

	// Sanity check.
	x = A.lu().solve(b);
	INFO(x);
	CHECK((A * x - b).norm() <= 1e-10);

	// Convert matrix to Meschach format.
	auto Amat = Eigen_to_Meschach(A);
	m_foutput(stderr, Amat);

	// Factorize the matrix.
	PERM* pivot  = px_get(4);
	PERM* block = px_get(4);
	spii_at_scope_exit(
		px_free(pivot);
		px_free(block);
	);
	BKPfactor(Amat, pivot, block);

	// Print the results.
	m_foutput(stderr, Amat);
	spii_at_scope_exit( m_free(Amat); );
	px_foutput(stderr, block);
	px_foutput(stderr, pivot);
	cerr << endl << endl;

	// Solve the linear system.
	VEC* bvec = v_get(4);
	spii_at_scope_exit( v_free(bvec); );
	for (int i = 0; i < 4; ++i) {
		bvec->ve[i] = b(i);
	}
	VEC* xvec = BKPsolve(Amat, pivot, block, bvec, nullptr);
	spii_at_scope_exit( v_free(xvec); );
	for (int i = 0; i < 4; ++i) {
		x(i) = xvec->ve[i];
	}
	INFO(x);
	INFO(A * x);
	CHECK((A * x - b).norm() <= 1e-10);

	MatrixXd B(4, 4);
	MatrixXd Q(4, 4);
	VectorXd tau(4);
	VectorXd lambda(4);
	B.setZero();
	Q.setZero();

	SelfAdjointEigenSolver<MatrixXd> eigensolver;

	double delta = 1e-10;

	// Extract the block diagonal matrix.
	int onebyone;
	for (int i = 0; i < n; i = onebyone ? i+1 : i+2 ) {
		onebyone = ( block->pe[i] == i );
		if ( onebyone ) {
		    B(i, i) = m_entry(Amat,i,i);
			lambda(i) = B(i, i);
			if (lambda(i) >= delta) {
				tau(i) = 0;
			}
			else {
				tau(i) = delta - lambda(i);
			}
			Q(i, i) = 1;
		}
		else {
		    auto a11 = m_entry(Amat,i,i);
		    auto a22 = m_entry(Amat,i+1,i+1);
		    auto a12 = m_entry(Amat,i+1,i);
			B(i,   i)   = a11;
			B(i+1, i)   = a12;
			B(i,   i+1) = a12;
			B(i+1, i+1) = a22;
			eigensolver.compute(B.block(i, i, 2, 2));

			lambda(i)   = eigensolver.eigenvalues()(0);
			lambda(i+1) = eigensolver.eigenvalues()(1);
			for (int k = i; k <= i + 1; ++k) {
				if (lambda(k) >= delta) {
					tau(k) = 0;
				}
				else {
					tau(k) = delta - lambda(k);
				}
			}

			Q.block(i, i, 2, 2) = eigensolver.eigenvectors();
		}
	}
	cerr << "B = \n" << B << endl << endl;
	cerr << "lambda = " << tau.transpose() << endl << endl;
	cerr << "tau = " << tau.transpose() << endl << endl;
	cerr << "Q = \n" << Q << endl << endl;
	cerr << "Q*lambda*Q^T = \n" << Q * lambda.asDiagonal() * Q.transpose() << endl << endl;

	// Check that the block-wise eigendecomposition was correct.
	CHECK(((B - Q * lambda.asDiagonal() * Q.transpose()).norm()) < 1e-10);

	MatrixXd F = Q * tau.asDiagonal() * Q.transpose();
	cerr << "F = Q*tau*Q^T = \n" << F << endl << endl;
	cerr << "B + F = \n" << B + F << endl << endl;
}

#ifdef USE_OPENMP
TEST_CASE("BKP-dense-threadsafe")
{
	using namespace std;
	using namespace Eigen;

	auto stress_test = [](unsigned seed)
	{
		mt19937_64 engine(seed);
		auto rand = bind(uniform_int_distribution<int>(-10, 10), ref(engine));
		const int n = 400;

		MatrixXd A(n, n);
		for (int i = 0; i < n; ++i) {
			for (int j = 0; j < n; ++j) {
				A(i, j) = rand();
			}
		}
		A = A.transpose() * A;
		auto Amat = Eigen_to_Meschach(A);
		spii_at_scope_exit( m_free(Amat); );

		// Factorize the matrix.
		PERM* pivot = px_get(n);
		spii_at_scope_exit( px_free(pivot); );
		PERM* block = px_get(n);
		spii_at_scope_exit( px_free(block); );
		BKPfactor(Amat, pivot, block);

		VectorXd b(n);
		VEC* bvec = v_get(n);
		spii_at_scope_exit( v_free(bvec); );
		for (int i = 0; i < n; ++i) {
			b(i) = rand();
			bvec->ve[i] = b(i);
		}

		for (int iteration = 1; iteration <= 20; ++iteration) {
			VEC* xvec = BKPsolve(Amat, pivot, block, bvec, 0);
			VectorXd x(n);
			for (int i = 0; i < n; ++i) {
				x(i) = xvec->ve[i];
			}
			v_free(xvec);
			double err = (A * x - b).norm() / b.norm();
			if (err > 1e-6) {
				throw std::runtime_error("Not thread-safe!");
			}
		}
	};

	try {
		auto f1 = async(launch::async, stress_test, 1);
		auto f2 = async(launch::async, stress_test, 2);
		auto f3 = async(launch::async, stress_test, 3);
		auto f4 = async(launch::async, stress_test, 4);
		auto f5 = async(launch::async, stress_test, 5);
		f1.get();
		f2.get();
		f3.get();
		f4.get();
		f5.get();
		SUCCEED();
	}
	catch (...) {
		FAIL();
	}
}
#endif

/*
TEST_CASE("BKP-sparse", "")
{
	int m = 5;
	int n = 5;
	int deg = 5;
	SPMAT* A = sp_get(m, n, deg);

	int I[25]    = {1,2,3,4,5,1,2,3,4,5,1,2,3,4,5,1,2,3,4,5,1,2,3,4,5};
	int J[25]    = {1,1,1,1,1,2,2,2,2,2,3,3,3,3,3,4,4,4,4,4,5,5,5,5,5};
	double V[25] = {1,7,3,5,1,5,2,6,6,7,4,1,2,8,1,7,5,9,1,4,1,9,0,7,7};

	for (int i = 0; i < 25; ++i) {
		if (V[i] != 0) {
			sp_set_val(A, I[i] - 1, J[i] - 1, V[i]);
		}
	}

	sp_foutput2(stderr, A);

	SPMAT* B = sp_copy(A);
	PERM* pivot  = px_get(5);
	PERM* blocks = px_get(5);
	spBKPfactor(B, pivot, blocks, 1e-16);

	sp_fo1utput2(stderr, B);
	px_foutput(stderr, pivot);
	px_foutput(stderr, blocks);

	SP_FREE(A);
	SP_FREE(B);
}
*/


#define _USE_MATH_DEFINES

#include <ceres/jet.h>
#include <ceres/rotation.h>
#include <geometry/covariance.h>
#include <gmock/gmock.h>
#include <gtest/gtest.h>

#include <Eigen/Dense>
#include <cmath>
#include <random>
#include <unsupported/Eigen/AutoDiff>

#include "geometry/transformations_functions.h"

class CovarianceFixture : public ::testing::Test {
 public:
  CovarianceFixture()
      : camera(geometry::Camera::CreatePerspectiveCamera(focal, 0.0, 0.0)),
        pose(geometry::Pose(Vec3d(0., 0., 0.), Vec3d(0, 0, 0))) {
    point_adiff[0].value() = point[0] = 0.;
    point_adiff[1].value() = point[1] = 0.;
    point_adiff[2].value() = point[2] = 3.;
    observation << 0.0, 0.0;
  }

  void RunAutodiffEval(const geometry::Camera& camera,
                       const geometry::Pose& pose) {
    // Prepare Eigen's Autodiff structures
    for (int i = 0; i < 3; ++i) {
      point_adiff[i].derivatives() = VecXd::Unit(3, i);
    }

    VecX<AScalar> pose_adiff(6);
    pose_adiff.segment<3>(3) = pose.TranslationCameraToWorld();
    pose_adiff.segment<3>(0) = pose.RotationCameraToWorldMin();
    for (int i = 0; i < 6; ++i) {
      pose_adiff(i).derivatives().resize(3);
      pose_adiff(i).derivatives().setZero();
    }

    VecX<AScalar> camera_adiff(3);
    const VecXd camera_params = camera.GetParametersValues();
    for (int i = 0; i < camera_params.size(); ++i) {
      camera_adiff(i).value() = camera_params(i);
      camera_adiff(i).derivatives().resize(3);
      camera_adiff(i).derivatives().setZero();
    }

    // Run project with Autodiff types to get expected jacobian
    AScalar transformed[3];
    geometry::PoseFunctor::Forward(point_adiff, pose_adiff.data(),
                                   &transformed[0]);
    geometry::Dispatch<geometry::ProjectFunction>(
        camera.GetProjectionType(), transformed, camera_adiff.data(),
        projection_expected);
  }

  template <class MAT>
  void CheckJacobian(const MAT& jacobian) {
    const double eps = 1e-12;
    for (int i = 0; i < 2; ++i) {
      for (int j = 0; j < 3; ++j) {
        ASSERT_NEAR(projection_expected[i].derivatives()(j), jacobian(i, j),
                    eps);
      }
      std::cout << projection_expected[i].value() << std::endl;
    }
  }

  const double focal{1.0};
  double point[3];
  typedef Eigen::AutoDiffScalar<VecXd> AScalar;
  AScalar point_adiff[3];
  AScalar projection_expected[2];
  Vec2d observation;

  const geometry::Camera camera;
  const geometry::Pose pose;
};

TEST_F(CovarianceFixture, EvaluatesPointJacobian) {
  Vec3d point_tmp(point[0], point[1], point[2]);

  const auto result = geometry::covariance::ComputeJacobianReprojectionError(
      camera, pose, observation, point_tmp);

  RunAutodiffEval(camera, pose);
  CheckJacobian(result.first);
  for (int i = 0; i < 2; ++i) {
    ASSERT_NEAR(0., result.second(i), 1e-10);
  }
}

TEST_F(CovarianceFixture, EvaluatesPointCovarianceKO) {
  Vec3d point_tmp(point[0], point[1], point[2]);

  auto covariance = geometry::covariance::ComputePointInverseCovariance(
                        {camera}, {pose}, {observation}, point_tmp)
                        .first;

  // Non-determined covariance for 1 projection
  ASSERT_TRUE(covariance.determinant() < 1e-12);
}

TEST_F(CovarianceFixture, EvaluatesPointCovarianceOK) {
  geometry::Pose pose_rotated_y;
  pose_rotated_y.SetWorldToCamRotation(Vec3d(0, -M_PI_2, 0));
  pose_rotated_y.SetOrigin(Vec3d(4, 0, 3));
  Vec3d point_tmp(point[0], point[1], point[2]);

  auto covariance = geometry::covariance::ComputePointInverseCovariance(
                        {camera, camera}, {pose, pose_rotated_y},
                        {observation, observation}, point_tmp)
                        .first;

  // Two pose looking with an angle of 90 degres
  ASSERT_TRUE(covariance.determinant() > 1e-12);
}

TEST_F(CovarianceFixture, EvaluatesPointCovarianceSmallBaseline) {
  const double baseline = 1e-5;
  const double angle = M_PI_2 - std::abs(std::atan2(point[2], baseline));

  geometry::Pose pose_rotated_y;
  pose_rotated_y.SetWorldToCamRotation(Vec3d(0, angle, 0));
  pose_rotated_y.SetOrigin(Vec3d(baseline, 0, 0));
  Vec3d point_tmp(point[0], point[1], point[2]);

  auto covariance = geometry::covariance::ComputePointInverseCovariance(
                        {camera, camera}, {pose, pose_rotated_y},
                        {observation, observation}, point_tmp)
                        .first;

  ASSERT_TRUE(covariance.determinant() < 1e-12);
}