// Author(s): Wieger Wesselink
// Copyright: see the accompanying file COPYING or copy at
// https://github.com/mCRL2org/mCRL2/blob/master/COPYING
//
// Distributed under 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)
//
/// \file pbesinst_test.cpp
/// \brief Add your file description here.

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

#include "mcrl2/lps/detail/test_input.h"
#include "mcrl2/modal_formula/detail/test_input.h"
#include "mcrl2/modal_formula/parse.h"
#include "mcrl2/pbes/is_bes.h"
#include "mcrl2/pbes/lps2pbes.h"
#include "mcrl2/pbes/pbesinst_finite_algorithm.h"
#include "mcrl2/pbes/pbesinst_symbolic.h"
#include "mcrl2/pbes/rewriter.h"
#include "mcrl2/pbes/txt2pbes.h"

using namespace mcrl2;
using namespace mcrl2::pbes_system;

inline
pbes pbesinst_lazy(const pbes& p)
{
  pbes q = p;
  pbesinst_algorithm algorithm(q.data());
  algorithm.run(q);
  return algorithm.get_result();
}

inline
pbes pbesinst_finite(const pbes& p)
{
  pbes q = p;
  pbesinst_finite_algorithm algorithm(data::jitty);
  algorithm.run(q);
  return q;
}

std::string test1 =
  "pbes                                                                              \n"
  "                                                                                \n"
  "nu X(b:Bool, n:Nat) = (val(b) => X(!b, n)) && (val(!b) => X(!b, n+1));            \n"
  "                                                                                \n"
  "init X(true,0);                                                                   \n"
  ;

std::string test2 =
  "pbes                                                                              \n"
  "                                                                                \n"
  "nu X(b:Bool, n:Nat) = forall c:Bool. X(c,n);                                      \n"
  "                                                                                \n"
  "init X(true,0);                                                                   \n"
  ;

std::string test3 =
  "pbes                                                                              \n"
  "                                                                                \n"
  "nu X(b:Bool, n:Nat) = exists c:Bool. X(c,n+1);                                    \n"
  "                                                                                \n"
  "init X(true,0);                                                                   \n"
  ;

std::string test4 =
  "pbes                                                                              \n"
  "                                                                                \n"
  "nu X(b:Bool, n:Nat) = val(b && n < 10) => X(!b,n+1);                              \n"
  "                                                                                \n"
  "init X(true,0);                                                                   \n"
  ;

std::string test5 =
  "sort D = struct d1 | d2;                                                          \n"
  "                                                                                \n"
  "pbes                                                                              \n"
  "                                                                                \n"
  "nu X(d:D, n:Nat) = val(d == d1 && n < 10) => X(d2,n+1);                           \n"
  "                                                                                \n"
  "init X(d1,0);                                                                     \n"
  ;

std::string test6 =
  "pbes                                                                              \n"
  "nu X(b:Bool) = forall c:Bool. X(if (c,!c,c));                                     \n"
  "                                                                                \n"
  "init X(true);                                                                     \n"
  ;

std::string test7 =
  "sort Enum2 = struct e1_5 | e0_5;                                                  \n"
  "                                                                                \n"
  "map                                                                               \n"
  "                                                                                \n"
  "     C5_fun2: Enum2 # Enum2 # Enum2  -> Enum2;                                    \n"
  "     C5_fun1: Enum2 # Nat # Nat  -> Nat;                                          \n"
  "                                                                                \n"
  "var  y23,y22,y21,x5,y14,y13,y12,y11,y10,x2,e3,e2,e1: Enum2;                       \n"
  "     y20,y19,y18,x4,y9,y8,y7,y6,y5,x1: Nat;                                       \n"
  "     y17,y16,y15,x3,y4,y3,y2,y1,y,x: Bool;                                        \n"
  "eqn                                                                               \n"
  "     C5_fun2(e0_5, y14, y13)  =  y14;                                             \n"
  "     C5_fun2(e1_5, y14, y13)  =  y13;                                             \n"
  "     C5_fun2(e3, x2, x2)  =  x2;                                                  \n"
  "     C5_fun1(e0_5,  y6, y5)  =  y5;                                               \n"
  "     C5_fun1(e1_5,  y6, y5)  =  y6;                                               \n"
  "     C5_fun1(e2,  x1, x1)  =  x1;                                                 \n"
  "                                                                                \n"
  "pbes nu X(s3_P: Enum2,  n_P: Nat) =                                               \n"
  "                                                                                \n"
  "(forall e: Enum2.  X(C5_fun2(e, e, e1_5), C5_fun1(e, 0, n_P))                     \n"
  "                                                                                \n"
  ")                                                                                \n"
  "                                                                                \n"
  "                                                                                \n"
  ";                                                                                \n"
  "                                                                                \n"
  "init X(e1_5,  0);                                                                 \n"
  ;

std::string test8 =
  "pbes                                                                              \n"
  "                                                                                \n"
  "nu X(b:Bool) = val(b) && Y(!b);                                                   \n"
  "                                                                                \n"
  "mu Y(c:Bool) = forall d:Bool. X(d && c) || Y(d);                                  \n"
  "                                                                                \n"
  "init X(true);                                                                     \n"
  ;

// This pbes triggered a garbage collection problem, that has been solved.
std::string random1 =
  "pbes                                                                                \n"
  "nu X0(c:Bool, n:Nat) = (forall n:Nat.((val(n < 3)) && (((val(n < 3)) || (exists m:Nat.((val(m < 3)) || (X3(m + 1, m > 0))))) && ((forall m:Nat.((val(m < 3)) && (!X2(m + 1, 1)))) => ((val(c)) || (val(n < 3))))))) || ((val(false)) || (X0(false, n + 1)));                                                                                \n"
  "nu X1(b:Bool) = (!(!(forall k:Nat.((val(k < 3)) && ((forall k:Nat.((val(k < 3)) && ((X2(1, k + 1)) && (val(false))))) || ((X4(k > 0, k + 1)) && (X1(k > 1)))))))) && (!(forall m:Nat.((val(m < 3)) && (((val(m < 2)) && (val(m > 0))) && (val(true))))));                                                                                \n"
  "mu X2(m:Nat, n:Nat) = (((val(m < 2)) && (X4(m == n, n + 1))) || ((val(false)) || ((val(true)) => (X0(n == m, 0))))) || (forall k:Nat.((val(k < 3)) && (exists m:Nat.((val(m < 3)) || ((val(n < 2)) && (X2(m + 1, m + 1)))))));                                                                                \n"
  "nu X3(n:Nat, c:Bool) = ((forall k:Nat.((val(k < 3)) && (!((forall m:Nat.((val(m < 3)) && (val(n > 0)))) => (val(c)))))) && ((X3(0, n < 3)) && (exists m:Nat.((val(m < 3)) || ((!(exists n:Nat.((val(n < 3)) || (val(m < 2))))) && (X2(n + 1, m + 1))))))) || ((!(!X1(n > 0))) || (val(false)));                                                                                \n"
  "nu X4(c:Bool, n:Nat) = (((exists m:Nat.((val(m < 3)) || (val(m > 0)))) && ((!(!X0(n < 3, 0))) || (!((val(n > 0)) => (!X2(0, 0)))))) => (forall k:Nat.((val(k < 3)) && (!(forall n:Nat.((val(n < 3)) && (val(n < 2)))))))) => (!(forall m:Nat.((val(m < 3)) && ((val(c)) && (forall m:Nat.((val(m < 3)) && (!X3(n + 1, false))))))));                                                                                \n"
  "                                                                                \n"
  "init X0(true, 0);                                                                                \n"
  ;

// This pbes triggered a garbage collection problem, that has been solved.
std::string random2 =
  "pbes                                                                                \n"
  "mu X0(m:Nat, b:Bool) = (X0(m + 1, m > 0)) && (((forall m:Nat.((val(m < 3)) && (forall k:Nat.((val(k < 3)) && (!((val(k > 1)) && (val(false)))))))) && (exists n:Nat.((val(n < 3)) || (!(!(val(n > 1))))))) || (exists n:Nat.((val(n < 3)) || (forall n:Nat.((val(n < 3)) && (exists n:Nat.((val(n < 3)) || (((forall m:Nat.((val(m < 3)) && (val(m == n)))) => (X3(0, n > 1))) && (!(!X2(n == m)))))))))));                                                                                \n"
  "mu X1(c:Bool, b:Bool) = (((!X1(c, true)) || ((!X2(b)) || (val(b)))) && (forall k:Nat.((val(k < 3)) && (((val(c)) => (X4(c, 1))) => (val(true)))))) => (val(true));                                                                                \n"
  "nu X2(b:Bool) = (!(!(((exists m:Nat.((val(m < 3)) || (val(m < 3)))) => ((X0(1, true)) => (!(val(false))))) && ((!((!X1(b, true)) && (!X3(0, false)))) => (val(false)))))) => (exists m:Nat.((val(m < 3)) || (forall k:Nat.((val(k < 3)) && (exists n:Nat.((val(n < 3)) || (forall n:Nat.((val(n < 3)) && (exists n:Nat.((val(n < 3)) || (forall n:Nat.((val(n < 3)) && (val(false))))))))))))));                                                                                \n"
  "mu X3(m:Nat, c:Bool) = (exists m:Nat.((val(m < 3)) || ((!(val(c))) && (forall k:Nat.((val(k < 3)) && (exists m:Nat.((val(m < 3)) || (forall k:Nat.((val(k < 3)) && (val(false))))))))))) || (exists m:Nat.((val(m < 3)) || (exists n:Nat.((val(n < 3)) || ((!((exists k:Nat.((val(k < 3)) || (!X4(false, k + 1)))) && ((!X1(m > 0, n < 2)) || ((val(n > 0)) || (val(c)))))) && (exists k:Nat.((val(k < 3)) || (exists n:Nat.((val(n < 3)) || (exists k:Nat.((val(k < 3)) || (X3(k + 1, k == n))))))))))))); \n"
  "nu X4(b:Bool, n:Nat) = (exists m:Nat.((val(m < 3)) || ((val(b)) || ((val(n > 1)) && ((X1(m > 1, n == m)) || (val(m > 0))))))) && ((exists k:Nat.((val(k < 3)) || (!(!((!X3(1, n > 0)) && (!(val(k == n)))))))) => (forall n:Nat.((val(n < 3)) && (exists k:Nat.((val(k < 3)) || (!(!(X4(n > 1, 0)))))))));                                                                                \n"
  "                                                                                \n"
  "init X0(0, true);                                                                                \n"
  ;

// This pbes triggered an error with pbesinst finite
std::string random3 =
  "pbes                                                                                \n"
  "mu X0(n:Nat, c:Bool) = ((!(((val(n < 2)) && (!X1)) && (exists k:Nat.((val(k < 3)) || (val(c)))))) && (((forall n:Nat.((val(n < 3)) && (!X4(n > 1)))) || (!(val(n > 1)))) => (val(true)))) && (exists n:Nat.((val(n < 3)) || (exists k:Nat.((val(k < 3)) || (forall m:Nat.((val(m < 3)) && (X0(k + 1, n < 3))))))));                                               \n"
  "mu X1 = (((!((val(true)) => (X2))) => (!(!(!(!X1))))) && (((val(false)) || (X3(1))) && (forall n:Nat.((val(n < 3)) && (val(true)))))) || (forall k:Nat.((val(k < 3)) && (!(forall k:Nat.((val(k < 3)) && (exists m:Nat.((val(m < 3)) || (val(k < 2)))))))));                                                                                \n"
  "mu X2 = ((exists k:Nat.((val(k < 3)) || (exists k:Nat.((val(k < 3)) || (exists m:Nat.((val(m < 3)) || ((val(m == k)) => (X1)))))))) => (((val(true)) => (!X2)) || (exists k:Nat.((val(k < 3)) || (val(false)))))) => (!((val(false)) => (!(X3(0)))));                                                                                \n"
  "mu X3(n:Nat) = ((exists n:Nat.((val(n < 3)) || (exists n:Nat.((val(n < 3)) || (forall n:Nat.((val(n < 3)) && (val(false)))))))) && (forall m:Nat.((val(m < 3)) && (((forall m:Nat.((val(m < 3)) && (exists m:Nat.((val(m < 3)) || (exists m:Nat.((val(m < 3)) || (!(val(n > 0))))))))) && ((!X4(m > 1)) || (!X2))) || (val(n < 2)))))) => ((val(n < 2)) => (X1)); \n"
  "nu X4(b:Bool) = ((val(true)) => ((forall m:Nat.((val(m < 3)) && (val(false)))) => ((X1) || ((!X3(0)) => (forall n:Nat.((val(n < 3)) && (val(n > 1)))))))) && (forall m:Nat.((val(m < 3)) && (forall n:Nat.((val(n < 3)) && ((X2) && (val(false)))))));                                                                                \n"
  "                                                                                \n"
  "init X0(0, true);                                                                                \n"
  ;

void test_pbes(const std::string& pbes_spec, bool test_finite, bool test_lazy)
{
  pbes p = txt2pbes(pbes_spec);
  std::cout << "------------------------------\n" << pbes_system::pp(p) << std::endl;
  if (!p.is_closed())
  {
    std::cout << "ERROR: the pbes is not closed!" << std::endl;
    return;
  }

  if (test_finite)
  {
    std::cout << "FINITE" << std::endl;
    try
    {
      using namespace pbes_system;
      pbes q1 = ::pbesinst_finite(p);
      std::cout << pbes_system::pp(q1) << std::endl;
    }
    catch (mcrl2::runtime_error e)
    {
      std::cout << "pbesinst failed: " << e.what() << std::endl;
    }
  }

  if (test_lazy)
  {
    std::cout << "LAZY" << std::endl;
    try
    {
      using namespace pbes_system;
      pbes q1 = pbesinst_lazy(p);
      std::cout << pbes_system::pp(q1) << std::endl;
    }
    catch (mcrl2::runtime_error e)
    {
      std::cout << "pbesinst failed: " << e.what() << std::endl;
    }
  }
}

BOOST_AUTO_TEST_CASE(test_pbesinst)
{
  test_pbes(test1, true, false);
  test_pbes(test2, true, true);
  test_pbes(test3, true, false);
  test_pbes(test4, true, true);
  test_pbes(test5, true, true);
  test_pbes(test6, true, true);
  test_pbes(test7, true, true);
  test_pbes(test8, true, true);
  test_pbes(random3, false, true);
}

BOOST_AUTO_TEST_CASE(test_pbesinst_finite)
{
  pbes p = txt2pbes(random3);
  pbes q = pbesinst_finite(p);
  std::cerr << pbes_system::pp(q) << std::endl;

  std::string text =
    "sort D = struct d1 | d2;                                        \n"
    "                                                                \n"
    "pbes                                                            \n"
    "                                                                \n"
    "nu X(d:D) = (val(d == d1) && X(d2)) || (val(d == d2) && X(d1)); \n"
    "                                                                \n"
    "init X(d1);                                                     \n"
    ;
  pbes p1 = txt2pbes(text);
  pbesinst_finite_algorithm algorithm(data::jitty);
  pbesinst_variable_map variable_map = mcrl2::pbes_system::detail::parse_pbes_parameter_map(p1, "X(*:D)");
  algorithm.run(p1, variable_map);
}

BOOST_AUTO_TEST_CASE(test_abp_no_deadlock)
{
  lps::specification spec=remove_stochastic_operators(lps::linearise(lps::detail::ABP_SPECIFICATION()));
  state_formulas::state_formula formula = state_formulas::parse_state_formula(lps::detail::NO_DEADLOCK(), spec);
  bool timed = false;
  pbes p = lps2pbes(spec, formula, timed);
  data::rewriter::strategy rewriter_strategy = data::jitty;
  bool print_equations = true;
  pbes_system::pbesinst_algorithm algorithm(p.data(), rewriter_strategy, print_equations);
  algorithm.run(p);
  pbes q = algorithm.get_result();
  std::cout << "--- ABP ---" << std::endl;
  std::cout << pbes_system::pp(q) << std::endl;
  BOOST_CHECK(is_bes(q));
}

// Example supplied by Tim Willemse, 23-05-2011
BOOST_AUTO_TEST_CASE(test_functions)
{
  std::string text =
    "sort D = struct one | two;           \n"
    "                                     \n"
    "map  f: D -> D;                      \n"
    "                                     \n"
    "eqn  f  =  lambda x: D. one;         \n"
    "                                     \n"
    "pbes nu X(d: D, g: D -> D) =         \n"
    "       forall e: D. X(e, g[e -> e]); \n"
    "                                     \n"
    "init X(one, f);                      \n"
    ;
  pbes p = txt2pbes(text);
  data::rewriter::strategy rewrite_strategy = data::jitty;
  pbesinst_finite_algorithm algorithm(rewrite_strategy);
  mcrl2::pbes_system::detail::pbes_parameter_map parameter_map = mcrl2::pbes_system::detail::parse_pbes_parameter_map(p, "X(*:D)");
  algorithm.run(p, parameter_map);
}

void test_pbesinst_symbolic(const std::string& text)
{
  pbes p;
  p = txt2pbes(text);
  pbesinst_symbolic_algorithm algorithm(p);
  algorithm.run();
}

BOOST_AUTO_TEST_CASE(test_pbesinst_symbolic1)
{
  test_pbesinst_symbolic(test2);
  test_pbesinst_symbolic(test4);
  test_pbesinst_symbolic(test5);
  test_pbesinst_symbolic(test6);
}

#ifdef MCRL2_EXTENDED_TESTS
BOOST_AUTO_TEST_CASE(test_pbesinst_slow)
{
  test_pbes(random1, false, true);
  test_pbes(random2, false, true);
}

// Note: this test takes a lot of time!
BOOST_AUTO_TEST_CASE(test_cabp)
{
    std::string CABP_SPECIFICATION =
            "% This file contains the cabp protocol as described in section 3.5 of         \n"
            "% S. Mauw and G.J. Veltink, editors, Algebraic Specification of Communication \n"
            "% Protocols, Cambridge tracts in theoretical computer science 36, Cambridge   \n"
            "% University Press, Cambridge 1993.                                           \n"
            "%                                                                             \n"
            "% With two data elements, the generated transition system has 464 states.     \n"
            "%                                                                             \n"
            "% It is interesting to see the clustering of this statespace in ltsgraph.     \n"
            "% The statespace after branching bisimulation contains 3 states and is        \n"
            "% exactly the same as the reduced statespace of the alternating bit protocol. \n"
            "%                                                                             \n"
            "% Note that it is interesting to compare the differences of the alternating   \n"
            "% bit protocol (abp), concurrent alternating bit protocol (cabp), one bit     \n"
            "% sliding window protocol (onebit) and the alternating bit protocol with      \n"
            "% independent acknowledgements (par), regarding the implementation, the       \n"
            "% the number of states and the external behaviour.                            \n"
            "                                                                              \n"
            "%-------------------------------  DATA  ----------------------------------    \n"
            "                                                                              \n"
            "sort DATA = struct d1 | d2;                                                   \n"
            "                                                                              \n"
            "%-------------------------------  error  ----------------------------------   \n"
            "                                                                              \n"
            "sort  error = struct ce | ae;                                                 \n"
            "                                                                              \n"
            "%-------------------------------  bit ------------------------------------    \n"
            "                                                                              \n"
            "sort  bit = struct bit0 | bit1;                                               \n"
            "                                                                              \n"
            "map   invert:bit -> bit;                                                      \n"
            "eqn   invert(bit1)=bit0;                                                      \n"
            "      invert(bit0)=bit1;                                                      \n"
            "                                                                              \n"
            "%-------------------------------  Frame ----------------------------------    \n"
            "                                                                              \n"
            "sort Frame = struct frame(getd : DATA, getb: bit);                            \n"
            "                                                                              \n"
            "%------------------------------  ACK   -----------------------------------    \n"
            "                                                                              \n"
            "sort ACK = struct ac;                                                         \n"
            "                                                                              \n"
            "%------------------------------  act   -----------------------------------    \n"
            "                                                                              \n"
            "act   r1,s2 : DATA;                                                           \n"
            "      c3,r3,s3,c4,r4,s4 : Frame;                                              \n"
            "      c4,r4,s4,c7,r7,s7 : error;                                              \n"
            "      c5,r5,s5,c8,r8,s8 : ACK;                                                \n"
            "      c6,r6,s6,c7,r7,s7 : bit;                                                \n"
            "      skip;                                                                   \n"
            "                                                                              \n"
            "%------------------------------  proc  -----------------------------------    \n"
            "                                                                              \n"
            "                                                                              \n"
            "proc  S = RM(bit0);                                                           \n"
            "      RM(b:bit) = sum d:DATA.r1(d).SF(frame(d,b));                            \n"
            "      SF(f:Frame) = s3(f).SF(f) + r8(ac).RM(invert(getb(f)));                 \n"
            "                                                                              \n"
            "      K  = sum f:Frame.r3(f).K(f);                                            \n"
            "      K(f:Frame) = (skip.s4(f)+skip.s4(ce)+skip).K;                           \n"
            "                                                                              \n"
            "      R = RF(bit0);                                                           \n"
            "      RF(b:bit) = sum d:DATA.r4(frame(d,b)).s2(d).s5(ac).RF(invert(b))        \n"
            "                     + sum d:DATA. r4(frame(d,invert(b))).RF(b)               \n"
            "                     + r4(ce).RF(b);                                          \n"
            "                                                                              \n"
            "      AS = AS(bit1);                                                          \n"
            "      AS(b:bit) = r5(ac).AS(invert(b)) + s6(b).AS(b);                         \n"
            "                                                                              \n"
            "      L = sum b:bit.r6(b) . L(b);                                             \n"
            "      L(b:bit) = ( skip.s7(b) + skip.s7(ae) + skip ).L;                       \n"
            "                                                                              \n"
            "      AR = AR(bit0);                                                          \n"
            "      AR(b:bit) = ( r7(ae) + r7(invert(b))) . AR(b)                           \n"
            "                   + r7(b).s8(ac).AR(invert(b));                              \n"
            "                                                                              \n"
            "init                                                                          \n"
            "   hide({c3,c4,c5,c6,c7,c8,skip},                                             \n"
            "     allow({c3,c4,c5,c6,c7,c8,skip,r1,s2},                                    \n"
            "       comm({r3|s3->c3, r4|s4->c4, r5|s5->c5, r6|s6->c6,                      \n"
            "                r7|s7->c7, r8|s8->c8},                                        \n"
            "               S || K || R || AS || L || AR )));                              \n"
    ;

    std::string INFINITELY_OFTEN_SEND = "nu X. mu Y. (<r1(d1)>X || <!r1(d1)>Y)";

    // create a pbes p
    lps::specification spec = remove_stochastic_operators(lps::linearise(CABP_SPECIFICATION));
    state_formulas::state_formula formula = state_formulas::parse_state_formula(INFINITELY_OFTEN_SEND, spec);
    bool timed = false;
    pbes p = lps2pbes(spec, formula, timed);
    pbes_system::detail::instantiate_global_variables(p);

    std::string expr = "(exists d_RM_00: DATA. (val(d_RM_00 == d1) && val(s31_RM == 1)) && X(2, bit0, frame(d_RM_00, b_RM), s32_K, f_K, s33_RF1, d_RF1, b_RF1, b_AS1, s35_L, b_L, s36_AR1, b_AR1)) || (((((((((((exists d_RM_00: DATA. (val(!(d_RM_00 == d1)) && val(s31_RM == 1)) && Y(2, bit0, frame(d_RM_00, b_RM), s32_K, f_K, s33_RF1, d_RF1, b_RF1, b_AS1, s35_L, b_L, s36_AR1, b_AR1)) || (exists e4_RF1_00: Bool, d4_RF1_00: DATA. val((s32_K == 3 && s33_RF1 == 1) && if(e4_RF1_00, frame(d4_RF1_00, invert(b_RF1)), frame(d4_RF1_00, b_RF1)) == f_K) && Y(s31_RM, b_RM, f_RM, 1, frame(d1, bit0), if(e4_RF1_00, 1, 2), if(e4_RF1_00, d1, d4_RF1_00), b_RF1, b_AS1, s35_L, b_L, s36_AR1, b_AR1))) || val(s32_K == 4 && s33_RF1 == 1) && Y(s31_RM, b_RM, f_RM, 1, frame(d1, bit0), 1, d1, b_RF1, b_AS1, s35_L, b_L, s36_AR1, b_AR1)) || val(s33_RF1 == 2) && Y(s31_RM, b_RM, f_RM, s32_K, f_K, 3, d1, b_RF1, b_AS1, s35_L, b_L, s36_AR1, b_AR1)) || val(s35_L == 1) && Y(s31_RM, b_RM, f_RM, s32_K, f_K, s33_RF1, d_RF1, b_RF1, b_AS1, 2, b_AS1, s36_AR1, b_AR1)) || (exists e5_L_00: Enum3. val(C3_fun2(e5_L_00, true, true, true) && s35_L == 2) && Y(s31_RM, b_RM, f_RM, s32_K, f_K, s33_RF1, d_RF1, b_RF1, b_AS1, C3_fun(e5_L_00, 1, 3, 4), C3_fun3(e5_L_00, bit0, b_L, bit0), s36_AR1, b_AR1))) || val(s35_L == 4 && s36_AR1 == 1) && Y(s31_RM, b_RM, f_RM, s32_K, f_K, s33_RF1, d_RF1, b_RF1, b_AS1, 1, bit0, 1, b_AR1)) || (exists e7_AR1_00: Bool. val((s35_L == 3 && s36_AR1 == 1) && if(e7_AR1_00, b_AR1, invert(b_AR1)) == b_L) && Y(s31_RM, b_RM, f_RM, s32_K, f_K, s33_RF1, d_RF1, b_RF1, b_AS1, 1, bit0, if(e7_AR1_00, 2, 1), b_AR1))) || val(s33_RF1 == 3) && Y(s31_RM, b_RM, f_RM, s32_K, f_K, 1, d1, invert(b_RF1), invert(b_AS1), s35_L, b_L, s36_AR1, b_AR1)) || (exists e_K_00: Enum3. val(C3_fun2(e_K_00, true, true, true) && s32_K == 2) && Y(s31_RM, b_RM, f_RM, C3_fun(e_K_00, 1, 3, 4), C3_fun1(e_K_00, frame(d1, bit0), f_K, frame(d1, bit0)), s33_RF1, d_RF1, b_RF1, b_AS1, s35_L, b_L, s36_AR1, b_AR1))) || val(s31_RM == 2 && s32_K == 1) && Y(2, bit0, f_RM, 2, f_RM, s33_RF1, d_RF1, b_RF1, b_AS1, s35_L, b_L, s36_AR1, b_AR1)) || val(s31_RM == 2 && s36_AR1 == 2) && Y(1, invert(getb(f_RM)), frame(d1, bit0), s32_K, f_K, s33_RF1, d_RF1, b_RF1, b_AS1, s35_L, b_L, 1, invert(b_AR1))";
    std::string subst = "s31_RM:Pos := 2; b_RM:bit := bit0; f_RM:Frame := frame(d2, bit0); s32_K:Pos := 3; f_K:Frame := frame(d2, bit0); s33_RF1:Pos := 1; d_RF1:DATA := d1; b_RF1:bit := bit0; b_AS1:bit := bit1; s35_L:Pos := 1; b_L:bit := bit0; s36_AR1:Pos := 1; b_AR1:bit := bit0";

    data::mutable_map_substitution<> sigma;
    pbes_expression t = parse_pbes_expression(expr, subst, p, sigma);
    pbesinst_algorithm algorithm(p.data());
    enumerate_quantifiers_rewriter& R = algorithm.rewriter();
    data::mutable_indexed_substitution<> sigma1;
    for (const auto& i: sigma)
    {
        sigma1[i.first] = i.second;
    }
    pbes_expression z = R(t, sigma1);
}

// Note: this test takes a lot of time!
BOOST_AUTO_TEST_CASE(test_balancing_plat)
{
    using namespace pbes_system;

    const std::string BALANCE_PLAT_SPECIFICATION =
            " % Specification of balancing coins to determine the single coin with                 \n"
            " % different weight.                                                                  \n"
            "                                                                                \n"
            " % C: Total number of coins                                                           \n"
            " map  C: Nat;                                                                         \n"
            " eqn  C = 12;                                                                         \n"
            "                                                                                \n"
            " % Every coin can be in one of four categories: NHL, NH, NL, and N,                   \n"
            " % where:                                                                             \n"
            " % N: possibly normal weight                                                          \n"
            " % H: possibly heavy weight                                                           \n"
            " % L: possibly light weight                                                           \n"
            " % We count the number of coins in every category, but we do not count                \n"
            " % the number of coins in N explicitly, because:                                      \n"
            " %   |N| = C - ( |NHL| + |NH| + |NL| )                                                \n"
            "                                                                                \n"
            " map                                                                                \n"
            "      lexleq: Nat # Nat # Nat # Nat # Nat # Nat -> Bool;                              \n"
            "      is_better: Nat # Nat # Nat # Nat # Nat # Nat -> Bool;                           \n"
            "      is_useful: Nat # Nat # Nat # Nat # Nat # Nat # Nat # Nat # Nat -> Bool;         \n"
            "      is_possible: Nat # Nat # Nat # Nat # Nat # Nat # Nat # Nat # Nat -> Bool;       \n"
            "                                                                                \n"
            " var  d1,d2,d3,e1,e2,e3,f1,f2,f3: Nat;                                                \n"
            "                                                                                \n"
            " eqn                                                                                \n"
            "      % lexicographical ordening on distributions; this is needed to                  \n"
            "      % eliminate half of the possibilities for balancing: only consider              \n"
            "      % X vs. Y and not Y vs. X, if X <= Y.                                           \n"
            "      lexleq(d1,d2,d3,e1,e2,e3) =                                                     \n"
            "          d1<e1 || (d1==e1 && d2<e2) || (d1==e1 && d2==e2 && d3<=e3);                 \n"
            "                                                                                \n"
            "      % determines whether a distribution d is 'better than' a                        \n"
            "      % distribution e, in the sense that in d we have more certainty (or             \n"
            "      % less uncertainty) about a larger number of coins                              \n"
            "      is_better(d1,d2,d3,e1,e2,e3) = d1+d2+d3 < e1+e2+e3 || d1 < e1;                  \n"
            "                                                                                \n"
            "      % determines whether weighing e against f is useful in situation d:             \n"
            "      % all possible outcomes should be an improvement                                \n"
            "      is_useful(d1,d2,d3,e1,e2,e3,f1,f2,f3) =                                         \n"
            "        is_better(Int2Nat(d1-e1-f1),Int2Nat(d2-e2-f2),Int2Nat(d3-e3-f3),d1,d2,d3) &&  \n"
            "        is_better(0,e1+e2,f1+f3,d1,d2,d3) &&                                          \n"
            "        is_better(0,f1+f2,e1+e3,d1,d2,d3);                                            \n"
            "                                                                                \n"
            "      % determines whether weighing e against f is possible in situation              \n"
            "      % d:                                                                            \n"
            "      % - for every category X: X(e) + X(f) <= X(d)                                   \n"
            "      % - if total(e) < total(f) then N 'normal' coins are added to e                 \n"
            "      %   such that N = total(f) - total(e), so N 'normal' coins must                 \n"
            "      %   be available in situation d, i.e. N <= C - total(d).                        \n"
            "      % - analogously if total(e) > total(f).                                         \n"
            "      is_possible(d1,d2,d3,e1,e2,e3,f1,f2,f3) =                                       \n"
            "        e1+f1 <= d1 && e2+f2 <= d2 && e3+f3 <= d3 &&                                  \n"
            "        ( e1+e2+e3 == f1+f2+f3 ||                                                     \n"
            "          (e1+e2+e3 < f1+f2+f3 && f1+f2+f3 - e1-e2-e3 <= C - d1-d2-d3) ||             \n"
            "          (f1+f2+f3 < e1+e2+e3 && e1+e2+e3 - f1-f2-f3 <= C - d1-d2-d3)                \n"
            "        );                                                                            \n"
            "                                                                                \n"
            " act  weigh, equal, greater, smaller: Nat # Nat # Nat # Nat # Nat # Nat;              \n"
            "      done;                                                                           \n"
            "                                                                                \n"
            " proc BalancingAct(NHL,NH,NL:Nat) =                                                   \n"
            "      % we're done if |NHL| + |NH| + |NL| == 1                                        \n"
            "        (NHL+NH+NL == 1) -> done . BalancingAct(NHL,NH,NL)                            \n"
            "                                                                                \n"
            "      + (NHL+NH+NL >  1) ->                                                           \n"
            "          (                                                                           \n"
            "            sum nhl_l,nh_l,nl_l:Nat, nhl_r,nh_r,nl_r:Nat .                            \n"
            "                                                                                \n"
            "            (lexleq(nhl_l,nh_l,nl_l,nhl_r,nh_r,nl_r) &&                               \n"
            "            is_possible(NHL,NH,NL,nhl_l,nh_l,nl_l,nhl_r,nh_r,nl_r) &&                 \n"
            "            is_useful(NHL,NH,NL,nhl_l,nh_l,nl_l,nhl_r,nh_r,nl_r)) ->                  \n"
            "                                                                                \n"
            "              weigh(nhl_l,nh_l,nl_l,nhl_r,nh_r,nl_r) .                                \n"
            "              (                                                                       \n"
            "              % left and right have equal weight                                      \n"
            "              ((NHL-nhl_l-nhl_r + NH-nh_l-nh_r + NL-nl_l-nl_r > 0) ->                 \n"
            "                equal(nhl_l,nh_l,nl_l,nhl_r,nh_r,nl_r) .                              \n"
            "                  BalancingAct(Int2Nat(NHL-nhl_l-nhl_r),                              \n"
            "                               Int2Nat(NH-nh_l-nh_r),                                 \n"
            "                               Int2Nat(NL-nl_l-nl_r)))                                \n"
            "              +                                                                       \n"
            "              % left is heavier than right                                            \n"
            "              ((nhl_l+nh_l + nhl_r+nl_r > 0) ->                                       \n"
            "                greater(nhl_l,nh_l,nl_l,nhl_r,nh_r,nl_r) .                            \n"
            "                  BalancingAct(0,nhl_l+nh_l,nhl_r+nl_r))                              \n"
            "              +                                                                       \n"
            "              % left is lighter than right                                            \n"
            "              ((nhl_r+nh_r + nhl_l+nl_l > 0) ->                                       \n"
            "                smaller(nhl_l,nh_l,nl_l,nhl_r,nh_r,nl_r) .                            \n"
            "                  BalancingAct(0,nhl_r+nh_r,nhl_l+nl_l))                              \n"
            "              )                                                                       \n"
            "          );                                                                          \n"
            "                                                                                \n"
            " init BalancingAct(C,0,0);                                                            \n"
    ;

    lps::specification spec = remove_stochastic_operators(lps::linearise(BALANCE_PLAT_SPECIFICATION));
    state_formulas::state_formula formula = state_formulas::parse_state_formula(lps::detail::NO_DEADLOCK(), spec);
    bool timed = false;
    pbes p = lps2pbes(spec, formula, timed);
    pbes_system::pbesinst_algorithm algorithm(p.data());
    algorithm.run(p);
    pbes q = algorithm.get_result();
}
#endif // MCRL2_EXTENDED_TESTS


#pragma once

#include <Eigen/Core>
#include <imgui/imgui.h>
#include <limits>
#include <nlohmann/json.hpp>

#define CCD_IM_ARRAYSIZE(_ARR) (int(sizeof(_ARR) / sizeof(*_ARR)))

namespace ImGui {

bool InputIntBounded(
    const char* label,
    int* val,
    int lower_bound = std::numeric_limits<int>::min(),
    int upper_bound = std::numeric_limits<int>::max(),
    int step = 1,
    int step_fast = 100,
    ImGuiInputTextFlags flags = 0);

bool InputDoubleBounded(
    const char* label,
    double* val,
    double lower_bound = -std::numeric_limits<double>::infinity(),
    double upper_bound = std::numeric_limits<double>::infinity(),
    double step = 1,
    double step_fast = 100,
    const char* format = "%.6f",
    ImGuiInputTextFlags flags = 0);

bool DragDouble(
    const char* label,
    double* v,
    double v_speed = 1.0,
    double v_min = 0.0,
    double v_max = 0.0,
    const char* format = "%.3f",
    float power = 1.0f); // If v_min >= v_max we have no bound

void TreeNodeJson(const nlohmann::json json);
bool DoubleColorEdit3(const char* label, Eigen::RowVector3d& color);
void HelpMarker(const char* desc);
} // namespace ImGui


#include "wire_mesh.h"

#include "../../list_to_matrix.h"
#include "../../slice.h"
#include "../../PI.h"
#include "convex_hull.h"
#include "mesh_boolean.h"
#include <Eigen/Geometry>
#include <vector>

template <
  typename DerivedWV,
  typename DerivedWE,
  typename DerivedV,
  typename DerivedF,
  typename DerivedJ>
IGL_INLINE void igl::copyleft::cgal::wire_mesh(
  const Eigen::MatrixBase<DerivedWV> & WV,
  const Eigen::MatrixBase<DerivedWE> & WE,
  const double th,
  const int poly_size,
  const bool solid,
  Eigen::PlainObjectBase<DerivedV> & V,
  Eigen::PlainObjectBase<DerivedF> & F,
  Eigen::PlainObjectBase<DerivedJ> & J)
{

  typedef typename DerivedWV::Scalar Scalar;
  // Canonical polygon to place at each endpoint
  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,3> MatrixX3S;
  MatrixX3S PV(poly_size,3);
  for(int p =0;p<PV.rows();p++)
  {
    const Scalar phi = (Scalar(p)/Scalar(PV.rows()))*2.*igl::PI;
    PV(p,0) = 0.5*cos(phi);
    PV(p,1) = 0.5*sin(phi);
    PV(p,2) = 0;
  }

  V.resize(WV.rows() + PV.rows() * 2 * WE.rows(),3);
  V.topLeftCorner(WV.rows(),3) = WV;
  // Signed adjacency list
  std::vector<std::vector<std::pair<int,int> > > A(WV.rows());
  // Inputs:
  //   e  index of edge
  //   c  index of endpoint [0,1]
  //   p  index of polygon vertex
  // Returns index of corresponding vertex in V
  const auto index =
    [&PV,&WV](const int e, const int c, const int p)->int
  {
    return WV.rows() + e*2*PV.rows() + PV.rows()*c + p;
  };
  const auto unindex =
    [&PV,&WV](int v, int & e, int & c, int & p)
  {
    assert(v>=WV.rows());
    v = v-WV.rows();
    e = v/(2*PV.rows());
    v = v-e*(2*PV.rows());
    c = v/(PV.rows());
    v = v-c*(PV.rows());
    p = v;
  };
  // loop over all edges
  for(int e = 0;e<WE.rows();e++)
  {
    // Fill in adjacency list as we go
    A[WE(e,0)].emplace_back(e,0);
    A[WE(e,1)].emplace_back(e,1);
    typedef Eigen::Matrix<Scalar,1,3> RowVector3S;
    const RowVector3S ev = WV.row(WE(e,1))-WV.row(WE(e,0));
    const Scalar len = ev.norm();
    // Unit edge vector
    const RowVector3S uv = ev.normalized();
    Eigen::Quaternion<Scalar> q;
    q = q.FromTwoVectors(RowVector3S(0,0,1),uv);
    // loop over polygon vertices
    for(int p = 0;p<PV.rows();p++)
    {
      RowVector3S qp = q*(PV.row(p)*th);
      // loop over endpoints
      for(int c = 0;c<2;c++)
      {
        // Direction moving along edge vector
        const Scalar dir = c==0?1:-1;
        // Amount (distance) to move along edge vector
        // Start with factor of thickness;
        // Max out amount at 1/3 of edge length so that there's always some
        // amount of edge
        Scalar dist = std::min(1.*th,len/3.0);
        // Move to endpoint, offset by amount
        V.row(index(e,c,p)) =
          qp+WV.row(WE(e,c)) + dist*dir*uv;
      }
    }
  }

  std::vector<std::vector<typename DerivedF::Index> > vF;
  std::vector<int> vJ;
  const auto append_hull =
    [&V,&vF,&vJ,&unindex,&WV](const Eigen::VectorXi & I, const int j)
  {
    MatrixX3S Vv;
    igl::slice(V,I,1,Vv);
    Eigen::MatrixXi Fv;
    convex_hull(Vv,Fv);
    for(int f = 0;f<Fv.rows();f++)
    {
      const Eigen::Array<int,1,3> face(I(Fv(f,0)), I(Fv(f,1)), I(Fv(f,2)));
      //const bool on_vertex = (face<WV.rows()).any();
      //if(!on_vertex)
      //{
      //  // This correctly prunes fcaes on the "caps" of convex hulls around
      //  // edges, but for convex hulls around vertices this will only work if
      //  // the incoming edges are not overlapping.
      //  //
      //  // Q: For convex hulls around vertices, is the correct thing to do:
      //  // check if all corners of face lie *on or _outside_* of plane of "cap"?
      //  //
      //  // H: Maybe, but if there's an intersection then the boundary of the
      //  // incoming convex hulls around edges is still not going to match up
      //  // with the boundary on the convex hull around the vertices.
      //  //
      //  // Might have to bite the bullet and always call self-union.
      //  bool all_same = true;
      //  int e0,c0,p0;
      //  unindex(face(0),e0,c0,p0);
      //  for(int i = 1;i<3;i++)
      //  {
      //    int ei,ci,pi;
      //    unindex(face(i),ei,ci,pi);
      //    all_same = all_same && (e0==ei && c0==ci);
      //  }
      //  if(all_same)
      //  {
      //    // don't add this face
      //    continue;
      //  }
      //}
      vF.push_back( { face(0),face(1),face(2)});
      vJ.push_back(j);
    }
  };
  // loop over each vertex
  for(int v = 0;v<WV.rows();v++)
  {
    // Gather together this vertex and the polygon vertices of all incident
    // edges
    Eigen::VectorXi I(1+A[v].size()*PV.rows());
    // This vertex
    I(0) = v;
    for(int n = 0;n<A[v].size();n++)
    {
      for(int p = 0;p<PV.rows();p++)
      {
        const int e = A[v][n].first;
        const int c = A[v][n].second;
        I(1+n*PV.rows()+p) = index(e,c,p);
      }
    }
    append_hull(I,v);
  }
  // loop over each edge
  for(int e = 0;e<WE.rows();e++)
  {
    // Gether together polygon vertices of both endpoints
    Eigen::VectorXi I(PV.rows()*2);
    for(int c = 0;c<2;c++)
    {
      for(int p = 0;p<PV.rows();p++)
      {
        I(c*PV.rows()+p) = index(e,c,p);
      }
    }
    append_hull(I,WV.rows()+e);
  }

  list_to_matrix(vF,F);
  if(solid)
  {
    // Self-union to clean up
    igl::copyleft::cgal::mesh_boolean(
      Eigen::MatrixXd(V),Eigen::MatrixXi(F),Eigen::MatrixXd(),Eigen::MatrixXi(),
      "union",
      V,F,J);
    for(int j=0;j<J.size();j++) J(j) = vJ[J(j)];
  }else
  {
    list_to_matrix(vJ,J);
  }
}

template <
  typename DerivedWV,
  typename DerivedWE,
  typename DerivedV,
  typename DerivedF,
  typename DerivedJ>
IGL_INLINE void igl::copyleft::cgal::wire_mesh(
  const Eigen::MatrixBase<DerivedWV> & WV,
  const Eigen::MatrixBase<DerivedWE> & WE,
  const double th,
  const int poly_size,
  Eigen::PlainObjectBase<DerivedV> & V,
  Eigen::PlainObjectBase<DerivedF> & F,
  Eigen::PlainObjectBase<DerivedJ> & J)
{
  return wire_mesh(WV,WE,th,poly_size,true,V,F,J);
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
template void igl::copyleft::cgal::wire_mesh<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, 1, 0, -1, 1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, double, int, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> >&);
#endif


#include "lwtnn/LightweightNeuralNetwork.hh"
#include "lwtnn/parse_json.hh"
#include "lwtnn/NanReplacer.hh"

#include <Eigen/Dense>

#include <iostream>
#include <string>

int main(int argc, char* argv[]) {
  // Read in the configuration. In Athena you should be able to replace
  // std::cin with a stringstream containing the text file
  auto config = lwt::parse_json(std::cin);

  // initialize the tagger from the configuration
  lwt::LightweightNeuralNetwork tagger(
    config.inputs, config.layers, config.outputs);
  lwt::NanReplacer replacer(config.defaults);

  // build some dummy inputs and feed them to the tagger
  lwt::ValueMap input{
    {"in1", 1}, {"in2", 2}, {"in3", NAN}, {"in4", 4} };
  auto cleaned_inputs = replacer.replace(input);
  auto out = tagger.compute(cleaned_inputs);

  // look at the outputs
  for (const auto& op: out) {
    std::cout << op.first << " " << op.second << std::endl;
  }

  return 0;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2011 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is 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)

#ifndef BOOST_GEOMETRY_GEOMETRY_POLICIES_RELATE_DE9IM_HPP
#define BOOST_GEOMETRY_GEOMETRY_POLICIES_RELATE_DE9IM_HPP

#include <boost/geometry/strategies/intersection_result.hpp>
#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/select_coordinate_type.hpp>

namespace boost { namespace geometry
{

namespace policies { namespace relate
{

template <typename S1, typename S2>
struct segments_de9im
{
    typedef de9im_segment return_type;
    typedef S1 segment_type1;
    typedef S2 segment_type2;
    typedef typename select_coordinate_type<S1, S2>::type coordinate_type;

    static inline return_type rays_intersect(bool on_segment,
                    double ra, double rb,
                    coordinate_type const& dx1, coordinate_type const& dy1,
                    coordinate_type const& dx2, coordinate_type const& dy2,
                    coordinate_type const& wx, coordinate_type const& wy,
                    S1 const& s1, S2 const& s2)
    {
        if(on_segment)
        {
            // 0 <= ra <= 1 and 0 <= rb <= 1
            // Now check if one of them is 0 or 1, these are "touch" cases
            bool a = math::equals(ra, 0.0) || math::equals(ra, 1.0);
            bool b = math::equals(rb, 0.0) || math::equals(rb, 1.0);
            if (a && b)
            {
                // Touch boundary/boundary: i-i == -1, i-b == -1, b-b == 0
                // Opposite: if both are equal they touch in opposite direction
                return de9im_segment(ra,rb,
                        -1, -1, 1,
                        -1,  0, 0,
                         1,  0, 2, false, math::equals(ra,rb));
            }
            else if (a || b)
            {
                // Touch boundary/interior: i-i == -1, i-b == -1 or 0, b-b == -1
                int A = a ? 0 : -1;
                int B = b ? 0 : -1;
                return de9im_segment(ra,rb,
                        -1,  B, 1,
                         A, -1, 0,
                         1,  0, 2);
            }

            // Intersects: i-i == 0, i-b == -1, i-e == 1
            return de9im_segment(ra,rb,
                     0, -1, 1,
                    -1, -1, 0,
                     1,  0, 2);
        }

        // Not on segment, disjoint
        return de9im_segment(ra,rb,
                -1, -1, 1,
                -1, -1, 0,
                 1,  0, 2);
    }

    static inline return_type collinear_touch(coordinate_type const& x,
                coordinate_type const& y, bool opposite, char)
    {
        return de9im_segment(0,0,
                -1, -1, 1,
                -1,  0, 0,
                 1,  0, 2,
                true, opposite);
    }

    template <typename S>
    static inline return_type collinear_interior_boundary_intersect(S const& s,
                bool a_within_b, bool opposite)
    {
        return a_within_b
            ? de9im_segment(0,0,
                 1, -1, -1,
                 0,  0, -1,
                 1,  0, 2,
                true, opposite)
            : de9im_segment(0,0,
                 1,  0, 1,
                -1,  0, 0,
                -1, -1, 2,
                true, opposite);
    }

    static inline return_type collinear_a_in_b(S1 const& s, bool opposite)
    {
        return de9im_segment(0,0,
                1, -1, -1,
                0, -1, -1,
                1,  0,  2,
                true, opposite);
    }
    static inline return_type collinear_b_in_a(S2 const& s, bool opposite)
    {
        return de9im_segment(0,0,
                 1,  0,  1,
                -1, -1,  0,
                -1, -1,  2,
                true, opposite);
    }

    static inline return_type collinear_overlaps(
                    coordinate_type const& x1, coordinate_type const& y1,
                    coordinate_type const& x2, coordinate_type const& y2, bool opposite)
    {
        return de9im_segment(0,0,
                1,  0, 1,
                0, -1, 0,
                1,  0, 2,
                true, opposite);
    }

    static inline return_type segment_equal(S1 const& s, bool opposite)
    {
        return de9im_segment(0,0,
                 1, -1, -1,
                -1,  0, -1,
                -1, -1,  2,
                 true, opposite);
    }

    static inline return_type degenerate(S1 const& segment, bool a_degenerate)
    {
            return a_degenerate
                ? de9im_segment(0,0,
                     0, -1, -1,
                    -1, -1, -1,
                     1,  0,  2,
                     false, false, false, true)
                : de9im_segment(0,0,
                     0, -1,  1,
                    -1, -1,  0,
                    -1, -1,  2,
                     false, false, false, true);
    }

    static inline return_type collinear_disjoint()
    {
        return de9im_segment(0,0,
                -1, -1, 1,
                -1, -1, 0,
                 1,  0, 2,
                true);
    }

    static inline return_type parallel()
    {
        return de9im_segment(0,0,
                -1, -1, 1,
                -1, -1, 0,
                 1,  0, 2, false, false, true);
    }
};

}} // namespace policies::relate

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_GEOMETRY_POLICIES_RELATE_DE9IM_HPP


//---------------------------------------------------------------------------//
// Copyright (c) 2021 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2021 Ilias Khairullin <ilias@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_PUBKEY_SSS_BASIC_TYPES_HPP
#define CRYPTO3_PUBKEY_SSS_BASIC_TYPES_HPP

#include <utility>
#include <set>
#include <type_traits>
#include <iterator>

#include <boost/assert.hpp>
#include <boost/concept_check.hpp>

#include <boost/range/concepts.hpp>

namespace nil {
    namespace crypto3 {
        namespace pubkey {
            template<typename Group>
            struct sss_basic_policy {
                //===========================================================================
                // internal secret sharing scheme types

                using private_element_type = typename Group::curve_type::scalar_field_type::value_type;
                using public_element_type = typename Group::value_type;
                using indexed_private_element_type = std::pair<std::size_t, private_element_type>;
                using indexed_public_element_type = std::pair<std::size_t, public_element_type>;

                //===========================================================================
                // public secret sharing scheme types

                using coeff_type = private_element_type;
                using public_coeff_type = public_element_type;
                using indexes_type = std::set<std::size_t>;

                //===========================================================================
                // general purposes functions

                static inline bool check_minimal_size(std::size_t size) {
                    return size >= 2;
                }

                static inline std::size_t get_min_threshold_value(std::size_t n) {
                    assert(check_minimal_size(n));

                    return (n + 1) / 2;
                }

                static inline bool check_participant_index(std::size_t i) {
                    return i > 0;
                }

                static inline bool check_participant_index(std::size_t i, std::size_t n) {
                    return check_participant_index(i) && i <= n;
                }

                static inline bool check_threshold_value(std::size_t t, std::size_t n) {
                    return check_minimal_size(t) && n >= t && t >= get_min_threshold_value(n);
                }

                static inline bool check_exp(std::size_t exp) {
                    return exp >= 0;
                }

                //===========================================================================
                // TODO: refactor
                static inline public_element_type get_public_element(const private_element_type &e) {
                    return e * public_element_type::one();
                }

                template<typename IndexedElementIt>
                static inline indexes_type get_indexes(IndexedElementIt first, IndexedElementIt last) {
                    BOOST_CONCEPT_ASSERT((boost::InputIteratorConcept<IndexedElementIt>));

                    indexes_type indexes;
                    for (auto it = first; it != last; ++it) {
                        bool emplace_status = indexes.emplace(it->get_index()).second;
                        assert(check_participant_index(it->get_index()) && emplace_status);
                    }

                    return indexes;
                }
            };
        }    // namespace pubkey
    }        // namespace crypto3
}    // namespace nil

#endif    // CRYPTO3_PUBKEY_SSS_BASIC_TYPES_HPP


#define BOOST_TEST_MAIN TestTokens
#include <boost/test/unit_test.hpp>
#include <stdexcept>
#include <stack>
#include <tuple>
#include <sam/Tokens.hpp>
#include <sam/VastNetflow.hpp>
#include <sam/FeatureMap.hpp>

using namespace sam;

struct F {
  VastNetflow netflow;
  std::shared_ptr<FeatureMap> featureMap = std::make_shared<FeatureMap>();
  std::stack<double> mystack;
  std::string key;

  F() {
    std::string s = "1,1,1365582756.384094,2013-04-10 08:32:36,"
                    "20130410083236.384094,17,UDP,172.20.2.18,"
                    "239.255.255.250,29986,1900,0,0,0,133,0,1,0,1,0,0";
    netflow = makeNetflow(s);
    key = "key";
  }

  ~F() {
  }

};

BOOST_FIXTURE_TEST_CASE( test_number_token, F )
{
  NumberToken<VastNetflow> number(featureMap, DestIp);

  bool b = number.evaluate(mystack, key, netflow);
  BOOST_CHECK_EQUAL(b, true);
  BOOST_CHECK_EQUAL(mystack.top(), DestIp);

}

BOOST_FIXTURE_TEST_CASE( test_add_token, F )
{
  mystack.push(1.6);
  mystack.push(3.5);

  AddOperator<VastNetflow> addOper(featureMap);

  bool b = addOper.evaluate(mystack, key, netflow);
  BOOST_CHECK_EQUAL(b, true);
  BOOST_CHECK_EQUAL(mystack.top(), 5.1);

  b = addOper.evaluate(mystack, key, netflow);
  BOOST_CHECK_EQUAL(b, false);

}

BOOST_FIXTURE_TEST_CASE( test_sub_token, F )
{
  mystack.push(1.6);
  mystack.push(3.5);

  SubOperator<VastNetflow> subOper(featureMap);

  bool b = subOper.evaluate(mystack, key, netflow);
  BOOST_CHECK_EQUAL(b, true);
  BOOST_CHECK_EQUAL(mystack.top(), -1.9);

  b = subOper.evaluate(mystack, key, netflow);
  BOOST_CHECK_EQUAL(b, false);

}

BOOST_FIXTURE_TEST_CASE( test_mult_token, F )
{
  mystack.push(3);
  mystack.push(2);

  MultOperator<VastNetflow> multOper(featureMap);

  bool b = multOper.evaluate(mystack, key, netflow);
  BOOST_CHECK_EQUAL(b, true);
  BOOST_CHECK_EQUAL(mystack.top(), 6);

  b = multOper.evaluate(mystack, key, netflow);
  BOOST_CHECK_EQUAL(b, false);

}

BOOST_FIXTURE_TEST_CASE( test_field_token, F )
{

  FieldToken<TimeSeconds, VastNetflow> fieldToken(featureMap);

  bool b = fieldToken.evaluate(mystack, key, netflow);
  BOOST_CHECK_EQUAL(b, true);
  BOOST_CHECK_EQUAL(mystack.top(), 1365582756.384094);
}

BOOST_FIXTURE_TEST_CASE( test_func_token, F )
{
  std::string identifier = "top2";
  std::string function   = "value";
  std::vector<double> parameters;
  parameters.push_back(1);

  auto func = [&function, &parameters](Feature const * feature)->double {
    auto topkFeature = static_cast<TopKFeature const *>(feature);
    if (function.compare(VALUE_FUNCTION) == 0) {
      if (parameters.size() != 1) {
        throw std::runtime_error("Expected there to be one parameter,"
                    " found " +
                    boost::lexical_cast<std::string>(parameters.size()));
      }
      int index = boost::lexical_cast<int>(parameters[0]);
      return topkFeature->getFrequencies()[index];
    }
    throw std::runtime_error("Evaluate with function " + function +
      " is not defined for class TopKFeature");

  };

  FuncToken<VastNetflow> funcToken(featureMap,
                               func,
                               identifier);

  // Nothing in featureMap, so should return false.
  bool b = funcToken.evaluate(mystack, key, netflow);
  BOOST_CHECK_EQUAL(b, false);

  std::vector<std::string> keys;
  std::vector<double> frequencies;
  keys.push_back("key1");
  keys.push_back("key2");
  frequencies.push_back(.4);
  frequencies.push_back(.3);

  TopKFeature feature(keys, frequencies);

  featureMap->updateInsert(key, identifier, feature);

  b = funcToken.evaluate(mystack, key, netflow);
  BOOST_CHECK_EQUAL(b, true);
  BOOST_CHECK_EQUAL(mystack.top(), .3);

}

BOOST_FIXTURE_TEST_CASE( test_prev_token, F )
{
  PrevToken<TimeSeconds, VastNetflow> prevToken1(featureMap);
  PrevToken<TimeSeconds, VastNetflow> prevToken2(featureMap);

  // We don't want the identifiers to be the same or that could cause problems.
  BOOST_CHECK(prevToken1.getIdentifier() != prevToken2.getIdentifier());

  // First pass should have no previous value, so fails.
  bool b = prevToken1.evaluate(mystack, key, netflow);
  BOOST_CHECK_EQUAL(b, false);

  // Second pass should have previous value.
  b = prevToken1.evaluate(mystack, key, netflow);
  BOOST_CHECK_EQUAL(b, true);
  BOOST_CHECK_EQUAL(mystack.top(), 1365582756.384094);

}


#pragma once

#include <vector>

#include <ros/ros.h>

#include <mil_tools/mil_tools.hpp>
#include <mil_vision_lib/cv_tools.hpp>
#include <mil_vision_lib/image_acquisition/ros_camera_stream.hpp>

#include <eigen_conversions/eigen_msg.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/StdVector>

using ROSCameraStream_Vec3 = mil_vision::ROSCameraStream<cv::Vec3b>;

class StereoBase
{
public:
  StereoBase();

  /**
  * Check if left+right cameras are publishing and are in sync
  * @see sync_thresh_
  */
  bool is_stereo_coherent();

  /**
  * A pure virtual method to segment left/right camera images
  * @param image left and right ROSimagestream will pass to this method
  * @see get_3d_feature_points()
  * @see ROSCameraStream
  * @return a vector of points that define the shape in 2d
  */
  virtual std::vector<cv::Point> get_2d_feature_points(cv::Mat image) = 0;

  /**
  * Use stereo intinsics and triangulation to map 2d features to 3d in stereo frame
  * @param max_z filter points that are greater than the given z value
  * @see get_2d_feature_points()
  * @see mil_vision::triangulate_Linear_LS
  * @return vector of points in 3d space or, if failed, a 0-sized vector
  */
  std::unique_ptr<std::vector<Eigen::Vector3d>> get_3d_feature_points(int max_z = 5);

  /**
  * Use 3d points to estimate a normal and a center point and return a pose
  * @param feature_pts_3d a vector of 3d points (currently only supports 4 points)
  * @param z_vector_min the minimum value the z-component of the normal vector should be
  * @see best_fit_plane_standard()
  * @return The translation and rotation with respect to [1,0,0] vector
  */
  std::unique_ptr<Eigen::Affine3d> get_3d_pose(std::vector<Eigen::Vector3d> feature_pts_3d, float z_vector_min = 0.5);

protected:
  std::unique_ptr<ROSCameraStream_Vec3> left_cam_stream_;
  std::unique_ptr<ROSCameraStream_Vec3> right_cam_stream_;

  /**
  * how often will image processing occur
  * also used as dt in kalman filter, defaults to 10 times per second
  * @see init_kalman_filter
  */
  double refresh_rate_;

  /**
  * maximum time difference between the left and right camera time stamps
  * @see is_stereo_coherent()
  */
  double sync_thresh_;

  /**
  * should the node run image processing
  */
  bool active_;

  /**
  * the number of states in the kalman filter
  * default to 18 which includes x,y,z and euler angles, and their respective 1st and 2nd dervatives
  */
  int n_states_;

  /**
  * number of measurments
  * default to 6 which contains x,y,z, and euler angles
  */
  int n_measurements_;

  /**
  * openCV's implementation of the kalman filter
  */
  cv::KalmanFilter k_filter_;

  /**
  * initializes/clears the kalman filter
  * @see k_filter_
  */
  void init_kalman_filter();

  /**
  * Updates the kalman filter and returns the predicted pose
  * @param pose The estimated 3d pose
  * @return The predicted pose
  */
  Eigen::Affine3d update_kalman_filter(const Eigen::Affine3d &pose);

private:
  /**
  * given two sets of points, finds a set of pair with shortest distances
  * @param features_l left camera feature points in 2D
  * @param features_r right camera feature points in 2D
  * @see get_3d_feature_points()
  * @return an vector of indices such that features_l[i] = features_r[correspondence[i]]
  */
  std::vector<int> shortest_pair_stereo_matching(const std::vector<cv::Point> &features_l,
                                                 const std::vector<cv::Point> &features_r, int y_axis_diff_thresh = 0);

  /**
  * Finds the plane of 4 points in standard form
  * @param feature_pts_3d a set of 3D points (currently supports only 4)
  * @return a vector that contains plane constans in standard form (A, B, C, D)
  */
  std::vector<double> best_fit_plane_standard(const std::vector<Eigen::Vector3d> &feature_pts_3d);

  /**
  * helper function for update_kalman_filter that converts to a cv::Mat
  * @param pose Estimated 3d pose
  * @return pose in a cv::Mat form of size (n_measurments_ * 1)
  */
  cv::Mat get_measurement_as_cv_mat(const Eigen::Affine3d &pose);
};


#pragma once

#include <GL/glew.h>
#include <vector>
#include <tuple>
#include <Eigen/Core>
#include "../geometry/PointNormal.hh"

namespace kt84 {

namespace phong_tessellation {
    enum struct Mode {
        TRIANGLES = 0,
        LINES,
        TRIANGLE_FAN,
        LINE_LOOP,
        LINE_STRIP
    };

    inline int& subdiv() {
        static int subdiv = 3;
        return subdiv;
    }
    inline double& weight() {
        static double weight = 0.7;
        return weight;
    }

    namespace internal {
        inline Mode& mode() {
            static Mode mode = Mode::TRIANGLES;
            return mode;
        }
        inline bool& enabled() {
            static bool enabled = true;
            return enabled;
        }
        inline std::vector<std::tuple<bool, int, double>>& config_stack() {
            static std::vector<std::tuple<bool, int, double>> config_stack;
            return config_stack;
        }
        inline std::vector<PointNormal>& pointnormals() {
            static std::vector<PointNormal> pointnormals;
            return pointnormals;
        }
        inline PointNormal project(const PointNormal& p, const PointNormal& q) {
            PointNormal result;
            result.head(3) = q.head(3) - (q - p).head(3).dot(p.tail(3)) * p.tail(3);
            result.tail(3) = p.tail(3);
            return result;
        }
        inline PointNormal interpolate(const Eigen::Vector3d& baryCoord, const PointNormal& pn0, const PointNormal& pn1, const PointNormal& pn2) {
            PointNormal result = baryCoord[0] * pn0 + baryCoord[1] * pn1 + baryCoord[2] * pn2;
            pn_normalize(result);
            return result;
        }
        inline PointNormal interpolate(double u, const PointNormal& pn0, const PointNormal& pn1) {
            PointNormal result = (1 - u) * pn0 + u * pn1;
            pn_normalize(result);
            return result;
        }
        inline void draw_triangle_sub(int i, int j, const PointNormal& pn0, const PointNormal& pn1, const PointNormal& pn2) {
            Eigen::Vector3d baryCoord;
            baryCoord[1] = i / static_cast<double>(subdiv());
            baryCoord[2] = j / static_cast<double>(subdiv());
            baryCoord[0] = 1.0 - baryCoord[1] - baryCoord[2];
            PointNormal p  = interpolate(baryCoord, pn0, pn1, pn2);
            PointNormal q0 = project(pn0, p);
            PointNormal q1 = project(pn1, p);
            PointNormal q2 = project(pn2, p);
            PointNormal q  = interpolate(baryCoord, q0, q1, q2);
            PointNormal r = (1.0 - weight()) * p + weight() * q;
            pn_normalize(r);
            glNormal3dv(&r[3]);
            glVertex3dv(&r[0]);
        }
        inline void draw_line_sub(int i, const PointNormal& pn0, const PointNormal& pn1) {
            double u = i / static_cast<double>(subdiv());
            PointNormal p  = interpolate(u, pn0, pn1);
            PointNormal q0 = project(pn0, p);
            PointNormal q1 = project(pn1, p);
            PointNormal q  = interpolate(u, q0, q1);
            PointNormal r = (1.0 - weight()) * p + weight() * q;
            pn_normalize(r);
            glNormal3dv(&r[3]);
            glVertex3dv(&r[0]);
        }
    }

    inline void begin(Mode mode_) { internal::mode() = mode_; }

    inline void vertex(const PointNormal& pn) { internal::pointnormals().push_back(pn); }
    inline void vertex(const Eigen::Vector3d& point, const Eigen::Vector3d& normal) {
        PointNormal pn;
        pn << point, normal;
        vertex(pn);
    }

    inline void enable () { internal::enabled() = true; }
    inline void disable() { internal::enabled() = false; }

    // config stack
    inline void push_config() {
        internal::config_stack().push_back(std::make_tuple(internal::enabled(), subdiv(), weight()));
    }
    inline void pop_config () {
        std::tie(internal::enabled(), subdiv(), weight()) = internal::config_stack().back();
        internal::config_stack().pop_back();
    }

    inline void draw_triangle(const PointNormal& pn0, const PointNormal& pn1, const PointNormal& pn2) {
        glBegin(GL_TRIANGLES);
        if (internal::enabled()) {
            for (int i = 0; i < subdiv(); ++i) {
                for (int j = 0; j < subdiv() - 1 - i; ++j) {
                    internal::draw_triangle_sub(i    , j    , pn0, pn1, pn2);
                    internal::draw_triangle_sub(i + 1, j    , pn0, pn1, pn2);
                    internal::draw_triangle_sub(i    , j + 1, pn0, pn1, pn2);
                    internal::draw_triangle_sub(i    , j + 1, pn0, pn1, pn2);
                    internal::draw_triangle_sub(i + 1, j    , pn0, pn1, pn2);
                    internal::draw_triangle_sub(i + 1, j + 1, pn0, pn1, pn2);
                }
                internal::draw_triangle_sub(i    , subdiv()     - i, pn0, pn1, pn2);
                internal::draw_triangle_sub(i    , subdiv() - 1 - i, pn0, pn1, pn2);
                internal::draw_triangle_sub(i + 1, subdiv() - 1 - i, pn0, pn1, pn2);
            }
        } else {
            glNormal3dv(&pn0[3]);    glVertex3dv(&pn0[0]);
            glNormal3dv(&pn1[3]);    glVertex3dv(&pn1[0]);
            glNormal3dv(&pn2[3]);    glVertex3dv(&pn2[0]);
        }
        glEnd();
    }

    inline void draw_line(const PointNormal& pn0, const PointNormal& pn1) {
        glBegin(GL_LINE_STRIP);
        if (internal::enabled()) {
            for (int i = 0; i < subdiv(); ++i) {
                internal::draw_line_sub(i    , pn0, pn1);
                internal::draw_line_sub(i + 1, pn0, pn1);
            }
        } else {
            glNormal3dv(&pn0[3]);    glVertex3dv(&pn0[0]);
            glNormal3dv(&pn1[3]);    glVertex3dv(&pn1[0]);
        }
        glEnd();
    }

    inline void end() {
        auto& mode = internal::mode();
        auto& pointnormals = internal::pointnormals();
        auto n = pointnormals.size();

        if (mode == Mode::TRIANGLES) {
            if (n < 3) return;
            size_t m = n / 3;
            for (size_t i = 0; i < m; ++i)
                draw_triangle(pointnormals[3 * i], pointnormals[3 * i + 1], pointnormals[3 * i + 2]);

        } else if (mode == Mode::LINES) {
            if (n < 2) return;
            size_t m = n / 2;
            for (size_t i = 0; i < m; ++i)
                draw_line(pointnormals[2 * i], pointnormals[2 * i + 1]);

        } else if (mode == Mode::TRIANGLE_FAN) {
            if (n < 3) return;
            for (size_t i = 1; i < n - 1; ++i)
                draw_triangle(pointnormals[0], pointnormals[i], pointnormals[i + 1]);

        } else if (mode == Mode::LINE_LOOP) {
            if (n < 2) return;
            for (size_t i = 0; i < n; ++i)
                draw_line(pointnormals[i], pointnormals[(i + 1) % n]);

        } else if (mode == Mode::LINE_STRIP) {
            if (n < 2) return;
            for (size_t i = 0; i < n - 1; ++i)
                draw_line(pointnormals[i], pointnormals[i + 1]);
        }

        pointnormals .clear();
    }
};

}


/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2018, Locus Robotics
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the copyright holder nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */
#include <fuse_constraints/absolute_orientation_3d_stamped_euler_constraint.h>
#include <fuse_core/eigen.h>
#include <fuse_core/eigen_gtest.h>
#include <fuse_core/uuid.h>
#include <fuse_variables/orientation_3d_stamped.h>
#include <geometry_msgs/Quaternion.h>

#include <ceres/covariance.h>
#include <ceres/problem.h>
#include <ceres/solver.h>
#include <Eigen/Geometry>
#include <gtest/gtest.h>

#include <utility>
#include <vector>

using fuse_constraints::AbsoluteOrientation3DStampedEulerConstraint;
using fuse_variables::Orientation3DStamped;

TEST(AbsoluteOrientation3DStampedEulerConstraint, Constructor)
{
  // Construct a constraint just to make sure it compiles.
  Orientation3DStamped orientation_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("walle"));
  fuse_core::Vector3d mean;
  mean << 1.0, 2.0, 3.0;
  fuse_core::Matrix3d cov;
  cov << 1.0, 0.1, 0.2, 0.1, 2.0, 0.3, 0.2, 0.3, 3.0;
  std::vector<Orientation3DStamped::Euler> axes =
    {Orientation3DStamped::Euler::YAW, Orientation3DStamped::Euler::ROLL, Orientation3DStamped::Euler::PITCH};
  EXPECT_NO_THROW(
    AbsoluteOrientation3DStampedEulerConstraint constraint("test", orientation_variable, mean, cov, axes));
}

TEST(AbsoluteOrientation3DStampedEulerConstraint, Covariance)
{
  // Verify the covariance <--> sqrt information conversions are correct
  Orientation3DStamped orientation_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("mo"));
  fuse_core::Vector3d mean;
  mean << 1.0, 2.0, 3.0;
  fuse_core::Matrix3d cov;
  cov << 1.0, 0.1, 0.2, 0.1, 2.0, 0.3, 0.2, 0.3, 3.0;
  std::vector<Orientation3DStamped::Euler> axes =
    {Orientation3DStamped::Euler::YAW, Orientation3DStamped::Euler::ROLL, Orientation3DStamped::Euler::PITCH};
  AbsoluteOrientation3DStampedEulerConstraint constraint("test", orientation_variable, mean, cov, axes);

  // Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))')
  fuse_core::Matrix3d expected_sqrt_info;
  expected_sqrt_info <<  1.008395589795798, -0.040950074712520, -0.063131365181801,
                         0.000000000000000,  0.712470499879096, -0.071247049987910,
                         0.000000000000000,  0.000000000000000,  0.577350269189626;
  fuse_core::Matrix3d expected_cov = cov;

  // Compare
  EXPECT_TRUE(expected_cov.isApprox(constraint.covariance(), 1.0e-9));
  EXPECT_TRUE(expected_sqrt_info.isApprox(constraint.sqrtInformation(), 1.0e-9));
}

TEST(AbsoluteOrientation3DStampedEulerConstraint, OptimizationFull)
{
  // Optimize a single pose and single constraint, verify the expected value and covariance are generated.
  // Create the variables
  auto orientation_variable = Orientation3DStamped::make_shared(ros::Time(1, 0), fuse_core::uuid::generate("spra"));
  orientation_variable->w() = 0.952;
  orientation_variable->x() = 0.038;
  orientation_variable->y() = -0.189;
  orientation_variable->z() = 0.239;

  // Create an absolute orientation constraint
  fuse_core::Vector3d mean;
  mean << 0.5, 1.0, 1.5;
  fuse_core::Matrix3d cov;
  cov << 1.0, 0.1, 0.2, 0.1, 2.0, 0.3, 0.2, 0.3, 3.0;
  std::vector<Orientation3DStamped::Euler> axes =
    {Orientation3DStamped::Euler::YAW, Orientation3DStamped::Euler::ROLL, Orientation3DStamped::Euler::PITCH};
  auto constraint = AbsoluteOrientation3DStampedEulerConstraint::make_shared(
    "test",
    *orientation_variable,
    mean,
    cov,
    axes);

  // Build the problem
  ceres::Problem::Options problem_options;
  problem_options.loss_function_ownership = fuse_core::Loss::Ownership;
  ceres::Problem problem(problem_options);
  problem.AddParameterBlock(
    orientation_variable->data(),
    orientation_variable->size(),
    orientation_variable->localParameterization());

  std::vector<double*> parameter_blocks;
  parameter_blocks.push_back(orientation_variable->data());
  problem.AddResidualBlock(
    constraint->costFunction(),
    constraint->lossFunction(),
    parameter_blocks);

  // Run the solver
  ceres::Solver::Options options;
  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);

  // Check
  Eigen::Quaterniond expected = Eigen::AngleAxisd(0.5, Eigen::Vector3d::UnitZ()) *
                                Eigen::AngleAxisd(1.5, Eigen::Vector3d::UnitY()) *
                                Eigen::AngleAxisd(1.0, Eigen::Vector3d::UnitX());
  EXPECT_NEAR(expected.w(), orientation_variable->w(), 5.0e-3);
  EXPECT_NEAR(expected.x(), orientation_variable->x(), 5.0e-3);
  EXPECT_NEAR(expected.y(), orientation_variable->y(), 5.0e-3);
  EXPECT_NEAR(expected.z(), orientation_variable->z(), 5.0e-3);

  // TODO(swilliams) Determine what I expect the covariance matrix to be and test it here
}

TEST(AbsoluteOrientation3DStampedEulerConstraint, OptimizationPartial)
{
  // Optimize a single pose and single constraint, verify the expected value and covariance are generated.
  // Create the variables
  auto orientation_variable = Orientation3DStamped::make_shared(ros::Time(1, 0), fuse_core::uuid::generate("spra"));
  orientation_variable->w() = 0.952;
  orientation_variable->x() = 0.038;
  orientation_variable->y() = -0.189;
  orientation_variable->z() = 0.239;

  // Create an absolute orientation constraint
  fuse_core::Vector2d mean1;
  mean1 << 0.5, 1.5;
  fuse_core::Matrix2d cov1;
  cov1 << 1.0, 0.2, 0.2, 3.0;
  std::vector<Orientation3DStamped::Euler> axes1 =
    {Orientation3DStamped::Euler::YAW, Orientation3DStamped::Euler::PITCH};
  auto constraint1 = AbsoluteOrientation3DStampedEulerConstraint::make_shared(
    "test",
    *orientation_variable,
    mean1,
    cov1,
    axes1);

  fuse_core::Vector1d mean2;
  mean2 << 1.0;
  fuse_core::Matrix1d cov2;
  cov2 << 2.0;
  std::vector<Orientation3DStamped::Euler> axes2 =
    {Orientation3DStamped::Euler::ROLL};
  auto constraint2 = AbsoluteOrientation3DStampedEulerConstraint::make_shared(
    "test",
    *orientation_variable,
    mean2,
    cov2,
    axes2);

  // Build the problem
  ceres::Problem::Options problem_options;
  problem_options.loss_function_ownership = fuse_core::Loss::Ownership;
  ceres::Problem problem(problem_options);
  problem.AddParameterBlock(
    orientation_variable->data(),
    orientation_variable->size(),
    orientation_variable->localParameterization());

  std::vector<double*> parameter_blocks;
  parameter_blocks.push_back(orientation_variable->data());
  problem.AddResidualBlock(
    constraint1->costFunction(),
    constraint1->lossFunction(),
    parameter_blocks);
  problem.AddResidualBlock(
    constraint2->costFunction(),
    constraint2->lossFunction(),
    parameter_blocks);

  // Run the solver
  ceres::Solver::Options options;
  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);

  // Check
  Eigen::Quaterniond expected = Eigen::AngleAxisd(0.5, Eigen::Vector3d::UnitZ()) *
                                Eigen::AngleAxisd(1.5, Eigen::Vector3d::UnitY()) *
                                Eigen::AngleAxisd(1.0, Eigen::Vector3d::UnitX());
  EXPECT_NEAR(expected.w(), orientation_variable->w(), 5.0e-3);
  EXPECT_NEAR(expected.x(), orientation_variable->x(), 5.0e-3);
  EXPECT_NEAR(expected.y(), orientation_variable->y(), 5.0e-3);
  EXPECT_NEAR(expected.z(), orientation_variable->z(), 5.0e-3);

  // TODO(swilliams) Determine what I expect the covariance matrix to be and test it here
}

TEST(AbsoluteOrientation3DStampedEulerConstraint, Serialization)
{
  // Construct a constraint
  Orientation3DStamped orientation_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("walle"));
  fuse_core::Vector3d mean;
  mean << 1.0, 2.0, 3.0;
  fuse_core::Matrix3d cov;
  cov << 1.0, 0.1, 0.2, 0.1, 2.0, 0.3, 0.2, 0.3, 3.0;
  std::vector<Orientation3DStamped::Euler> axes =
    {Orientation3DStamped::Euler::YAW, Orientation3DStamped::Euler::ROLL, Orientation3DStamped::Euler::PITCH};
  AbsoluteOrientation3DStampedEulerConstraint expected("test", orientation_variable, mean, cov, axes);

  // Serialize the constraint into an archive
  std::stringstream stream;
  {
    fuse_core::TextOutputArchive archive(stream);
    expected.serialize(archive);
  }

  // Deserialize a new constraint from that same stream
  AbsoluteOrientation3DStampedEulerConstraint actual;
  {
    fuse_core::TextInputArchive archive(stream);
    actual.deserialize(archive);
  }

  // Compare
  EXPECT_EQ(expected.uuid(), actual.uuid());
  EXPECT_EQ(expected.variables(), actual.variables());
  EXPECT_MATRIX_EQ(expected.mean(), actual.mean());
  EXPECT_MATRIX_EQ(expected.sqrtInformation(), actual.sqrtInformation());
}

int main(int argc, char **argv)
{
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_EXPONENTIAL_FUNCTIONS_GENERIC_LOG2_HPP_INCLUDED
#define NT2_EXPONENTIAL_FUNCTIONS_GENERIC_LOG2_HPP_INCLUDED

#include <nt2/exponential/functions/log2.hpp>
#include <boost/simd/sdk/simd/meta/is_native.hpp>
#include <nt2/exponential/functions/scalar/impl/logarithm.hpp>
#include <nt2/exponential/functions/simd/common/impl/logarithm.hpp>
#include <nt2/include/functions/simd/frexp.hpp>
#include <nt2/include/functions/simd/ilog2.hpp>
#include <nt2/include/functions/simd/log2.hpp>

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( log2_, tag::cpu_
                            , (A0)
                            , (generic_< arithmetic_<A0> >)
                            )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE NT2_FUNCTOR_CALL(1)
    {
      return nt2::ilog2(a0);
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( log2_, tag::cpu_
                            , (A0)(A1)
                            , (generic_< floating_<A0> >)
                              (generic_< integer_<A1> >)
                            )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A1& a1) const
    {
      return frexp(a0, a1);
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( log2_, tag::cpu_
                            , (A0)(A1)(A2)
                            , (generic_< floating_<A0> >)
                              (generic_< floating_<A1> >)
                              (generic_< integer_<A2> >)
                            )
  {
    typedef void result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A1& a1, A2& a2) const
    {
      return frexp(a0, a1, a2);
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( log2_, tag::cpu_
                            , (A0)
                            , (generic_< floating_<A0> >)
                            )
  {
    typedef A0 result_type;
    typedef typename boost::simd::meta::is_native<A0>::type is_native_t;
    BOOST_FORCEINLINE NT2_FUNCTOR_CALL(1)
    {
      return details::logarithm<A0,is_native_t>::log2(a0);
    }
  };
} }

#endif