#include "gtest/gtest.h"
#include "buddy/bdd.h"

// ::std
#include <iostream>
#include <vector>
#include <utility>
#include <limits>
#include <sstream>
#include <string>
#include <cstdlib>
#include <map>
#include <boost/cast.hpp>

#include <boost/static_assert.hpp>

// ::wali::domains::binrel
#include "wali/domains/binrel/BinRel.hpp"
#include "wali/domains/binrel/ProgramBddContext.hpp"

using namespace std;
using namespace wali;
using namespace wali::domains::binrel;
using namespace wali::domains::binrel::details;

#include "baselines.hpp"

namespace{

  struct RawBuddyContext
  {
    RawBuddyContext(int numvars = 4) {
      assert(!bdd_isrunning());
      int err = bdd_init(10000,1000);
      err |= bdd_setvarnum(numvars);
      assert(!err);
    }

    ~RawBuddyContext() {
      bdd_done();
    }
  };

  struct SubsetsOfFour
  {
    bdd b0, b1, b2, b3, b01, b02, b03, b12, b13,
      b23, b012, b013, b023, b123, b0123;

    SubsetsOfFour()
    {
      assert(bdd_isrunning());
      if (bdd_varnum() < 2) {
        int err = bdd_setvarnum(2);
        assert(!err);
      }

      bdd zero  = bdd_nithvar(0) & bdd_nithvar(1);
      bdd one   = bdd_nithvar(0) & bdd_ithvar(1);
      bdd two   = bdd_ithvar(0) & bdd_nithvar(1);
      bdd three = bdd_ithvar(0) & bdd_ithvar(1);

      b0 = zero;
      b1 = one;
      b2 = two;
      b3 = three;

      b01 = zero | one;
      b02 = zero | two;
      b03 = zero | three;
      b12 = one | two;
      b13 = one | three;
      b23 = two | three;

      b012 = b01 | two;
      b013 = b01 | three;
      b023 = b02 | three;
      b123 = b12 | three;

      b0123 = b012 | three;
    }
  };

  TEST(BinRelSelfTest, copyTest){
    ProgramBddContext ctx;
    ctx.addBoolVar("a");

    ProgramBddContext ctx2 = ctx;
  }

  TEST(BinRelSelfTest, repopulateCacheOnCopy){
      ProgramBddContext ctx;
      ctx.addBoolVar("a");

      ProgramBddContext ctx2 = ctx;
      sem_elem_t b = new BinRel(&ctx2, bddtrue);

      // This should just not fail an exception is what I'm checking.
      b->one()->extend(b);
  }

  TEST(BinRelSelfTest, repopulateCacheOnAssign){
      ProgramBddContext ctx;
      ctx.addBoolVar("a");

      ProgramBddContext ctx2;
      ctx2 = ctx;
      sem_elem_t b = new BinRel(&ctx2, bddtrue);

      // This should just not fail an exception is what I'm checking.
      b->one()->extend(b);
  }

  TEST(BinRelSelfTest, creationTest){
    program_bdd_context_t brm = new ProgramBddContext();
    brm->addBoolVar("a");
    brm->addBoolVar("b");
    brm->addBoolVar("c");
    brm->addBoolVar("d");
    brm->addIntVar("p",4);
    brm->addIntVar("q",4);
    brm->addIntVar("r",4);
    brm->addIntVar("s",4);
    ASSERT_EQ(brm->size(), 8u);

    bdd a = brm->From("a");
    bdd p = brm->From("p");

    sem_elem_t se1 = new BinRel(brm.get_ptr(),a,false);
    sem_elem_t se2 = new BinRel(brm.get_ptr(),p,false);

    stringstream ss;
    se1->print(ss << "se1: ") << endl;
    se2->print(ss << "se2: ") << endl;
    ASSERT_TRUE(compareOutput("BinRelSelfTest","creationTest",ss));
  }

  TEST(ProgramBddContext, mapConstructorNoVars)
  {
    std::map<std::string, int> vars;
    ProgramBddContext ctx(vars);

    EXPECT_EQ(0u, ctx.size());
  }

  TEST(ProgramBddContext, mapConstructorFewVars)
  {
    std::map<std::string, int> vars;
    vars["a"] = 2;
    vars["b"] = 2;
    vars["c"] = 4;
    ProgramBddContext ctx(vars);

    EXPECT_EQ(3u, ctx.size());
  }

  TEST(ProgramBddContext, vectorConstructorEmptyVec)
  {
    std::vector<std::map<std::string, int> > vars;
    ProgramBddContext ctx(vars);

    EXPECT_EQ(0u, ctx.size());
  }

  TEST(ProgramBddContext, mapConstructorOneLevelFewVars)
  {
    std::vector<std::map<std::string, int> > vars;
    vars.resize(1);
    vars.at(0)["a"] = 2;
    vars.at(0)["b"] = 2;
    vars.at(0)["c"] = 4;
    ProgramBddContext ctx(vars);

    EXPECT_EQ(3u, ctx.size());
  }

  TEST(ProgramBddContext, mapConstructorFewLevelFewVars)
  {
    std::vector<std::map<std::string, int> > vars;
    vars.resize(3);
    vars.at(0)["a"] = 2;
    vars.at(1)["b"] = 2;
    vars.at(1)["c"] = 4;
    vars.at(2)["d"] = 4;
    vars.at(2)["e"] = 4;
    ProgramBddContext ctx(vars);

    EXPECT_EQ(5u, ctx.size());
  }

  TEST(BinRelSelfTest, selfTest){
    program_bdd_context_t brm = new ProgramBddContext();
    brm->addBoolVar("a");
    brm->addBoolVar("b");
    brm->addIntVar("p",4);

    bdd a = brm->From("a");
    a = brm->Assign("b",a);
    sem_elem_t se1 = new BinRel(brm.get_ptr(),a,false);
    wali::test_semelem_impl(se1);
  }

  class BinRelTestBool: public ::testing::Test{
    protected:
      virtual void SetUp(){
        brm = new ProgramBddContext();
        brm->addBoolVar("a");
        brm->addBoolVar("b");
        brm->addBoolVar("c");
        //ASSERT_EQ(voc.size(), 3);
      }
    protected:
      program_bdd_context_t brm;
  };

  TEST_F(BinRelTestBool,constantTests){
    stringstream ss;
    sem_elem_t se = new BinRel(brm.get_ptr(),brm->False(),false);
    se->one()->print(ss << "one(): ") << endl;
    se->zero()->print(ss << "zero(): ") << endl;
    EXPECT_TRUE(compareOutput("BinRelTestBool","constantTests",ss));
  }

  TEST_F(BinRelTestBool,extendTests){
    stringstream ss;
    bdd a;
    a = brm->Assign("c", brm->And(brm->From("a"),brm->From("b")));
    sem_elem_t se1 = new BinRel(brm.get_ptr(),a,false);
    a = brm->Assume(brm->From("a"), brm->From("b"));
    sem_elem_t se2 = new BinRel(brm.get_ptr(),a,false);
    sem_elem_t one = se1->one();
    sem_elem_t zero = se1->zero();

    EXPECT_TRUE(zero->equal(zero->extend(se1)));
    EXPECT_TRUE(se2->equal(se2->extend(one)));

    se1->print(ss << "se1: ") << endl;
    se2->print(ss << "se2: ") << endl;
    se1->extend(se2)->print(ss << "se1->extend(se2): ") << endl;
    se2->extend(se1)->print(ss << "se2->extend(se1): ") << endl;

    EXPECT_TRUE(compareOutput("BinRelTestBool","extendTests",ss));

    binrel_t be1,be2;
    be1 = boost::polymorphic_downcast<BinRel*>(se1.get_ptr());
    be2 = boost::polymorphic_downcast<BinRel*>(se2.get_ptr());

    EXPECT_TRUE(se1->extend(se2.get_ptr())->equal((be1 * be2).get_ptr()));
    EXPECT_TRUE(se2->extend(se1.get_ptr())->equal((be2 * be1).get_ptr()));

    EXPECT_TRUE(compareOutput("BinRelTestBool","extendTests",ss));
  }

  TEST_F(BinRelTestBool,combineTests){
    stringstream ss;
    bdd a;
    a = brm->Assign("c", brm->And(brm->From("a"),brm->From("b")));
    sem_elem_t se1 = new BinRel(brm.get_ptr(),a,false);
    a = brm->Assume(brm->From("a"), brm->From("b"));
    sem_elem_t se2 = new BinRel(brm.get_ptr(),a,false);
    sem_elem_t one = se1->one();
    sem_elem_t zero = se1->zero();

    EXPECT_TRUE(se1->equal(zero->combine(se1)));
    EXPECT_TRUE(one->equal(one->combine(one)));

    se1->print(ss << "se1: ") << endl;
    se2->print(ss << "se2: ") << endl;
    se1->combine(se2)->print(ss << "se1->combine(se2): ") << endl;
    se2->combine(se1)->print(ss << "se2->combine(se1): ") << endl;

    EXPECT_TRUE(compareOutput("BinRelTestBool","combineTests",ss));

    binrel_t be1,be2;
    be1 = boost::polymorphic_downcast<BinRel*>(se1.get_ptr());
    be2 = boost::polymorphic_downcast<BinRel*>(se2.get_ptr());
    EXPECT_TRUE(se1->combine(se2.get_ptr())->equal((be1 | be2).get_ptr()));
  }

  TEST_F(BinRelTestBool, transposeTests){
    stringstream ss;
    bdd a;
    a = brm->Assign("c", brm->And(brm->From("a"),brm->From("b")));
    sem_elem_tensor_t se1 = new BinRel(brm.get_ptr(),a,false);
    a = brm->Assume(brm->From("a"), brm->From("b"));
    sem_elem_tensor_t se2 = new BinRel(brm.get_ptr(),a,false);

    se1->print(ss << "se1: ") << endl;
    se1->transpose()->print(ss << "se1: ") << endl;
    se2->print(ss << "se2: ") << endl;
    se2->transpose()->print(ss << "se2: ") << endl;

    EXPECT_TRUE(compareOutput("BinRelTestBool","transposeTests",ss));
  }

  TEST_F(BinRelTestBool, tensorTests){
    brm = new ProgramBddContext();
    brm->addBoolVar("a");
    brm->addBoolVar("b");
    //ASSERT_EQ(voc.size(), 2);

    stringstream ss;
    bdd a;
    a = brm->Assume(brm->From("a"), brm->From("b"));
    sem_elem_tensor_t se1 = new BinRel(brm.get_ptr(),a,false);
    a = brm->Assume(brm->From("a"), brm->Not(brm->From("b")));
    sem_elem_tensor_t se2 = new BinRel(brm.get_ptr(),a,false);
    sem_elem_tensor_t se3 = se1->tensor(se2.get_ptr());

    se1->print(ss << "se1: ") << endl;
    se2->print(ss << "se2: ") << endl;
    se3->print(ss << "se3: ") << endl;

    EXPECT_TRUE(compareOutput("BinRelTestBool","tensorTests",ss));

  }

  TEST_F(BinRelTestBool, detensorTest){
    stringstream ss;
    bdd a;
    a = brm->Assume(brm->From("a"), brm->From("b"));
    sem_elem_tensor_t se1 = new BinRel(brm.get_ptr(),a,false);
    a = brm->Assume(brm->From("b"), brm->From("c"));
    sem_elem_tensor_t se2 = new BinRel(brm.get_ptr(),a,false);
    sem_elem_tensor_t se3 = se1->tensor(se2.get_ptr());
    sem_elem_tensor_t se4 = se3->detensor();
    sem_elem_tensor_t se5 =
      boost::polymorphic_downcast<SemElemTensor*>(se1->extend(se2.get_ptr()).get_ptr());

    se1->print(ss << "se1: ") << endl;
    se2->print(ss << "se2: ") << endl;
    se3->print(ss << "se3: ") << endl;
    se4->print(ss << "se4: ") << endl;
    se5->print(ss << "se5: ") << endl;

    EXPECT_TRUE(compareOutput("BinRelTestBool","detensorTests",ss));
    EXPECT_TRUE(se5->equal(se4.get_ptr()));
  }

  TEST_F(BinRelTestBool, detensorTransposeTest){
    stringstream ss;
    bdd a;
    a = brm->Assign("a", brm->From("c"));
    sem_elem_tensor_t se1 = new BinRel(brm.get_ptr(),a,false);
    a = brm->Assume(brm->From("b"), brm->From("c"));
    sem_elem_tensor_t se2 = new BinRel(brm.get_ptr(),a,false);
    sem_elem_tensor_t se3 = se1->tensor(se2.get_ptr());
    sem_elem_tensor_t se4 = se3->detensor();
    sem_elem_tensor_t se5 =
      boost::polymorphic_downcast<SemElemTensor*>(se1->extend(se2.get_ptr()).get_ptr());
    sem_elem_tensor_t se6 = se2->tensor(se1.get_ptr());
    sem_elem_tensor_t se7 = se6->detensorTranspose();
    sem_elem_tensor_t se8 =
      boost::polymorphic_downcast<SemElemTensor*>(se2->extend(se1.get_ptr()).get_ptr());

    EXPECT_TRUE(se5->equal(se4.get_ptr()));
    EXPECT_TRUE(se8->equal(se7.get_ptr()));
  }

  class BinRelTestInt: public ::testing::Test{
    protected:
      virtual void SetUp(){
        brm = new ProgramBddContext();
        brm->addIntVar("a",4);
        brm->addIntVar("b",4);
        brm->addIntVar("c",4);
        //ASSERT_EQ(voc.size(), 3);
      }
    protected:
      sem_elem_tensor_t screenVar(string var, sem_elem_tensor_t wt);
      program_bdd_context_t brm;
  };

  sem_elem_tensor_t BinRelTestInt::screenVar(string var, sem_elem_tensor_t wt){
    sem_elem_tensor_t screen = new BinRel(brm.get_ptr(),brm->Assume(brm->From(var),brm->Const(0)), false);
    return boost::polymorphic_downcast<SemElemTensor*>(wt->extend(screen.get_ptr()).get_ptr());
  }

  TEST_F(BinRelTestInt,testScreen){
    stringstream ss;
    sem_elem_tensor_t wt = new BinRel(brm.get_ptr(),brm->Const(0), false);
    wt = boost::polymorphic_downcast<SemElemTensor*>(wt->one().get_ptr());
    wt->print(ss << "W[1]: ") << endl;
    wt = screenVar("a",wt);
    wt->print(ss << "W[1 & a==0]: ") << endl;
    wt = screenVar("b",wt);
    wt->print(ss << "W[1 && a==0 && b==0]: ") << endl;
    wt = screenVar("c",wt);
    wt->print(ss << "W[1 && a==0 && b==0 && c==0]: ") << endl;
    EXPECT_TRUE(compareOutput("BinRelTestInt","testScreen",ss));
  }

  TEST(BinRelTestRandom, baseDomain){
    // //Maybe this should be done in a more repeat-friendly way// //
    stringstream ss;
    bool failed = false, dump = false;
    program_bdd_context_t brm = new ProgramBddContext();
    brm->addIntVar( "a", 4);
    brm->addBoolVar( "b");
    brm->addIntVar( "c", 4);
    brm->addBoolVar( "d");
    //ASSERT_EQ(voc.size(),4);

    srand((unsigned)time(NULL));
    for(int h=0;h<20;++h){
      bdd r1bdd = brm->tGetRandomTransformer();
      bdd r2bdd = brm->tGetRandomTransformer();
      bdd r3bdd = brm->tGetRandomTransformer();

      sem_elem_t r1 = new BinRel(brm.get_ptr(),r1bdd,false);
      sem_elem_t r2 = new BinRel(brm.get_ptr(),r2bdd,false);
      sem_elem_t r3 = new BinRel(brm.get_ptr(),r3bdd,false);
      sem_elem_t w1,w2,w3,w4,w5;

      w1 = r1->combine(r2);
      w2 = r2->combine(r1);
      if(!w1->equal(w2) || dump){
        ss << "#################################\nr1 | r2 <> r2 | r1\n\n";
        r1->print(ss << "r1: ") << endl;
        r2->print(ss << "r2: ") << endl;
        w1->print(ss << "r1 | r2: ") << endl;
        w2->print(ss << "r2 | r1: ") << endl;
        failed = true;
      }

      w1 = (r1->extend(r2))->combine(r1->extend(r3));
      w2 = r1->extend(r2->combine(r3));;
      if(!w1->equal(w2) || dump){
        ss << "#################################\nr1 & (r2 | r3) <> r1 & r2 | r1 & r3\n\n";
        r1->print(ss << "r1: ") << endl;
        r2->print(ss << "r2: ") << endl;
        r3->print(ss << "r3: ") << endl;
        w1->print(ss << "r1 & (r2 | r3): ") << endl;
        w2->print(ss << "r1 & r2 | r1 & r3: ") << endl;
        failed = true;
      }

      w1 = (r1->extend(r3))->combine(r2->extend(r3));
      w2 = (r1->combine(r2))->extend(r3);
      if(!w1->equal(w2) || dump){
        ss << "#################################\n(r1 | r2 ) & r3 <> r1 & r3 | r2 & r3\n\n";
        r1->print(ss << "r1: ") << endl;
        r2->print(ss << "r2: ") << endl;
        r3->print(ss << "r3: ") << endl;
        w1->print(ss << "(r1 | r2) & r3: ") << endl;
        w2->print(ss << "r1 & r3 | r2 & r3: ") << endl;
        failed = true;
      }
    }

    if(failed){
      cerr << "\n Differences logged in BinRelTestRandom_baseDomain.output\n";
      writeOutput("BinRelTestRandom","baseDomain", ss);
    }
    ASSERT_FALSE(failed);
  }

  TEST(BinRelTestRandom, tensorDomain){
    // //Maybe this should be done in a more repeat-friendly way// //
    stringstream ss;
    bool failed = false;
    bool dump = false;
    program_bdd_context_t brm = new ProgramBddContext();
    brm->addIntVar( "a", 4);
    brm->addBoolVar( "b");
    brm->addIntVar( "c", 4);
    brm->addBoolVar( "d");
    //ASSERT_EQ(voc.size(),4);

    srand((unsigned)time(NULL));
    for(int h=0;h<20;++h){
      bdd r1bdd = brm->tGetRandomTransformer(true);
      bdd r2bdd = brm->tGetRandomTransformer(true);
      bdd r3bdd = brm->tGetRandomTransformer(true);

      sem_elem_t r1 = new BinRel(brm.get_ptr(),r1bdd,true);
      sem_elem_t r2 = new BinRel(brm.get_ptr(),r2bdd,true);
      sem_elem_t r3 = new BinRel(brm.get_ptr(),r3bdd,true);
      sem_elem_t w1,w2,w3,w4,w5;

      w1 = r1->combine(r2);
      w2 = r2->combine(r1);
      if(!w1->equal(w2) || dump){
        ss << "#################################\n[tensor] r1 | r2 <> r2 | r1\n\n";
        r1->print(ss << "r1: ") << endl;
        r2->print(ss << "r2: ") << endl;
        w1->print(ss << "r1 | r2: ") << endl;
        w2->print(ss << "r2 | r1: ") << endl;
        failed = true;
      }

      w1 = (r1->extend(r2))->combine(r1->extend(r3));
      w2 = r1->extend(r2->combine(r3));;
      if(!w1->equal(w2) || dump){
        ss << "#################################\n[tensor] r1 & (r2 | r3) <> r1 & r2 | r1 & r3\n\n";
        r1->print(ss << "r1: ") << endl;
        r2->print(ss << "r2: ") << endl;
        r3->print(ss << "r3: ") << endl;
        w1->print(ss << "r1 & (r2 | r3): ") << endl;
        w2->print(ss << "r1 & r2 | r1 & r3: ") << endl;
        failed = true;
      }

      w1 = (r1->extend(r3))->combine(r2->extend(r3));
      w2 = (r1->combine(r2))->extend(r3);
      if(!w1->equal(w2) || dump){
        ss << "#################################\n[tensor] (r1 | r2 ) & r3 <> r1 & r3 | r2 & r3\n\n";
        r1->print(ss << "r1: ") << endl;
        r2->print(ss << "r2: ") << endl;
        r3->print(ss << "r3: ") << endl;
        w1->print(ss << "(r1 | r2) & r3: ") << endl;
        w2->print(ss << "r1 & r3 | r2 & r3: ") << endl;
        failed = true;
      }
    }

    if(failed){
      cerr << "\n Differences logged in BinRelTestRandom_baseDomain.output\n";
      writeOutput("BinRelTestRandom","tensorDomain", ss);
    }
    ASSERT_FALSE(failed);
  }

  TEST(BinRelTestRandom, baseTensorDomain){
    // //Maybe this should be done in a more repeat-friendly way// //
    stringstream ss;
    bool failed = false;
    bool dump = false;
    program_bdd_context_t brm = new ProgramBddContext();
    brm->addIntVar( "a", 4);
    brm->addBoolVar( "b");
    brm->addIntVar( "c", 4);
    brm->addBoolVar( "d");

    srand(static_cast<unsigned>(time(NULL)));
    //for debugging
    //srand(1);

    for(int h=0;h<20;++h){
      bdd b1bdd = brm->tGetRandomTransformer(false);
      bdd b2bdd = brm->tGetRandomTransformer();
      bdd b3bdd = brm->tGetRandomTransformer();
      bdd b4bdd = brm->tGetRandomTransformer(false);
      bdd t1bdd = brm->tGetRandomTransformer(true);
      bdd t2bdd = brm->tGetRandomTransformer(true);

      sem_elem_tensor_t b1 = new BinRel(brm.get_ptr(),b1bdd,false);
      sem_elem_tensor_t b2 = new BinRel(brm.get_ptr(),b2bdd,false);
      sem_elem_tensor_t b3 = new BinRel(brm.get_ptr(),b3bdd,false);
      sem_elem_tensor_t b4 = new BinRel(brm.get_ptr(),b4bdd,false);
      sem_elem_tensor_t t1 = new BinRel(brm.get_ptr(),t1bdd,true);
      sem_elem_tensor_t t2 = new BinRel(brm.get_ptr(),t2bdd,true);
      sem_elem_tensor_t w1,w2,w3,w4;

      w1 = b1->tensor(b2.get_ptr());
      w2 = b3->tensor(b4.get_ptr());
      w1 = boost::polymorphic_downcast<SemElemTensor*>(w1->extend(w2.get_ptr()).get_ptr());

      w3 = boost::polymorphic_downcast<SemElemTensor*>(b1->extend(b3.get_ptr()).get_ptr());
      w4 = boost::polymorphic_downcast<SemElemTensor*>(b2->extend(b4.get_ptr()).get_ptr());
      w3 = w3->tensor(w4.get_ptr());

      if(!w1->equal(w3) || dump){
        ss << "#################################\n(b1,b2) & (b3,b4) <> (b1 & b3,b2 & b4)\n\n";

        b1->print(ss << "b1: ") << endl;
        b2->print(ss << "b2: ") << endl;
        b3->print(ss << "b3: ") << endl;
        b4->print(ss << "b4: ") << endl;
        w1 = b1->tensor(b2.get_ptr());
        w1->print(ss << "(b1,b2): ") << endl;
        w2 = b3->tensor(b4.get_ptr());
        w2->print(ss << "(b3,b4): ") << endl;
        w1 = boost::polymorphic_downcast<SemElemTensor*>(w1->extend(w2.get_ptr()).get_ptr());
        w1->print(ss << "(b1,b2) & (b3,b4): ") << endl;

        b1->print(ss << "b1: ") << endl;
        b2->print(ss << "b2: ") << endl;
        b3->print(ss << "b3: ") << endl;
        b4->print(ss << "b4: ") << endl;
        w3 = boost::polymorphic_downcast<SemElemTensor*>(b1->extend(b3.get_ptr()).get_ptr());
        w3->print(ss << "b1 & b3: ") << endl;
        w4 = boost::polymorphic_downcast<SemElemTensor*>(b2->extend(b4.get_ptr()).get_ptr());
        w4->print(ss << "b2 & b4: ") << endl;
        w3 = w3->tensor(w4.get_ptr());
        w3->print(ss << "(b1 & b3,b2 & b4): ") << endl;

        failed = true;
      }

      w1 = boost::polymorphic_downcast<SemElemTensor*>(t1->combine(t2.get_ptr()).get_ptr());
      w1 = w1->detensor();

      w2 = t1->detensor();
      w3 = t2->detensor();
      w2 = boost::polymorphic_downcast<SemElemTensor*>(w2->combine(w3.get_ptr()).get_ptr());

      if(!w1->equal(w2) || dump){
        ss << "#################################\nD(t1 | t2) <> D(t1) | D(t2)\n\n";
        t1->print(ss << "t1: ") << endl;
        t2->print(ss << "t2: ") << endl;

        w1 = boost::polymorphic_downcast<SemElemTensor*>(t1->combine(t2.get_ptr()).get_ptr());
        w1->print(ss << "t1 | t2: ") << endl;
        w1 = w1->detensor();
        w1->print(ss << "D(t1 | t2): ") << endl;

        w2 = t1->detensor();
        w2->print(ss << "D(t1): ") << endl;
        w3 = t2->detensor();
        w3->print(ss << "D(t2): ") << endl;
        w2 = boost::polymorphic_downcast<SemElemTensor*>(w2->combine(w3.get_ptr()).get_ptr());
        w2->print(ss << "D(t1) | D(t2): ") << endl;

        failed = true;
      }

      w1 = b1->tensor(b2.get_ptr());
      w1 = w1->detensor();

      w2 = boost::polymorphic_downcast<SemElemTensor*>(b1->extend(b2.get_ptr()).get_ptr());

      if(!w1->equal(w2) || dump){
        ss << "#################################\nD((b1, b2)) <> b1 & b2\n\n";
        b1->print(ss << "b1: ") << endl;
        b2->print(ss << "b2: ") << endl;
        w1 = b1->tensor(b2.get_ptr());
        w1->print(ss << "(b1,r2): ") << endl;
        w1 = w1->detensor();
        w1->print(ss << "D((b1,r2)): ") << endl;

        w2 = boost::polymorphic_downcast<SemElemTensor*>(b1->extend(b2.get_ptr()).get_ptr());
        w2->print(ss << "b1 & b2: ") << endl;

        failed = true;
      }
    }

    if(failed){
      cerr << "\n Differences logged in BinRelTestRandom_baseDomain.output\n";
      writeOutput("BinRelTestRandom","baseTensorDomain", ss);
    }
    ASSERT_FALSE(failed);
  }

  TEST(BinRelTestRandom, tensorDomainSetInt){
    //same as tensorDomain, but uses setIntVars to create the vocabulary.
    // //Maybe this should be done in a more repeat-friendly way// //
    stringstream ss;
    bool failed = false;
    bool dump = false;
    program_bdd_context_t brm = new ProgramBddContext();
    std::map<std::string, int> vars;
    vars["a"] = 4;
    vars["b"] = 2;
    vars["c"] = 4;
    vars["d"] = 2;
    brm->setIntVars(vars);
    //ASSERT_EQ(voc.size(),4);

    srand((unsigned)time(NULL));
    for(int h=0;h<20;++h){
      bdd r1bdd = brm->tGetRandomTransformer(true);
      bdd r2bdd = brm->tGetRandomTransformer(true);
      bdd r3bdd = brm->tGetRandomTransformer(true);

      sem_elem_t r1 = new BinRel(brm.get_ptr(),r1bdd,true);
      sem_elem_t r2 = new BinRel(brm.get_ptr(),r2bdd,true);
      sem_elem_t r3 = new BinRel(brm.get_ptr(),r3bdd,true);
      sem_elem_t w1,w2,w3,w4,w5;

      w1 = r1->combine(r2);
      w2 = r2->combine(r1);
      if(!w1->equal(w2) || dump){
        ss << "#################################\n[tensor] r1 | r2 <> r2 | r1\n\n";
        r1->print(ss << "r1: ") << endl;
        r2->print(ss << "r2: ") << endl;
        w1->print(ss << "r1 | r2: ") << endl;
        w2->print(ss << "r2 | r1: ") << endl;
        failed = true;
      }

      w1 = (r1->extend(r2))->combine(r1->extend(r3));
      w2 = r1->extend(r2->combine(r3));;
      if(!w1->equal(w2) || dump){
        ss << "#################################\n[tensor] r1 & (r2 | r3) <> r1 & r2 | r1 & r3\n\n";
        r1->print(ss << "r1: ") << endl;
        r2->print(ss << "r2: ") << endl;
        r3->print(ss << "r3: ") << endl;
        w1->print(ss << "r1 & (r2 | r3): ") << endl;
        w2->print(ss << "r1 & r2 | r1 & r3: ") << endl;
        failed = true;
      }

      w1 = (r1->extend(r3))->combine(r2->extend(r3));
      w2 = (r1->combine(r2))->extend(r3);
      if(!w1->equal(w2) || dump){
        ss << "#################################\n[tensor] (r1 | r2 ) & r3 <> r1 & r3 | r2 & r3\n\n";
        r1->print(ss << "r1: ") << endl;
        r2->print(ss << "r2: ") << endl;
        r3->print(ss << "r3: ") << endl;
        w1->print(ss << "(r1 | r2) & r3: ") << endl;
        w2->print(ss << "r1 & r3 | r2 & r3: ") << endl;
        failed = true;
      }
    }

    if(failed){
      cerr << "\n Differences logged in BinRelTestRandom_baseDomain.output\n";
      writeOutput("BinRelTestRandom","tensorDomain", ss);
    }
    ASSERT_FALSE(failed);
  }

  TEST(BinRelTestRandom, tensorDomainSetIntTwoSets){
    // Same as tensorDomain, but uses setIntVars to create the vocabulary.
    // Additionally, the variables are grouped into two interleaving sets.
    // //Maybe this should be done in a more repeat-friendly way// //
    stringstream ss;
    bool failed = false;
    bool dump = false;
    program_bdd_context_t brm = new ProgramBddContext();
    std::vector<std::map<std::string, int> > vars;
    std::map<std::string, int> vars1;
    std::map<std::string, int> vars2;
    vars1["a"] = 4;
    vars1["b"] = 2;
    vars2["c"] = 4;
    vars2["d"] = 2;
    vars.push_back(vars1);
    vars.push_back(vars2);
    brm->setIntVars(vars);
    //ASSERT_EQ(voc.size(),4);

    srand((unsigned)time(NULL));
    for(int h=0;h<20;++h){
      bdd r1bdd = brm->tGetRandomTransformer(true);
      bdd r2bdd = brm->tGetRandomTransformer(true);
      bdd r3bdd = brm->tGetRandomTransformer(true);

      sem_elem_t r1 = new BinRel(brm.get_ptr(),r1bdd,true);
      sem_elem_t r2 = new BinRel(brm.get_ptr(),r2bdd,true);
      sem_elem_t r3 = new BinRel(brm.get_ptr(),r3bdd,true);
      sem_elem_t w1,w2,w3,w4,w5;

      w1 = r1->combine(r2);
      w2 = r2->combine(r1);
      if(!w1->equal(w2) || dump){
        ss << "#################################\n[tensor] r1 | r2 <> r2 | r1\n\n";
        r1->print(ss << "r1: ") << endl;
        r2->print(ss << "r2: ") << endl;
        w1->print(ss << "r1 | r2: ") << endl;
        w2->print(ss << "r2 | r1: ") << endl;
        failed = true;
      }

      w1 = (r1->extend(r2))->combine(r1->extend(r3));
      w2 = r1->extend(r2->combine(r3));;
      if(!w1->equal(w2) || dump){
        ss << "#################################\n[tensor] r1 & (r2 | r3) <> r1 & r2 | r1 & r3\n\n";
        r1->print(ss << "r1: ") << endl;
        r2->print(ss << "r2: ") << endl;
        r3->print(ss << "r3: ") << endl;
        w1->print(ss << "r1 & (r2 | r3): ") << endl;
        w2->print(ss << "r1 & r2 | r1 & r3: ") << endl;
        failed = true;
      }

      w1 = (r1->extend(r3))->combine(r2->extend(r3));
      w2 = (r1->combine(r2))->extend(r3);
      if(!w1->equal(w2) || dump){
        ss << "#################################\n[tensor] (r1 | r2 ) & r3 <> r1 & r3 | r2 & r3\n\n";
        r1->print(ss << "r1: ") << endl;
        r2->print(ss << "r2: ") << endl;
        r3->print(ss << "r3: ") << endl;
        w1->print(ss << "(r1 | r2) & r3: ") << endl;
        w2->print(ss << "r1 & r3 | r2 & r3: ") << endl;
        failed = true;
      }
    }

    if(failed){
      cerr << "\n Differences logged in BinRelTestRandom_baseDomain.output\n";
      writeOutput("BinRelTestRandom","tensorDomain", ss);
    }
    ASSERT_FALSE(failed);
  }

  // The following test just creates some add operations. The point is so
  // that if both the slow and fast adders are turned on, then it will make
  // sure they get the same result.
  TEST(wali$domains$binrel$ProgramBddContext$$bddPlus, slowAndFastAdderAgree)
  {
      for (int i=2; i<10; ++i) {
          ProgramBddContext ctx;
          std::map<std::string, int> vars;
          vars["x"] = i;
          ctx.setIntVars(vars);
          ctx.Plus(ctx.From("x"), ctx.From("x"));
      }
  }

  TEST(wali$domains$binrel$ProgramBddContext$$Assign, canAssignAConstToNotTheBiggestVariable)
  {
      ProgramBddContext ctx;
      std::map<std::string, int> vars;
      vars["x"] = 2;
      vars["y"] = 4;
      ctx.setIntVars(vars);

      // x = 0 should obviously succeed
      bdd assigned = ctx.Assign("x", ctx.Const(0));

      ASSERT_TRUE(assigned != bddfalse);

      // We want to check that there's exactly one satisfying assignment for
      // this (to make sure there's not something strange going
      // on). 'assigned' should be the bdd " x' = 0 " but with some extra
      // stuff tacked on for the auxiliary variables. We will make sure that,
      // if we manually construct that bdd independently, take the
      // complement, and 'and' it with 'assigned', the result is unsat --
      // meaning that every satisfying assignement to 'assigned' has x'=0.
      int x_prime_fdd_num = ctx["x"]->baseRhs;
      bdd x_prime_is_zero = fdd_ithvar(x_prime_fdd_num, 0);

      ASSERT_TRUE(bddfalse == (assigned & !x_prime_is_zero));
  }

#undef SETVARADDVARTESTS
#if defined(SETVARADDVARTESTS)
  /**
   * The following 4 tests try the following configurations:
   *
   * Configuration 1:
   * call 'addIntVar' several times (for variables a-n)
   * construct some weights
   * call 'addIntVar' (for variable w)
   * do some binrel operations
   *
   * Configuration 2:
   * call 'setIntVars' once (for a-n)
   * construct some weights
   * call 'addIntVar' (for variable w)
   * do some binrel operations
   *
   * Configuration 3:
   * call 'setIntVars' once for a-n
   * call 'addIntVar' for w
   * construct some weights
   * do some binrel operations
   *
   * Configuration 4:
   * call 'setIntVars' once for both a-n and w at once
   * construct some weights
   * do some binrel operations
   **/
  TEST(SetVarAddVarTests, one){
    program_bdd_context_t brm = new ProgramBddContext();

    brm->addIntVar("a", 4);
    brm->addIntVar("b", 4);
    brm->addIntVar("c", 4);
    brm->addIntVar("d", 4);
    brm->addIntVar("e", 4);
    brm->addIntVar("f", 4);
    brm->addIntVar("g", 4);
    brm->addIntVar("h", 4);
    brm->addIntVar("i", 4);
    brm->addIntVar("j", 4);
    brm->addIntVar("k", 4);
    brm->addIntVar("l", 4);
    brm->addIntVar("m", 4);
    brm->addIntVar("n", 4);

    binrel_t b1 = new BinRel(brm.get_ptr(), brm->Assign("a", brm->Plus(brm->From("b"), brm->From("c"))));
    binrel_t b2 = new BinRel(brm.get_ptr(), brm->Assign("b", brm->Plus(brm->Const(2), brm->From("d"))));

    brm->addIntVar("w", 15);

    b2 = new BinRel(brm.get_ptr(), brm->Assign("w", brm->Plus(brm->Const(4), brm->From("w"))));
    sem_elem_t b3 = b1->extend(b2.get_ptr());
    b3 = b3->combine(b3);
  }

  TEST(SetVarAddVarTests, two){
    program_bdd_context_t brm = new ProgramBddContext();

    map<string, int> vars;
    vars["a"] = 4;
    vars["b"] = 4;
    vars["c"] = 4;
    vars["d"] = 4;
    vars["e"] = 4;
    vars["f"] = 4;
    vars["g"] = 4;
    vars["h"] = 4;
    vars["i"] = 4;
    vars["j"] = 4;
    vars["k"] = 4;
    vars["l"] = 4;
    vars["m"] = 4;
    vars["n"] = 4;
    brm->setIntVars(vars);

    binrel_t b1 = new BinRel(brm.get_ptr(), brm->Assign("a", brm->Plus(brm->From("b"), brm->From("c"))));
    binrel_t b2 = new BinRel(brm.get_ptr(), brm->Assign("b", brm->Plus(brm->Const(2), brm->From("d"))));

    brm->addIntVar("w", 15);

    b2 = new BinRel(brm.get_ptr(), brm->Assign("w", brm->Plus(brm->Const(4), brm->From("w"))));
    sem_elem_t b3 = b1->extend(b2.get_ptr());
    b3 = b3->combine(b3);
  }

  TEST(SetVarAddVarTests, three){
    program_bdd_context_t brm = new ProgramBddContext();

    map<string, int> vars;
    vars["a"] = 4;
    vars["b"] = 4;
    vars["c"] = 4;
    vars["d"] = 4;
    vars["e"] = 4;
    vars["f"] = 4;
    vars["g"] = 4;
    vars["h"] = 4;
    vars["i"] = 4;
    vars["j"] = 4;
    vars["k"] = 4;
    vars["l"] = 4;
    vars["m"] = 4;
    vars["n"] = 4;
    brm->setIntVars(vars);

    brm->addIntVar("w", 15);

    binrel_t b1 = new BinRel(brm.get_ptr(), brm->Assign("a", brm->Plus(brm->From("b"), brm->From("c"))));
    binrel_t b2 = new BinRel(brm.get_ptr(), brm->Assign("b", brm->Plus(brm->Const(2), brm->From("d"))));

    b2 = new BinRel(brm.get_ptr(), brm->Assign("w", brm->Plus(brm->Const(4), brm->From("w"))));
    sem_elem_t b3 = b1->extend(b2.get_ptr());
    b3 = b3->combine(b3);
  }

  TEST(SetVarAddVarTests, four){
    program_bdd_context_t brm = new ProgramBddContext();

    map<string, int> vars;
    vars["a"] = 4;
    vars["b"] = 4;
    vars["c"] = 4;
    vars["d"] = 4;
    vars["e"] = 4;
    vars["f"] = 4;
    vars["g"] = 4;
    vars["h"] = 4;
    vars["i"] = 4;
    vars["j"] = 4;
    vars["k"] = 4;
    vars["l"] = 4;
    vars["m"] = 4;
    vars["n"] = 4;

    vars["w"] = 15;

    brm->setIntVars(vars);

    binrel_t b1 = new BinRel(brm.get_ptr(), brm->Assign("a", brm->Plus(brm->From("b"), brm->From("c"))));
    binrel_t b2 = new BinRel(brm.get_ptr(), brm->Assign("b", brm->Plus(brm->Const(2), brm->From("d"))));

    b2 = new BinRel(brm.get_ptr(), brm->Assign("w", brm->Plus(brm->Const(4), brm->From("w"))));
    sem_elem_t b3 = b1->extend(b2.get_ptr());
    b3 = b3->combine(b3);
  }
#endif //SETVARADDVARTESTS

#if defined(BINREL_STATS)
  TEST(StatsTests, basicStatCollectionAndPrinting1)
  {
    program_bdd_context_t brm = new ProgramBddContext();

    brm->addIntVar("c", 5);
    brm->addIntVar("d", 5);

    sem_elem_t a = new BinRel(brm.get_ptr(), brm->True());
    sem_elem_t b = new BinRel(brm.get_ptr(), brm->Plus(brm->From("c"), brm->From("d")));

    sem_elem_t c;
    c = a->extend(a);
    c = c->extend(c);

    c = c->combine(a);
    c = c->combine(a);

    sem_elem_tensor_t e = boost::polymorphic_downcast<SemElemTensor*>(a.get_ptr());
    sem_elem_tensor_t f = boost::polymorphic_downcast<SemElemTensor*>(b.get_ptr());
    sem_elem_tensor_t g,h;

    g = boost::polymorphic_downcast<SemElemTensor*>(f.get_ptr());
    g = g->transpose();

    g = e->tensor(f.get_ptr());
    h = e->tensor(f.get_ptr());

    g = boost::polymorphic_downcast<SemElemTensor*>(g->extend(h.get_ptr()).get_ptr());
    g = boost::polymorphic_downcast<SemElemTensor*>(g->extend(h.get_ptr()).get_ptr());
    g = boost::polymorphic_downcast<SemElemTensor*>(g->combine(h.get_ptr()).get_ptr());

    c = g->detensor();
    c = h->detensorTranspose();

    stringstream ss;

    brm->printStats(ss);
    EXPECT_TRUE(compareOutput("StatTests", "basicStatCollectionAndPrinting1", ss));

  }
#endif

  TEST(Unclassified, dumbTensorTest1)
  {
    program_bdd_context_t brm = new ProgramBddContext();
    brm->addBoolVar("b");

    binrel_t a = new BinRel(brm.get_ptr(), brm->True());
    a = boost::polymorphic_downcast<BinRel*>(a->one().get_ptr());

    binrel_t b = boost::polymorphic_downcast<BinRel*>(a->tensor(a.get_ptr()).get_ptr());
    b->print(cout) << std::endl;
    binrel_t wt = boost::polymorphic_downcast<BinRel*>(b->detensorTranspose().get_ptr());
    if(wt == NULL){
      ASSERT_TRUE(false);
      return;
    }
    cout << wt->count;
    wt->print(cout) << std::endl;
    ASSERT_TRUE(b->detensor()->equal(a));
    ASSERT_TRUE(b->detensorTranspose()->equal(a));

  }

#define STRESS_TEST_CONST1 500
#define STRESS_TEST_CONST2 20000
  TEST(SetVarAddVarTests, DISABLED_stressTest){
    // Try to using setVars vs addVar
    {
      // Using addIntVar
      cout << "Using addIntVar" << endl;
      program_bdd_context_t brm;
      brm = new ProgramBddContext();
      map<string, int> vars;
      for(unsigned i=0; i<STRESS_TEST_CONST1; ++i){
        stringstream ss;
        ss << "var " << i;
        brm->addIntVar(ss.str(), 3);
      }
      cout << "Done adding variables" << endl;
      bdd bdds[STRESS_TEST_CONST1];
      unsigned i = 0;
      for(ProgramBddContext::iterator iter = brm->begin(); iter != brm->end(); ++iter, ++i)
        bdds[i] = brm->Plus(brm->From(iter->first), brm->Const(2));

      sem_elem_t sems[STRESS_TEST_CONST2];
      for(unsigned i=0; i<STRESS_TEST_CONST2; ++i){
        unsigned j = i % 200;
        unsigned k = (int)( i * 4.8) % 200;
        sem_elem_t sem1 = new BinRel(brm.get_ptr(), bdds[j]);
        sem_elem_t sem2 = new BinRel(brm.get_ptr(), bdds[k]);
        sems[i] = sem1->extend(sem2);
      }
    }

    {
      cout << "Using SetIntVar" << endl;
      // Using setIntVar
      program_bdd_context_t brm;
      brm = new ProgramBddContext();
      map<string, int> vars;
      for(unsigned i=0; i<STRESS_TEST_CONST1; ++i){
        stringstream ss;
        ss << "var " << i;
        vars[ss.str()] = 3;
      }
      brm->setIntVars(vars);
      cout << "Done adding variables" << endl;

      bdd bdds[STRESS_TEST_CONST1];
      unsigned i = 0;
      for(ProgramBddContext::iterator iter = brm->begin(); iter != brm->end(); ++iter, ++i)
        bdds[i] = brm->Plus(brm->From(iter->first), brm->Const(2));

      sem_elem_t sems[STRESS_TEST_CONST2];
      for(unsigned i=0; i<STRESS_TEST_CONST2; ++i){
        unsigned j = i % 200;
        unsigned k = (int)( i * 4.8) % 200;
        sem_elem_t sem1 = new BinRel(brm.get_ptr(), bdds[j]);
        sem_elem_t sem2 = new BinRel(brm.get_ptr(), bdds[k]);
        sems[i] = sem1->extend(sem2);
      }
    }
  }

#if (TENSOR_MATCHED_PAREN == 1) && (NWA_DETENSOR == 1) && 0
  TEST(BddContext, bitLevelInfo)
  {
    program_bdd_context_t con;
    con = new ProgramBddContext();
    map<string, int> vars;
    vars["a"] = 2;
    vars["b"] = 2;
    con->setIntVars(vars);

    bddinfo_t a = (*con)["a"];
    bddinfo_t b = (*con)["b"];

    bdd t;

    // Setup Level sets
    con->setupLevelSets();

    /*
     * t looks like:
     * "a".tensor1Lhs          *
     *                       1/ \0
     *              ----------   ---------
     *          bddfalse              bddtrue
     **/
    t = fdd_ithvar(a->tensor1Lhs, 0);
    EXPECT_TRUE(con->isRootInVocTensor1Lhs(t));
    EXPECT_FALSE(con->isRootRelevant(bdd_low(t)));
    EXPECT_FALSE(con->isRootRelevant(bdd_high(t)));

    /*
     * t looks like:
     * "a".tensor1Rhs         *
     *                      1/ \0
     *                 ------   \
     * "a".tensor1Lhs  *         \
     *               1/ \         \
     *               /   ----------
     *            bddtrue        bddfalse
     **/
    t = fdd_ithvar(a->tensor1Rhs, 1) & fdd_ithvar(a->tensor1Lhs, 1);
    EXPECT_TRUE(con->isRootInVocTensor1Rhs(t));
    EXPECT_TRUE(con->isRootInVocTensor1Lhs(bdd_high(t)));
    EXPECT_FALSE(con->isRootRelevant(bdd_low(t)));

    /*
     * t looks like:
     * "b".tensor2Lhs         *
     *                      1/ \0
     * "b".tensor2Rhs       /   *
     *                     /  0/ \1
     * "a".tensor2Lhs      |  /   *
     *                     |  | 1/ \0
     * "a".tensor2Rhs      |  |  |  *
     *                     |  |  |0/ \1
     *                  -----------   \
     *                bddfalse       bddtrue
     **/
    t = fdd_ithvar(b->tensor2Lhs, 0) & fdd_ithvar(a->tensor2Lhs, 0) & fdd_ithvar(b->tensor2Rhs, 1) & fdd_ithvar(a->tensor2Rhs, 1);
    EXPECT_TRUE(con->isRootInVocTensor2Lhs(t));
    t = bdd_low(t);
    EXPECT_TRUE(con->isRootInVocTensor2Rhs(t));
    t = bdd_high(t);
    EXPECT_TRUE(con->isRootInVocTensor2Lhs(t));
    t = bdd_low(t);
    EXPECT_TRUE(con->isRootInVocTensor2Rhs(t));
    t = bdd_high(t);
    EXPECT_TRUE(t == bddtrue);

    /*
     * t looks like:
     * "a".tensor1Rhs         *
     *                      1/ \0
     * "a".tensor1Lhs       /   *
     *                     /  0/ \1
     * "b".tensor1Rhs      |  /   *
     *                     |  | 1/ \0
     * "b".tensor1Lhs      |  |  |  *
     *                     |  |  |0/ \1
     *                  -----------   \
     *                bddfalse       bddtrue
     **/
    t = fdd_ithvar(a->tensor1Rhs, 0) & fdd_ithvar(a->tensor1Lhs, 1) & fdd_ithvar(b->tensor1Rhs, 0) & fdd_ithvar(b->tensor1Lhs, 1);
    EXPECT_TRUE(con->isRootInVocTensor1Rhs(t));
    t = bdd_low(t);
    EXPECT_TRUE(con->isRootInVocTensor1Lhs(t));
    t = bdd_high(t);
    EXPECT_TRUE(con->isRootInVocTensor1Rhs(t));
    t = bdd_low(t);
    EXPECT_TRUE(con->isRootInVocTensor1Lhs(t));
    t = bdd_high(t);
    EXPECT_TRUE(t == bddtrue);

  }
#endif //#if defined(TENSOR_MATCHED_PAREN)

  TEST(wali$domains$binrel$details$$bddImplies, trueImpliesTrue)
  {
    RawBuddyContext buddy;
    EXPECT_TRUE(bddImplies(bddtrue, bddtrue));
  }

  TEST(wali$domains$binrel$details$$bddImplies, trueDoesNotSubsumeFalse)
  {
    RawBuddyContext buddy;
    EXPECT_FALSE(bddImplies(bddtrue, bddfalse));
  }

  TEST(wali$domains$binrel$details$$bddImplies, falseImpliesTrue)
  {
    RawBuddyContext buddy;
    EXPECT_TRUE(bddImplies(bddfalse, bddtrue));
  }

  TEST(wali$domains$binrel$details$$bddImplies, falseImpliesFalse)
  {
    RawBuddyContext buddy;
    EXPECT_TRUE(bddImplies(bddfalse, bddfalse));
  }

  TEST(wali$domains$binrel$details$$bddImplies, subsetsOfFourBattery)
  {
    RawBuddyContext buddy(2);
    SubsetsOfFour subsets;

    bdd bdds[] = {
      bddfalse, subsets.b0, subsets.b1, subsets.b2, subsets.b3, subsets.b01, subsets.b02,
      subsets.b03, subsets.b12, subsets.b13, subsets.b23, subsets.b012, subsets.b013,
      subsets.b023, subsets.b123, subsets.b0123
    };

    bool answers[][16] = {
      //            {},    {0}    {1}    {2}    {3}    {01}   {02}   {03}   {12}   {13}   {23}   {012}  {013}  {023}  {123}  {0123}
      /* {}    */ { true,  true,  true,  true,  true,  true,  true,  true,  true,  true,  true,  true,  true,  true,  true,  true },

      /* {0}   */ { false, true,  false, false, false, true,  true,  true,  false, false, false, true,  true,  true,  false, true },
      /* {1}   */ { false, false, true,  false, false, true,  false, false, true,  true,  false, true,  true,  false, true,  true },
      /* {2}   */ { false, false, false, true,  false, false, true,  false, true,  false, true,  true,  false, true,  true,  true },
      /* {3}   */ { false, false, false, false, true,  false, false, true,  false, true,  true,  false, true,  true,  true,  true },

      /* {01}  */ { false, false, false, false, false, true,  false, false, false, false, false, true,  true,  false, false, true },
      /* {02}  */ { false, false, false, false, false, false, true,  false, false, false, false, true,  false, true,  false, true },
      /* {03}  */ { false, false, false, false, false, false, false, true,  false, false, false, false, true,  true,  false, true },
      /* {12}  */ { false, false, false, false, false, false, false, false, true,  false, false, true,  false, false, true,  true },
      /* {13}  */ { false, false, false, false, false, false, false, false, false, true,  false, false, true,  false, true,  true },
      /* {23}  */ { false, false, false, false, false, false, false, false, false, false, true,  false, false, true,  true,  true },

      /* {012} */ { false, false, false, false, false, false, false, false, false, false, false, true,  false, false, false, true },
      /* {013} */ { false, false, false, false, false, false, false, false, false, false, false, false, true,  false, false, true },
      /* {023} */ { false, false, false, false, false, false, false, false, false, false, false, false, false, true,  false, true },
      /* {123} */ { false, false, false, false, false, false, false, false, false, false, false, false, false, false, true,  true },

      /* {0124}*/ { false, false, false, false, false, false, false, false, false, false, false, false, false, false, false, true }
    };

#define NUM_ELEMENTS(array) (sizeof(array)/sizeof((array)[0]))

    BOOST_STATIC_ASSERT(NUM_ELEMENTS(answers) == NUM_ELEMENTS(bdds));
    BOOST_STATIC_ASSERT(NUM_ELEMENTS(answers[0]) == NUM_ELEMENTS(bdds));

    for (int i=0; i<2; ++i) {
      if (i==1) {
        // reverse the order
        int order[] = {1, 0};
        int old_var_level_0 = bdd_var2level(0);
        ASSERT_EQ(bdd_varnum(), static_cast<int>(NUM_ELEMENTS(order)));
        bdd_setvarorder(order);
        EXPECT_NE(old_var_level_0, bdd_var2level(0));
      }

      for (size_t left_no=0; left_no<NUM_ELEMENTS(bdds); ++left_no) {
        for (size_t right_no=0; right_no<NUM_ELEMENTS(bdds); ++right_no) {
          std::stringstream ss;
          ss << "Checking whether BDD " << left_no << " is a subset of " << right_no;
          SCOPED_TRACE(ss.str());

          bdd left = bdds[left_no];
          bdd right = bdds[right_no];
          bool expected = answers[left_no][right_no];
          bool actual = bddImplies(left, right);
          bool actual_imp = bddImplies_using_bdd_imp(left, right);
          bool actual_rec = bddImplies_recursive(left, right);

          EXPECT_EQ(expected, actual);
          EXPECT_EQ(expected, actual_imp);
          EXPECT_EQ(expected, actual_rec);
        }
      }
    }
  }

  TEST(wali$domains$binrel$ProgramBddContext$printHistoryAndRecreate, setIntVarsMap)
  {
    stringstream ss;
    ProgramBddContext * con = new ProgramBddContext();
    std::map<string, int> m;
    m["a"] = 3;
    m["b"] = 4;
    con->setIntVars(m);
    con->printHistory(ss);

    ProgramBddContext * hist;
    hist = ProgramBddContext::buildFromHistory(ss);
    BddContext::const_iterator cit = con->begin();
    ASSERT_TRUE(string("a") == cit->first);
    ASSERT_TRUE(3 == cit->second->maxVal);
    ++cit;
    ASSERT_TRUE(string("b") == cit->first);
    ASSERT_TRUE(4 == cit->second->maxVal);
    delete con;
    delete hist;
  }

  TEST(wali$domains$binrel$$BinRel$$underApproximates, bddContainmentChecks)
  {
    ProgramBddContext context;

    map<string, int> vars;
    vars["x"] = 4;
    context.setIntVars(vars);

    bdd plus1 = context.Assign("x", context.Plus(context.From("x"), context.Const(1)));
    bdd plus2 = context.Assign("x", context.Plus(context.From("x"), context.Const(2)));
    bdd plus3 = context.Assign("x", context.Plus(context.From("x"), context.Const(3)));

    binrel_t plus1_rel = new BinRel(&context, plus1);
    binrel_t plus2_rel = new BinRel(&context, plus2);
    binrel_t plus3_rel = new BinRel(&context, plus3);

    sem_elem_t plus12_rel = plus1_rel->combine(plus2_rel.get_ptr());
    sem_elem_t plus13_rel = plus1_rel->combine(plus3_rel.get_ptr());
    sem_elem_t plus123_rel = plus12_rel->combine(plus3_rel.get_ptr());

    EXPECT_TRUE(plus1_rel->zero()->underApproximates(plus1_rel.get_ptr())); //    0 <== {1}
    EXPECT_TRUE(plus1_rel->underApproximates(plus1_rel.get_ptr()));         //  {1} <== {1}
    EXPECT_TRUE(plus1_rel->underApproximates(plus12_rel.get_ptr()));        //  {1} <== {12}
    EXPECT_FALSE(plus12_rel->underApproximates(plus1_rel.get_ptr()));       // {12} </= {1}
    EXPECT_TRUE(plus12_rel->underApproximates(plus123_rel.get_ptr()));      // {12} <== {123}
    EXPECT_FALSE(plus12_rel->underApproximates(plus13_rel.get_ptr()));      // {12} </= {13}
    EXPECT_FALSE(plus13_rel->underApproximates(plus12_rel.get_ptr()));      // {13} </= {12}
  }

  TEST(wali$domains$binrel$ProgramBddContext$printHistoryAndRecreate, bddSaveNRestore)
  {
    stringstream ss;
    {
      ProgramBddContext * con = new ProgramBddContext();
      std::map<string, int> m;
      m["a"] = 3;
      m["b"] = 4;
      con->setIntVars(m);
      bdd b = con->Assign("a", con->From("b"));
      con->printHistory(ss);
      bdd_fnsave("dusty_bdd", b);
      delete con;
    }
    ProgramBddContext * hist;
    hist = ProgramBddContext::buildFromHistory(ss);
    bdd c;
    bdd_fnload("dusty_bdd", c);
    bdd d = hist->Assign("a", hist->From("b"));
    ASSERT_EQ(c,d);
    delete hist;
  }

  TEST(wali$domains$binrel$$BinRel$$Assume, differentVocSizes)
  {
    ProgramBddContext p;
    map< string, int> m;
    m["a"] = 2;
    m["b"] = 4;
    p.setIntVars(m);

    bdd b = p.Assume(p.From("a"), p.Const(0));
    ASSERT_NE(b, bddfalse);
  }
} //namespace

// Yo, Emacs!
// Local Variables:
//   c-file-style: "ellemtel"
//   c-basic-offset: 2
// End:


/* Copyright (C) 2020 Francisco Vial - All Rights Reserved
 * You may use, distribute and modify this code under the
 * terms of the WTFPL.
 */

#ifndef SRC_UTILS_HPP_
#define SRC_UTILS_HPP_

#include <iostream>
#include <string>
#include <vector>
#include <gmp.h>
#include <gmpxx.h>
#include <boost/thread.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/filesystem.hpp>

extern int N_THREADS;

using std::vector;
using std::string;

void read_moduli_from_csv(string, vector<mpz_class>*, vector<string>*, int);
int product_tree(vector<mpz_class>*);
int product_tree_multithread(vector<mpz_class>*);
int product_tree_seq(vector<mpz_class>*);
void write_level_to_file(int l, vector<mpz_class> *);
void read_level_from_file(int, vector<mpz_class> *);
void read_variable_from_file(int level, int index, mpz_class *x);
void remainders_squares(int, vector<mpz_class> *);
void remainders_squares_simple(int, vector<mpz_class> *);
void remainders_squares_fast(int, vector<mpz_class> *);
void remainders_squares_fast_multithread(int levels, vector<mpz_class> *R);
void remainders_squares_fast_seq(int levels, vector<mpz_class> *R);
void mt_level_mult(vector<mpz_class> *, vector<mpz_class> *);
void partial_remainders(int, vector<mpz_class>*, vector<mpz_class>*);

void my_mpz_inp_raw(mpz_class &, FILE *);

#endif /* SRC_UTILS_HPP_ */


#ifndef __GYRO_HH__
#define __GYRO_HH__
/********************************* TRICK HEADER *******************************
PURPOSE:
      (Gyro model interface definition)
*******************************************************************************/

#include <armadillo>
#include <aux.hh>
#include "Module.hh"
#include "global_constants.hh"
#include "stochastic.hh"
#include "nxbus_engine.h"

class Gyro : public Sensor {
  TRICK_INTERFACE(Gyro);

 public:
  char name[256];

  Gyro() : VECTOR_INIT(WBICB, 3), VECTOR_INIT(EWBIB, 3){};

  virtual ~Gyro() {}
  virtual void init() = 0;
  virtual void algorithm(double int_step) = 0;

  virtual arma::vec3 get_computed_WBIB() { return WBICB; }
  virtual arma::vec3 get_error_of_computed_WBIB() { return EWBIB; }
  virtual int write_to_(const char *bus_name) { return 0; }

  std::function<arma::vec3()> grab_WBIB;
  std::function<arma::vec3()> grab_FSPB;

 protected:
  VECTOR(WBICB,
         3); /* *o  (r/s)   Computed inertial body rate in body coordinate */

  VECTOR(EWBIB, 3); /* *o  (r/s)   Error in angular vel of body wrt earth */

  double qqcx; /* *o  (d/s)   INS computed pitch rate */
  double rrcx; /* *o  (d/s)   INS computed yaw rate */
  double ppcx; /* *o  (d/s)   INS computed roll rate */
};

#endif  // __GYRO__


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2012 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#ifndef GOAL_ORIENTED_ERROR_ESTIMATOR_HH
#define GOAL_ORIENTED_ERROR_ESTIMATOR_HH

#include <cmath>
#include <iostream>
#include <type_traits>
#include <utility>

#include <boost/fusion/include/at_c.hpp>
#include <boost/mpl/at.hpp>

#include "algorithm/newton_bridge.hh"
#include "algorithm/errorDistribution.hh"
#include "linalg/jacobiPreconditioner.hh"
#include "fem/hierarchicErrorEstimator.hh"
#include "fem/variables.hh"
#include "linalg/direct.hh"
#include "utilities/enums.hh"

namespace Kaskade
{
  bool biggerThanAbs(const std::pair<double,int>& x1, const std::pair<double,int>&  x2)
  {
    return fabs(x1.first)>fabs(x2.first);
  }

  double add(const double& x1, const std::pair<double,int>&  x2)
  {
    return x1+x2.first;
  }

  template <template <class,class> class TemplateFunctional, class OriginalVariableSetDescription, class ExtensionVariableSetDescription, class ExtensionSpace>
  class GoalOrientedErrorEstimator : public AbstractHierarchicalErrorEstimator
  {
    static constexpr int noOfVariables = ExtensionVariableSetDescription::noOfVariables;

    // define functional types
    typedef TemplateFunctional<OriginalVariableSetDescription,OriginalVariableSetDescription> F_hh;
    typedef TemplateFunctional<OriginalVariableSetDescription,ExtensionVariableSetDescription> F_he;
    typedef TemplateFunctional<ExtensionVariableSetDescription,OriginalVariableSetDescription> F_eh;
    typedef TemplateFunctional<ExtensionVariableSetDescription,ExtensionVariableSetDescription> F_ee;

    static constexpr int yIdx = F_hh::yIdx;
    static constexpr int uIdx = F_hh::uIdx;
    static constexpr int pIdx = F_hh::lIdx;

    // define an assembler for each functional
    typedef VariationalFunctionalAssembler<LinearizationAt<F_hh> > Assembler_hh;
    typedef VariationalFunctionalAssembler<LinearizationAt<F_eh> > Assembler_eh;
    typedef VariationalFunctionalAssembler<LinearizationAt<F_he> > Assembler_he;
    typedef VariationalFunctionalAssembler<LinearizationAt<F_ee> > Assembler_ee;

    typedef typename OriginalVariableSetDescription::VariableSet VariableSet;
    typedef typename ExtensionVariableSetDescription::VariableSet ExtensionVariableSet;

    typedef typename ExtensionVariableSetDescription::GridView::template Codim<0>::Iterator CellIterator;

  public:
    typedef typename F_hh::Scalar Scalar;
    static constexpr int dim = VariableSet::Descriptions::Grid::dimension;

    // error estimator and corresponding assembler
    typedef HierarchicErrorEstimator<LinearizationAt<F_hh>,ExtensionVariableSetDescription,ExtensionVariableSetDescription,HierarchicErrorEstimatorDetail::TakeAllD2<LinearizationAt<F_hh> > > ErrorEstimator;
    typedef VariationalFunctionalAssembler<ErrorEstimator> ExtensionAssembler;

    // coefficient vectors
    typedef typename OriginalVariableSetDescription::template CoefficientVectorRepresentation<>::type Vector;
    typedef typename OriginalVariableSetDescription::template CoefficientVectorRepresentation<> VectorCreator;
    typedef typename OriginalVariableSetDescription::template CoefficientVectorRepresentation<uIdx,uIdx+1>::type UVector;
    typedef typename OriginalVariableSetDescription::template CoefficientVectorRepresentation<yIdx,yIdx+1>::type YVector;
    typedef typename OriginalVariableSetDescription::template CoefficientVectorRepresentation<uIdx,uIdx+1> UVectorCreator;
    typedef typename OriginalVariableSetDescription::template CoefficientVectorRepresentation<yIdx,yIdx+1> YVectorCreator;

    // coefficient vectors in extended space
    typedef typename ExtensionVariableSetDescription::template CoefficientVectorRepresentation<>::type ExtensionVector;
    typedef typename ExtensionVariableSetDescription::template CoefficientVectorRepresentation<uIdx,uIdx+1>::type UExtensionVector;
    typedef typename ExtensionVariableSetDescription::template CoefficientVectorRepresentation<yIdx,yIdx+1>::type YExtensionVector;
    typedef typename ExtensionVariableSetDescription::template CoefficientVectorRepresentation<> ExtensionVectorCreator;
    typedef typename ExtensionVariableSetDescription::template CoefficientVectorRepresentation<uIdx,uIdx+1> UExtensionVectorCreator;
    typedef typename ExtensionVariableSetDescription::template CoefficientVectorRepresentation<yIdx,yIdx+1> YExtensionVectorCreator;

    GoalOrientedErrorEstimator(ExtensionVariableSetDescription& extensionVariableSetDescription_, ExtensionSpace& extensionSpace_, Scalar fraction_=0.8, size_t maxNoOfCells_=5000, bool onlyLowerTriangle_=false)
    : extensionVariableSetDescription(extensionVariableSetDescription_), extensionSpace(extensionSpace_), extensionAssembler(extensionVariableSetDescription.spaces),
      fraction(fraction_), maxNoOfCells(maxNoOfCells_), onlyLowerTriangle(onlyLowerTriangle_)
    {}

    ~GoalOrientedErrorEstimator(){}

    void operator()(AbstractVector const& x_, AbstractVector const& dx_, int step)
    {
      if(extensionSpace.gridManager().grid().size(0) > maxNoOfCells)
      {
        std::cout << "Skipping error estimation." << std::endl;
        return;
      }

      std::cout << "Starting error estimation." << std::endl;

      Bridge::Vector<VariableSet> const& x = dynamic_cast<const Bridge::Vector<VariableSet>&>(x_);
      Bridge::Vector<VariableSet> const& dx = dynamic_cast<const Bridge::Vector<VariableSet>&>(dx_);

      //OriginalVariableSetDescription const& variableSetDescription = x.get().descriptions;

      using namespace boost::fusion;

      MatrixProperties property = MatrixProperties::GENERAL;
      DirectType directType = DirectType::UMFPACK;

      std::cout << "computing residual in extension space." << std::endl;
      //ExtensionAssembler extensionAssembler(extensionVariableSetDescription.spaces);
      extensionAssembler.assemble(ErrorEstimator(LinearizationAt<F_hh>(*f_hh,x.get()),dx.get()));
      AssembledGalerkinOperator<ExtensionAssembler,0,noOfVariables,0,noOfVariables> H_ee(extensionAssembler); // in space extension
      ExtensionVector rhs_e(extensionAssembler.rhs());
      rhs_e *= -1;
      ExtensionVector dx_e(ExtensionVectorCreator::init(extensionVariableSetDescription));
      // compute correction in extension space
      directInverseOperator(H_ee,directType,property).apply(rhs_e,dx_e);
      // create all necessary assemblers
      Assembler_hh assembler_hh(x.get().descriptions.spaces);
      Assembler_ee assembler_ee(extensionVariableSetDescription.spaces);
      Assembler_eh assembler_eh(extensionVariableSetDescription.spaces);
      Assembler_he assembler_he(extensionVariableSetDescription.spaces);

      assembler_hh.assemble(linearization(*f_hh,x.get()));
      assembler_he.assemble(linearization(*f_he,x.get()));
      ExtensionVariableSet x_e(extensionVariableSetDescription);
      x_e *= 0;
      assembler_eh.assemble(linearization(*f_eh,x_e));
      assembler_ee.assemble(linearization(*f_ee,x_e));

      /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
      std::cout << "computing weight functions 1" << std::endl;
      // compute weight function for state variable
      // 1. get rhs
      YVector tmpRhs_h(YVectorCreator::init(x.get().descriptions));
      YExtensionVector tmpRhs_e(YExtensionVectorCreator::init(extensionVariableSetDescription));

      UVector du_h( at_c<uIdx>(dx.get().data).coefficients() );
      UExtensionVector du_e( at_c<uIdx>(dx_e.data) );

      {
        AssembledGalerkinOperator<Assembler_hh,pIdx,pIdx+1,uIdx,uIdx+1> B_hh(assembler_hh, false);
        AssembledGalerkinOperator<Assembler_he,pIdx,pIdx+1,uIdx,uIdx+1> B_he(assembler_he, false);
        AssembledGalerkinOperator<Assembler_eh,pIdx,pIdx+1,uIdx,uIdx+1> B_eh(assembler_eh, false);
        AssembledGalerkinOperator<Assembler_ee,pIdx,pIdx+1,uIdx,uIdx+1> B_ee(assembler_ee, false);

        B_hh.apply(du_h, tmpRhs_h);
        B_eh.applyscaleadd(1.0, du_e, tmpRhs_h);
        tmpRhs_h *= -1;
        B_he.apply(du_h, tmpRhs_e);
        B_ee.applyscaleadd(1.0, du_e, tmpRhs_e);
        tmpRhs_e *= -1;
      }

      // 2. compute weight
      AssembledGalerkinOperator<Assembler_hh,pIdx,pIdx+1,yIdx,yIdx+1> A_hh(assembler_hh, false);
      AssembledGalerkinOperator<Assembler_eh,pIdx,pIdx+1,yIdx,yIdx+1> A_eh(assembler_eh, false);
      AssembledGalerkinOperator<Assembler_ee,pIdx,pIdx+1,yIdx,yIdx+1> A_ee(assembler_ee, false);
      AssembledGalerkinOperator<Assembler_he,pIdx,pIdx+1,yIdx,yIdx+1> A_he(assembler_he, false);
      YVector wy_h(YVectorCreator::init(x.get().descriptions));
      wy_h *= 0;
      YExtensionVector wy_e(YExtensionVectorCreator::init(extensionVariableSetDescription));

      //JacobiPreconditioner<AssembledGalerkinOperator<Assembler_hh,pIdx,pIdx+1,yIdx,yIdx+1> >(A_hh).apply(tmpRhs_h,wy_h);
      //A_hh.apply(tmpRhs_h, wy_h);
      InverseLinearOperator<DirectSolver<YVector,YVector> > ds_hh(DirectSolver<YVector,YVector>(A_hh,DirectType::MUMPS,MatrixProperties::SYMMETRIC));
      ds_hh.apply(tmpRhs_h, wy_h);
      A_he.applyscaleadd(-1.0,wy_h,tmpRhs_e);
      //JacobiPreconditioner<AssembledGalerkinOperator<Assembler_ee,pIdx,pIdx+1,yIdx,yIdx+1> >(A_ee).apply(tmpRhs_e,wy_e);
      directInverseOperator(A_ee,directType,property).apply(tmpRhs_e, wy_e);

      /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
      std::cout << "computing weight functions 2" << std::endl;
      // compute weight function for adjoint variable
      // 1. get rhs
      {
        AssembledGalerkinOperator<Assembler_hh,yIdx,yIdx+1,yIdx,yIdx+1> H_hh(assembler_hh, false);
        AssembledGalerkinOperator<Assembler_he,yIdx,yIdx+1,yIdx,yIdx+1> H_he(assembler_he, false);
        AssembledGalerkinOperator<Assembler_eh,yIdx,yIdx+1,yIdx,yIdx+1> H_eh(assembler_eh, false);
        AssembledGalerkinOperator<Assembler_ee,yIdx,yIdx+1,yIdx,yIdx+1> H_ee(assembler_ee, false);
        H_hh.apply(wy_h,tmpRhs_h);
        H_eh.applyscaleadd(1.0,wy_e,tmpRhs_h);
        tmpRhs_h *= -1;
        H_he.apply(wy_h,tmpRhs_e);
        H_ee.applyscaleadd(1.0,wy_e,tmpRhs_e);
        tmpRhs_e *= -1;
      }

      // 2. compute weight
      YVector wp_h(YVectorCreator::init(x.get().descriptions));
      YExtensionVector wp_e(YExtensionVectorCreator::init(extensionVariableSetDescription));
      {
        //JacobiPreconditioner<AssembledGalerkinOperator<Assembler_hh,pIdx,pIdx+1,yIdx,yIdx+1> >(A_hh).apply(tmpRhs_h,wp_h);
        directInverseOperator(A_hh,directType,property).apply(tmpRhs_h, wp_h);
        A_he.applyscaleadd(-1.0,wp_h, tmpRhs_e);
        //JacobiPreconditioner<AssembledGalerkinOperator<Assembler_ee,pIdx,pIdx+1,yIdx,yIdx+1> >(A_ee).apply(tmpRhs_e,wp_e);
        directInverseOperator(A_ee,directType,property).apply(tmpRhs_e, wp_e);
      }

      Scalar estimatedError = 0.5 * ( at_c<0>(wy_e.data)*at_c<yIdx>(rhs_e.data)
          + at_c<0>(du_e.data)*at_c<uIdx>(rhs_e.data)
          + at_c<0>(wp_e.data)*at_c<pIdx>(rhs_e.data) );

      std::cout << "estimated error: " << estimatedError << std::endl;

      // get number of cells corresponding to edges resp. edge shape functions used in the error estimator
      CellIterator cend = extensionVariableSetDescription.gridView.template end<0>();
      std::vector<size_t> neighBoringCells(extensionVariableSetDescription.gridView.size(dim-1),0);
      for(CellIterator ci = extensionVariableSetDescription.gridView.template begin<0>(); ci!=cend; ++ci)
      {
        // iterate over cells edges
        for(int edgeIdInEntity=0; edgeIdInEntity<3; ++edgeIdInEntity)
        {
          int id = extensionVariableSetDescription.gridView.indexSet().subIndex(*ci, edgeIdInEntity, dim-1);
          if(id < neighBoringCells.size())
            neighBoringCells[id]++;
        }
      }
//      for(size_t i=0; i<neighBoringCells.size(); ++i) if(neighBoringCells[i]==0) std::cout << "missing cells at edge id: " << i << std::endl;
//
      // estimate cell-wise error distribution
      typename ExtensionVariableSetDescription::VariableSet mySol(extensionVariableSetDescription);
      mySol *= 0;
      std::vector<Scalar> edgeErrorIndicators(at_c<yIdx>(dx_e.data).N());
      std::cout << "edgeErrorIndicators: " << edgeErrorIndicators.size() << std::endl;
      std::cout << "mySol: " << boost::fusion::at_c<1>(mySol.data).coefficients().N() << std::endl;
      for(size_t i=0; i<edgeErrorIndicators.size(); ++i)
      {
        edgeErrorIndicators[i] = 0.5 * ( at_c<0>(wy_e.data)[i]*at_c<yIdx>(rhs_e.data)[i] + at_c<0>(du_e.data)[0]*at_c<uIdx>(rhs_e.data)[0] + at_c<0>(wp_e.data)[i]*at_c<pIdx>(rhs_e.data)[i] );
        boost::fusion::at_c<1>(mySol.data).coefficients()[i][0] = edgeErrorIndicators[i];
        edgeErrorIndicators[i] /= neighBoringCells[i];
      }

      errorDistribution.clear();
      errorDistribution.resize(extensionVariableSetDescription.gridView.size(0),std::make_pair(0.0,0));
      auto const& is = extensionSpace.gridManager().grid().leafIndexSet();
//
//      typedef ErrorDistribution<F_hh,ExtensionVariableSetDescription> EnergyError;
//      EnergyError energyError(*f_hh,x.get(),mySol);
//      typedef VariationalFunctionalAssembler<LinearizationAt<EnergyError> > EnergyErrorAssembler;
//      EnergyErrorAssembler eeAssembler(extensionSpace.gridManager(), energyError.getSpaces());
//
//      eeAssembler.assemble(linearization(energyError,x.get()), EnergyErrorAssembler::RHS);
//      typename EnergyError::ErrorVector distError( eeAssembler.rhs() );
//      //if(!nearMinimizer_) distError *= 0;
//      typename EnergyError::AnsatzVars::VariableSet mde( energyError.getVariableSetDescription() );
//      mde = distError;

      for (CellIterator ci=extensionVariableSetDescription.gridView.template begin<0>(); ci!=cend; ++ci)
//        errorDistribution[is.index(*ci)] = std::make_pair( fabs(boost::fusion::at_c<0>(mde.data).value(*ci,Dune::FieldVector<Scalar,dim>(0.3))) , is.index(*ci));
      {
        size_t index = is.index(*ci);
        errorDistribution[index] = std::make_pair(0,index);
        size_t nEdges = (dim==2) ? 3 : 6;
        for(int edgeIdInEntity=0; edgeIdInEntity<nEdges; ++edgeIdInEntity)
        {
          typedef typename ExtensionVariableSetDescription::GridView::Grid Grid;
//          std::cout << "edgeInInEntity: " << edgeIdInEntity << std::endl;
          size_t globalEdgeId = Dune::GenericReferenceElements<typename Grid::ctype,Grid::dimension>::simplex().subEntity(edgeIdInEntity,Grid::dimension-1,0,Grid::dimension);
          //size_t globalEdgeId = is.subIndex(*ci, edgeIdInEntity, dim-1);
          errorDistribution[index].first += edgeErrorIndicators[globalEdgeId];
        }
     }

      totalError = std::accumulate(errorDistribution.begin(), errorDistribution.end(), 0.0, add);
      //      typedef FEFunctionSpace<DiscontinuousLagrangeMapper<Scalar,typename ExtensionSpace::GridView> > AnsatzSpace;
      //      typedef boost::fusion::vector<AnsatzSpace const*> AnsatzSpaces;
      //      typedef Variable<SpaceIndex<0>,Components<1>,VariableId<0> > AnsatzVariableInformation;
      //      typedef boost::fusion::vector<AnsatzVariableInformation> VariableDescriptions;
      //      typedef VariableSetDescription<AnsatzSpaces,VariableDescriptions> AnsatzVars;
      //      AnsatzSpace discontinuousSpace(extensionSpace.gridManager(),extensionSpace.gridManager().grid().leafView(),0);
      //      AnsatzSpaces discontinuousSpaces(&discontinuousSpace);
      //      std::string dname[] = { "error" };
      //      AnsatzVars errorRepVarSetDesc(discontinuousSpaces,dname);
      //      typename AnsatzVars::VariableSet discontinuousErrorRepresentation(errorRepVarSetDesc);
      //
      //      std::cout << "before transfer" << std::endl;
      //      spaceTransfer(boost::fusion::at_c<0>(discontinuousErrorRepresentation.data),
      //                    boost::fusion::at_c<0>(edgeErrorElement.data));
      //      std::cout << "after" << std::endl;
      //
      ////     typename AnsatzSpace::template Element<1> errorRep(discontinuousSpace);
      //
      //      for(CellIterator ci = extensionVariableSetDescription.gridView.template begin<0>(); ci!=cend; ++ci)
      //      {
      ////        typedef typename std::remove_pointer<typename boost::mpl::at_c<typename OriginalVariableSetDescription::Spaces,0>::type>::type Space;
      ////        typedef typename Space::Mapper::GlobalIndexRange IndexMapper;
      //        typedef typename ExtensionSpace::Mapper::GlobalIndexRange IndexMapper;
      //        Scalar err = 0;
      //        IndexMapper mapper = extensionSpace.mapper().globalIndices(*ci);
      //        typename IndexMapper::iterator iend = mapper.end();
      //        size_t index = extensionVariableSetDescription.gridView.indexSet().index(*ci);
      //        std::cout << "index: " << index << std::endl;
      //        if(index==2)
      //        {
      //          for(typename IndexMapper::iterator iter=mapper.begin(); iter!=iend; ++iter)
      //          {
      //            std::cout << "it: " << *iter << std::endl;
      //            std::cout << "err: " << err << std::endl;
      //            std::cout << "eei: " << edgeErrorIndicators[*iter] << std::endl;
      ////            std::cout << "nbc: " << neighBoringCells[*iter] << std::endl;
      //
      //            err += edgeErrorIndicators[*iter];//neighBoringCells[*iter];
      //          }
      //        }
      //        else
      //        for(typename IndexMapper::iterator iter=mapper.begin(); iter!=iend; ++iter) err += edgeErrorIndicators[*iter];///neighBoringCells[*iter];
      //
      ////        errorDistribution[extensionVariableSetDescription.gridView.indexSet().index(*ci)] = err;
      //
      //        errorDistribution[index] = std::make_pair(err,index);
      //        if(std::isinf(err))
      //        {
      //          std::cout << "err: inf at index " << index << std::endl;
      //          exit(-1);
      //        }
      //        if(std::isnan(err))
      //        {
      //          std::cout << "err: nan at index " << index << std::endl;
      //          exit(-1);
      //        }
      //      }
      //      std::cout << "total error " << std::endl;
      //      totalError = std::accumulate(errorDistribution.begin(), errorDistribution.end(), 0.0, add);
      //      std::cout << "total error done" << std::endl;
      ////            std::vector<Scalar> errorDistribution(variableSetDescription.gridView.size(0),0);
      ////            for(CellIterator ci = variableSetDescription.gridView.template begin<0>(); ci!=cend; ++ci)
      ////            {
      ////              for(int edgeIdInEntity=0; edgeIdInEntity<(dim==2) ? 3 : 6; ++edgeIdInEntity)
      ////              {
      ////                size_t globalEdgeId = variableSetDescription.gridView.indexSet().subIndex(*ci, edgeIdInEntity, dim-1);
      ////
      ////              }
      ////            }

    }

    template <typename... Args>
    void initializeFunctionals(Args... args)
    {
      f_hh.reset(new F_hh(args...));
      f_eh.reset(new F_eh(args...));
      f_he.reset(new F_he(args...));
      f_ee.reset(new F_ee(args...));
    }

    bool nearMinimizer() const { return true; }

    void refineGrid()
    {
      if(extensionSpace.gridManager().grid().size(0) > maxNoOfCells)
      {
        std::cout << "ERROR ESTIMATOR: Grid has more than " << maxNoOfCells << " cells -> No more refinement." << std::endl;
        return;
      }

      std::sort(errorDistribution.begin(), errorDistribution.end(), biggerThanAbs);

      double errLevel = errorDistribution[3*errorDistribution.size()/4].first;
      auto const& is = extensionSpace.gridManager().grid().leafIndexSet();

      CellIterator cend = extensionVariableSetDescription.gridView.template end<0>();
      for (CellIterator ci=extensionVariableSetDescription.gridView.template begin<0>(); ci!=cend; ++ci) // iterate over cells
        if(errorDistribution[is.index(*ci)].first > errLevel) extensionSpace.gridManager().mark(1,*ci);

//      Scalar bulkCriterionTolerance = fraction*totalError;
//      std::cout << "totalError: " << totalError << std::endl;
//      std::cout << "bulkCriterionTolerance: " << bulkCriterionTolerance << std::endl;
//      Scalar bulkErrorSquared = 0;
//
//      std::vector<std::pair<Scalar,size_t> > bulkErrorDistribution;
//      auto const& is = extensionSpace.gridManager().grid().leafIndexSet();
//      CellIterator cend = extensionVariableSetDescription.gridView.template end<0>();
//
//      while(bulkErrorSquared < bulkCriterionTolerance)
//      {
//        bulkErrorDistribution.push_back(errorDistribution[bulkErrorDistribution.size()]);
//        bulkErrorSquared += bulkErrorDistribution.back().first;
//      }
//
//      std::cout << "number of candidates for refinement: " << bulkErrorDistribution.size() << std::endl;
//
//
//
//      // Refine mesh.
//      for (CellIterator ci=extensionVariableSetDescription.gridView.template begin<0>(); ci!=cend; ++ci) // iterate over cells
//      {
//        for(int i=0; i<bulkErrorDistribution.size(); ++i) // iterate over chosen part of the error distribution
//          if(is.index(*ci) == bulkErrorDistribution[i].second)
//            extensionSpace.gridManager().mark(1,*ci);
//      }

      extensionSpace.gridManager().adaptAtOnce();
    }

    double estimatedAbsoluteError() const
    {
      return fabs(totalError);
    }

    size_t gridSize() const final
    {
      return extensionSpace.gridManager().grid().size(0);
    }

  private:
    //OriginalVariableSetDescription const& variableSetDescription;
    ExtensionVariableSetDescription& extensionVariableSetDescription;
    ExtensionSpace& extensionSpace;
    ExtensionAssembler extensionAssembler;
    Scalar fraction;
    Scalar totalError;
    size_t maxNoOfCells;
    bool onlyLowerTriangle;
    std::vector<std::pair<double,size_t> > errorDistribution;
    //    std::vector<Scalar> errorDistribution;
    std::unique_ptr<F_hh> f_hh;
    std::unique_ptr<F_eh> f_eh;
    std::unique_ptr<F_he> f_he;
    std::unique_ptr<F_ee> f_ee;
  };
}

#endif


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#define BOOST_TEST_MAIN

#include <limits>
#include <string>
#include <thread>

#include <boost/make_shared.hpp>
#include <boost/test/unit_test.hpp>

#include "Tudat/Astrodynamics/BasicAstrodynamics/unitConversions.h"
#include "Tudat/Mathematics/BasicMathematics/linearAlgebra.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/physicalConstants.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/orbitalElementConversions.h"

#include "Tudat/External/SpiceInterface/spiceInterface.h"
#include "Tudat/Mathematics/NumericalIntegrators/rungeKuttaCoefficients.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/accelerationModel.h"
#include "Tudat/InputOutput/basicInputOutput.h"
#include "Tudat/SimulationSetup/PropagationSetup/dynamicsSimulator.h"
#include "Tudat/SimulationSetup/EnvironmentSetup/defaultBodies.h"
#include "Tudat/SimulationSetup/EnvironmentSetup/createBodies.h"
#include "Tudat/SimulationSetup/EstimationSetup/createNumericalSimulator.h"
#include "Tudat/Mathematics/NumericalIntegrators/createNumericalIntegrator.h"

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_encke_propagator )

// Test Encke propagator for point mass central body.
BOOST_AUTO_TEST_CASE( testEnckePopagatorForPointMassCentralBodies )
{
    // Test simulation for diffe  rent central body cases
    for( unsigned int simulationCase = 0; simulationCase < 2; simulationCase++ )
    {
        //Using declarations.
        using namespace tudat::interpolators;
        using namespace tudat::numerical_integrators;
        using namespace tudat::spice_interface;
        using namespace tudat::simulation_setup;
        using namespace tudat::basic_astrodynamics;
        using namespace tudat::orbital_element_conversions;
        using namespace tudat::propagators;

        //Load spice kernels.
        spice_interface::loadStandardSpiceKernels( );

        // Define bodies in simulation.
        unsigned int totalNumberOfBodies = 7;
        std::vector< std::string > bodyNames;
        bodyNames.resize( totalNumberOfBodies );
        bodyNames[ 0 ] = "Earth";
        bodyNames[ 1 ] = "Mars";
        bodyNames[ 2 ] = "Sun";
        bodyNames[ 3 ] = "Venus";
        bodyNames[ 4 ] = "Moon";
        bodyNames[ 5 ] = "Mercury";
        bodyNames[ 6 ] = "Jupiter";

        double initialEphemerisTime = 1.0E7;
        double finalEphemerisTime = 2.0E7;
        double maximumTimeStep = 3600.0;
        double buffer = 5.0 * maximumTimeStep;

        // Create bodies needed in simulation
        NamedBodyMap bodyMap = createBodies(
                    getDefaultBodySettings( bodyNames, initialEphemerisTime - buffer, finalEphemerisTime + buffer ) );
        setGlobalFrameBodyEphemerides( bodyMap, "SSB", "ECLIPJ2000" );

        // Set accelerations between bodies that are to be taken into account.
        SelectedAccelerationMap accelerationMap;
        std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfEarth;
        accelerationsOfEarth[ "Sun" ].push_back( std::make_shared< AccelerationSettings >( central_gravity ) );
        accelerationsOfEarth[ "Moon" ].push_back( std::make_shared< AccelerationSettings >( central_gravity ) );
        accelerationsOfEarth[ "Jupiter" ].push_back( std::make_shared< AccelerationSettings >( central_gravity ) );
        accelerationMap[ "Earth" ] = accelerationsOfEarth;

        std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfMars;
        accelerationsOfMars[ "Sun" ].push_back( std::make_shared< AccelerationSettings >( central_gravity ) );
        accelerationsOfMars[ "Earth" ].push_back( std::make_shared< AccelerationSettings >( central_gravity ) );
        accelerationsOfMars[ "Jupiter" ].push_back( std::make_shared< AccelerationSettings >( central_gravity ) );
        accelerationMap[ "Mars" ] = accelerationsOfMars;

        std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfMoon;
        accelerationsOfMoon[ "Sun" ].push_back( std::make_shared< AccelerationSettings >( central_gravity ) );
        accelerationsOfMoon[ "Earth" ].push_back( std::make_shared< AccelerationSettings >( central_gravity ) );
        accelerationsOfMoon[ "Jupiter" ].push_back( std::make_shared< AccelerationSettings >( central_gravity ) );
        accelerationMap[ "Moon" ] = accelerationsOfMoon;

        // Propagate Earth, Mars and Moon
        std::vector< std::string > bodiesToPropagate;
        bodiesToPropagate.push_back( "Earth" );
        bodiesToPropagate.push_back( "Mars" );
        bodiesToPropagate.push_back( "Moon" );

        unsigned int numberOfNumericalBodies = bodiesToPropagate.size( );

        // Define central bodies: all Sun for simulationCase = 0, Earth and Mars: Sun, Moon: Earth for simulationCase = 1
        std::vector< std::string > centralBodies;
        std::map< std::string, std::string > centralBodyMap;
        centralBodies.resize( numberOfNumericalBodies );
        for( int i = 0; i < 3; i++ )
        {
            if( i == 2 && simulationCase == 1 )
            {
                centralBodies[ i ] = "Earth";
            }
            else
            {
                centralBodies[ i ] = "Sun";
            }
            centralBodyMap[ bodiesToPropagate[ i ] ] = centralBodies[ i ];
        }

        // Get initial states for bodies.
        Eigen::VectorXd systemInitialState = Eigen::VectorXd( bodiesToPropagate.size( ) * 6 );
        for( unsigned int i = 0; i < numberOfNumericalBodies ; i++ )
        {
            systemInitialState.segment( i * 6 , 6 ) =
                    bodyMap[ bodiesToPropagate[ i ] ]->getStateInBaseFrameFromEphemeris( initialEphemerisTime ) -
                    bodyMap[ centralBodies[ i ] ]->getStateInBaseFrameFromEphemeris( initialEphemerisTime );
        }

        // Create acceleratiuon models.
        AccelerationMap accelerationModelMap = createAccelerationModelsMap(
                    bodyMap, accelerationMap, centralBodyMap );

        // Create integrator settings.
        std::shared_ptr< IntegratorSettings< > > integratorSettings =
                std::make_shared< IntegratorSettings< > >
                ( rungeKutta4,
                  initialEphemerisTime, 250.0 );

        // Create propagation settings (Cowell)
        std::shared_ptr< TranslationalStatePropagatorSettings< double > > propagatorSettings =
                std::make_shared< TranslationalStatePropagatorSettings< double > >
                ( centralBodies, accelerationModelMap, bodiesToPropagate, systemInitialState, finalEphemerisTime );

        // Propagate orbit with Cowell method
        SingleArcDynamicsSimulator< double > dynamicsSimulator2(
                    bodyMap, integratorSettings, propagatorSettings, true, false, true );

        // Define ephemeris interrogation settings.
        double initialTestTime = initialEphemerisTime + 10.0 * maximumTimeStep;
        double finalTestTime = finalEphemerisTime - 10.0 * maximumTimeStep;
        double testTimeStep = 1.0E4;

        // Get resutls of Cowell integration at given times.
        double currentTestTime = initialTestTime;
        std::map< double, Eigen::Matrix< double, 18, 1 > > cowellIntegrationResults;
        while( currentTestTime < finalTestTime )
        {
            cowellIntegrationResults[ currentTestTime ].segment( 0, 6 ) =
                    bodyMap[ "Earth" ]->getStateInBaseFrameFromEphemeris( currentTestTime );
            cowellIntegrationResults[ currentTestTime ].segment( 6, 6 ) =
                    bodyMap[ "Mars" ]->getStateInBaseFrameFromEphemeris( currentTestTime );
            cowellIntegrationResults[ currentTestTime ].segment( 12, 6 ) =
                    bodyMap[ "Moon" ]->getStateInBaseFrameFromEphemeris( currentTestTime );

            currentTestTime += testTimeStep;
        }

        // Create propagation settings (Encke)
        propagatorSettings = std::make_shared< TranslationalStatePropagatorSettings< double > >
                ( centralBodies, accelerationModelMap, bodiesToPropagate, systemInitialState, finalEphemerisTime, encke );

        // Propagate orbit with Encke method
        SingleArcDynamicsSimulator< double > dynamicsSimulator(
                    bodyMap, integratorSettings, propagatorSettings, true, false, true );

        // Get resutls of Encke integration at given times.
        currentTestTime = initialTestTime;
        std::map< double, Eigen::Matrix< double, 18, 1 > > enckeIntegrationResults;
        while( currentTestTime < finalTestTime )
        {
            enckeIntegrationResults[ currentTestTime ].segment( 0, 6 ) =
                    bodyMap[ "Earth" ]->getStateInBaseFrameFromEphemeris( currentTestTime );
            enckeIntegrationResults[ currentTestTime ].segment( 6, 6 ) =
                    bodyMap[ "Mars" ]->getStateInBaseFrameFromEphemeris( currentTestTime );
            enckeIntegrationResults[ currentTestTime ].segment( 12, 6 ) =
                    bodyMap[ "Moon" ]->getStateInBaseFrameFromEphemeris( currentTestTime );
            currentTestTime += testTimeStep;
        }

        // Compare results of Cowell and Encke propagations
        std::map< double, Eigen::Matrix< double, 18, 1 > >::iterator enckeIterator = enckeIntegrationResults.begin( );
        std::map< double, Eigen::Matrix< double, 18, 1 > >::iterator cowellIterator = cowellIntegrationResults.begin( );
        for( unsigned int i = 0; i < enckeIntegrationResults.size( ); i++ )
        {
            for( int j= 0; j< 3; j++ )
            {
                BOOST_CHECK_SMALL( ( enckeIterator->second - cowellIterator->second ).segment( j, 1 )( 0 ), 0.01 );
            }

            for( int j = 6; j < 9; j++ )
            {
                BOOST_CHECK_SMALL( ( enckeIterator->second - cowellIterator->second ).segment( j, 1 )( 0 ), 0.01 );
            }

            for( int j = 12; j < 15; j++ )
            {
                BOOST_CHECK_SMALL( ( enckeIterator->second - cowellIterator->second ).segment( j, 1 )( 0 ), 0.1 );
            }

            for( int j = 3; j < 6; j++ )
            {
                BOOST_CHECK_SMALL( ( enckeIterator->second - cowellIterator->second ).segment( j, 1 )( 0 ), 1.0E-8 );
            }

            for( int j = 9; j < 12; j++ )
            {
                BOOST_CHECK_SMALL( ( enckeIterator->second - cowellIterator->second ).segment( j, 1 )( 0 ), 1.0E-8 );

            }

            for( int j = 15; j < 18; j++ )
            {
                BOOST_CHECK_SMALL( ( enckeIterator->second - cowellIterator->second ).segment( j, 1 )( 0 ), 1.0E-6 );

            }
            enckeIterator++;
            cowellIterator++;
        }
    }
}

// Test Encke propagator for point mass, and spherical harmonics central body.
BOOST_AUTO_TEST_CASE( testEnckePopagatorForSphericalHarmonicCentralBodies )
{
    for( unsigned int simulationCase = 0; simulationCase < 4; simulationCase++ )
    {
        using namespace tudat;
        using namespace simulation_setup;
        using namespace propagators;
        using namespace numerical_integrators;
        using namespace orbital_element_conversions;
        using namespace basic_mathematics;
        using namespace gravitation;
        using namespace numerical_integrators;

        // Load Spice kernels.
        spice_interface::loadStandardSpiceKernels( );

        // Set simulation time settings.
        const double simulationStartEpoch = 0.0;
        const double simulationEndEpoch = tudat::physical_constants::JULIAN_DAY;

        // Define body settings for simulation.
        std::vector< std::string > bodiesToCreate;
        bodiesToCreate.push_back( "Sun" );
        bodiesToCreate.push_back( "Earth" );
        bodiesToCreate.push_back( "Moon" );
        bodiesToCreate.push_back( "Mars" );
        bodiesToCreate.push_back( "Venus" );

        // Create body objects.
        std::map< std::string, std::shared_ptr< BodySettings > > bodySettings =
                getDefaultBodySettings( bodiesToCreate, simulationStartEpoch - 300.0, simulationEndEpoch + 300.0 );
        for( unsigned int i = 0; i < bodiesToCreate.size( ); i++ )
        {
            bodySettings[ bodiesToCreate.at( i ) ]->ephemerisSettings->resetFrameOrientation( "J2000" );
            bodySettings[ bodiesToCreate.at( i ) ]->rotationModelSettings->resetOriginalFrame( "J2000" );
        }
        NamedBodyMap bodyMap = createBodies( bodySettings );

        // Create spacecraft object.
        bodyMap[ "Vehicle" ] = std::make_shared< simulation_setup::Body >( );
        bodyMap[ "Vehicle" ]->setConstantBodyMass( 400.0 );
        bodyMap[ "Vehicle" ]->setEphemeris( std::make_shared< ephemerides::TabulatedCartesianEphemeris< > >(
                                                std::shared_ptr< interpolators::OneDimensionalInterpolator
                                                < double, Eigen::Vector6d  > >( ), "Earth", "J2000" ) );
        std::shared_ptr< RadiationPressureInterfaceSettings > vehicleRadiationPressureSettings =
                std::make_shared< CannonBallRadiationPressureInterfaceSettings >(
                    "Sun", 4.0, 1.2, std::vector< std::string >{ "Earth", "Moon" } );
        bodyMap[ "Vehicle" ]->setRadiationPressureInterface(
                    "Sun", createRadiationPressureInterface(
                        vehicleRadiationPressureSettings, "Vehicle", bodyMap ) );

        // Finalize body creation.
        setGlobalFrameBodyEphemerides( bodyMap, "SSB", "J2000" );

        // Define propagator settings variables.
        SelectedAccelerationMap accelerationMap;
        std::vector< std::string > bodiesToPropagate;
        std::vector< std::string > centralBodies;

        // Define propagation settings.
        std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfVehicle;

        // Use only central gravity for Earth
        if( simulationCase < 2 )
        {
            accelerationsOfVehicle[ "Earth" ].push_back( std::make_shared< AccelerationSettings >(
                                                             basic_astrodynamics::central_gravity ) );
        }
        // Use spherical harmonics for Earth
        else
        {
            accelerationsOfVehicle[ "Earth" ].push_back(
                        std::make_shared< SphericalHarmonicAccelerationSettings >( 5, 5 ) );

        }

        // Use perturbations other than Earth gravity
        if( simulationCase % 2 == 0 )
        {
            accelerationsOfVehicle[ "Sun" ].push_back( std::make_shared< AccelerationSettings >(
                                                           basic_astrodynamics::central_gravity ) );
            accelerationsOfVehicle[ "Moon" ].push_back( std::make_shared< AccelerationSettings >(
                                                            basic_astrodynamics::central_gravity ) );
            accelerationsOfVehicle[ "Mars" ].push_back( std::make_shared< AccelerationSettings >(
                                                            basic_astrodynamics::central_gravity ) );
            accelerationsOfVehicle[ "Venus" ].push_back( std::make_shared< AccelerationSettings >(
                                                             basic_astrodynamics::central_gravity ) );
            accelerationsOfVehicle[ "Sun" ].push_back( std::make_shared< AccelerationSettings >(
                                                           basic_astrodynamics::cannon_ball_radiation_pressure ) );
        }
        accelerationMap[ "Vehicle" ] = accelerationsOfVehicle;
        bodiesToPropagate.push_back( "Vehicle" );
        centralBodies.push_back( "Earth" );
        basic_astrodynamics::AccelerationMap accelerationModelMap = createAccelerationModelsMap(
                    bodyMap, accelerationMap, bodiesToPropagate, centralBodies );

        // Set Keplerian elements for Vehicle.
        Eigen::Vector6d vehicleInitialStateInKeplerianElements;
        vehicleInitialStateInKeplerianElements( semiMajorAxisIndex ) = 8000.0E3;
        vehicleInitialStateInKeplerianElements( eccentricityIndex ) = 0.1;
        vehicleInitialStateInKeplerianElements( inclinationIndex ) = unit_conversions::convertDegreesToRadians( 85.3 );
        vehicleInitialStateInKeplerianElements( argumentOfPeriapsisIndex )
                = unit_conversions::convertDegreesToRadians( 235.7 );
        vehicleInitialStateInKeplerianElements( longitudeOfAscendingNodeIndex )
                = unit_conversions::convertDegreesToRadians( 23.4 );
        vehicleInitialStateInKeplerianElements( trueAnomalyIndex ) = unit_conversions::convertDegreesToRadians( 139.87 );

        double earthGravitationalParameter = bodyMap.at( "Earth" )->getGravityFieldModel( )->getGravitationalParameter( );
        const Eigen::Vector6d vehicleInitialState = convertKeplerianToCartesianElements(
                    vehicleInitialStateInKeplerianElements, earthGravitationalParameter );

        // Define propagator settings (Cowell)
        std::shared_ptr< TranslationalStatePropagatorSettings< double > > propagatorSettings =
                std::make_shared< TranslationalStatePropagatorSettings< double > >
                ( centralBodies, accelerationModelMap, bodiesToPropagate, vehicleInitialState, simulationEndEpoch );

        // Define integrator settings.
        const double fixedStepSize = 5.0;
        std::shared_ptr< IntegratorSettings< > > integratorSettings =
                std::make_shared< IntegratorSettings< > >
                ( rungeKutta4, 0.0, fixedStepSize );

        // Propagate orbit with Cowell method
        SingleArcDynamicsSimulator< double > dynamicsSimulator2(
                    bodyMap, integratorSettings, propagatorSettings, true, false, true );

        // Define ephemeris interrogation settings.
        double initialTestTime = simulationStartEpoch + 10.0 * fixedStepSize;
        double finalTestTime = simulationEndEpoch - 10.0 * fixedStepSize;
        double testTimeStep = 1.0E4;

        // Get resutls of Cowell integration at given times.
        double currentTestTime = initialTestTime;
        std::map< double, Eigen::Matrix< double, 6, 1 > > cowellIntegrationResults;
        while( currentTestTime < finalTestTime )
        {
            cowellIntegrationResults[ currentTestTime ].segment( 0, 6 ) =
                    bodyMap[ "Vehicle" ]->getEphemeris( )->getCartesianState( currentTestTime );

            currentTestTime += testTimeStep;
        }

        // Create propagation settings (Encke)
        propagatorSettings = std::make_shared< TranslationalStatePropagatorSettings< double > >
                ( centralBodies, accelerationModelMap, bodiesToPropagate, vehicleInitialState, simulationEndEpoch, encke );

        // Propagate orbit with Encke method
        SingleArcDynamicsSimulator< double > dynamicsSimulator(
                    bodyMap, integratorSettings, propagatorSettings, true, false, true );

        // Get resutls of Encke integration at given times.
        currentTestTime = initialTestTime;
        std::map< double, Eigen::Matrix< double, 6, 1 > > enckeIntegrationResults;
        while( currentTestTime < finalTestTime )
        {
            enckeIntegrationResults[ currentTestTime ].segment( 0, 6 ) =
                    bodyMap[ "Vehicle" ]->getEphemeris( )->getCartesianState( currentTestTime );
            currentTestTime += testTimeStep;
        }

        // Compare results of Cowell and Encke propagations
        std::map< double, Eigen::Matrix< double, 6, 1 > >::iterator enckeIterator = enckeIntegrationResults.begin( );
        std::map< double, Eigen::Matrix< double, 6, 1 > >::iterator cowellIterator = cowellIntegrationResults.begin( );
        for( unsigned int i = 0; i < enckeIntegrationResults.size( ); i++ )
        {
            for( int j= 0; j< 3; j++ )
            {
                BOOST_CHECK_SMALL( ( enckeIterator->second - cowellIterator->second )( j ), 0.02 );
            }

            for( int j = 3; j < 6; j++ )
            {
                BOOST_CHECK_SMALL( ( enckeIterator->second - cowellIterator->second )( j ), 1.0E-5 );

            }
            enckeIterator++;
            cowellIterator++;
        }
    }
}

//! test if Encke propagator works properly for high eccentricities. Test if the propagation continmues unimpeded for
//! large eccentricities (was found to fail in rpevious version due to no convergence of root finder
BOOST_AUTO_TEST_CASE( testEnckePopagatorForHighEccentricities )
{
    using namespace tudat;
    using namespace simulation_setup;
    using namespace propagators;
    using namespace numerical_integrators;
    using namespace orbital_element_conversions;
    using namespace basic_mathematics;
    using namespace gravitation;
    using namespace numerical_integrators;
    using namespace unit_conversions;

    std::vector< double > testEccentricities;
    testEccentricities.push_back( 0.8 );
    testEccentricities.push_back( 0.9 );
    testEccentricities.push_back( 0.95 );
    testEccentricities.push_back( 0.99 );

    for( unsigned testCase = 0; testCase < testEccentricities.size( ); testCase++ )
    {
        // Set simulation end epoch.
        const double simulationEndEpoch = 2.0 * tudat::physical_constants::JULIAN_DAY;

        // Set numerical integration fixed step size.
        const double fixedStepSize = 15.0;

        // Define body settings for simulation.
        std::map< std::string, std::shared_ptr< BodySettings > > bodySettings;
        bodySettings[ "Earth" ] = std::make_shared< BodySettings >( );
        bodySettings[ "Earth" ]->ephemerisSettings = std::make_shared< ConstantEphemerisSettings >(
                    Eigen::Vector6d::Zero( ), "SSB", "J2000" );
        bodySettings[ "Earth" ]->gravityFieldSettings = std::make_shared< GravityFieldSettings >( central_spice );

        // Create Earth object
        NamedBodyMap bodyMap = createBodies( bodySettings );

        // Create spacecraft object.
        bodyMap[ "Asterix" ] = std::make_shared< simulation_setup::Body >( );

        // Finalize body creation.
        setGlobalFrameBodyEphemerides( bodyMap, "SSB", "J2000" );

        // Define propagator settings variables.
        SelectedAccelerationMap accelerationMap;
        std::vector< std::string > bodiesToPropagate;
        std::vector< std::string > centralBodies;

        // Define propagation settings.
        std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfAsterix;
        accelerationsOfAsterix[ "Earth" ].push_back( std::make_shared< AccelerationSettings >(
                                                         basic_astrodynamics::central_gravity ) );
        accelerationMap[ "Asterix" ] = accelerationsOfAsterix;
        bodiesToPropagate.push_back( "Asterix" );
        centralBodies.push_back( "Earth" );

        // Create acceleration models and propagation settings.
        basic_astrodynamics::AccelerationMap accelerationModelMap = createAccelerationModelsMap(
                    bodyMap, accelerationMap, bodiesToPropagate, centralBodies );

        // Set Keplerian elements for Asterix.
        Eigen::Vector6d asterixInitialStateInKeplerianElements;
        asterixInitialStateInKeplerianElements( semiMajorAxisIndex ) = 7500.0E3;
        asterixInitialStateInKeplerianElements( eccentricityIndex ) = testEccentricities.at( testCase );
        asterixInitialStateInKeplerianElements( inclinationIndex ) = convertDegreesToRadians( 85.3 );
        asterixInitialStateInKeplerianElements( argumentOfPeriapsisIndex )
                = convertDegreesToRadians( 235.7 );
        asterixInitialStateInKeplerianElements( longitudeOfAscendingNodeIndex )
                = convertDegreesToRadians( 23.4 );
        asterixInitialStateInKeplerianElements( trueAnomalyIndex ) = convertDegreesToRadians( 139.87 );

        // Convert Asterix state from Keplerian elements to Cartesian elements.
        double earthGravitationalParameter = bodyMap.at( "Earth" )->getGravityFieldModel( )->getGravitationalParameter( );
        Eigen::VectorXd systemInitialState = convertKeplerianToCartesianElements(
                    asterixInitialStateInKeplerianElements,
                    earthGravitationalParameter );

        TranslationalPropagatorType propagatorType = encke;
        std::shared_ptr< TranslationalStatePropagatorSettings< double > > propagatorSettings =
                std::make_shared< TranslationalStatePropagatorSettings< double > >
                ( centralBodies, accelerationModelMap, bodiesToPropagate, systemInitialState, simulationEndEpoch, propagatorType);

        std::shared_ptr< IntegratorSettings< > > integratorSettings =
                std::make_shared< RungeKuttaVariableStepSizeSettings< > >
                ( 0.0, fixedStepSize,
                  RungeKuttaCoefficients::rungeKuttaFehlberg78, 1.0E-4, 3600.0, 1.0E-14, 1.0E-14 );

        // Create simulation object and propagate dynamics.
        SingleArcDynamicsSimulator< > dynamicsSimulator(
                    bodyMap, integratorSettings, propagatorSettings, true, false, false );
        std::map< double, Eigen::VectorXd > integrationResult = dynamicsSimulator.getEquationsOfMotionNumericalSolution( );

        // Check if orbit is properly propagated
        Eigen::VectorXd analyticalSolution;
        double finalTime = integrationResult.rbegin( )->first;

        // Check if final time is correctly reached
        BOOST_CHECK_EQUAL( simulationEndEpoch <=  finalTime, true );

        // Loosen velocity tolerance for high eccentricity
        double toleranceMultiplier = ( testCase > 2 ) ? 100.0 : 1.0;

        // Compare propagated orbit against numerical result.
        for( std::map< double, Eigen::VectorXd >::const_iterator resultIterator = integrationResult.begin( );
             resultIterator != integrationResult.end( ); resultIterator++ )
        {
            analyticalSolution = convertKeplerianToCartesianElements( propagateKeplerOrbit(
                        asterixInitialStateInKeplerianElements, resultIterator->first, earthGravitationalParameter ),
                                         earthGravitationalParameter );
            for( unsigned int i = 0; i < 3; i++ )
            {
                BOOST_CHECK_SMALL( std::fabs( analyticalSolution( i ) - resultIterator->second( i ) ), 1.0E-2 );
                BOOST_CHECK_SMALL( std::fabs( analyticalSolution( i  + 3 ) - resultIterator->second( i + 3  ) ),
                                   toleranceMultiplier * 5.0E-6 );

            }
        }
    }

}
BOOST_AUTO_TEST_SUITE_END( )

}

}