#include <freeglut.h>
#include <glm/glm.hpp>
#include <Eigen/sparse>
#include <Eigen/Dense>
#include <iostream>

#include "cloth.h"
#include "camera.h"

using namespace std;
using namespace Eigen;

double mouseX = 0, mouseY = 0;
bool firstMouse = true;

/* ---Parameters--- */
int cloth_width = 8;
int cloth_height = 8;
double R = 0.25e-3;
double L = 1e-3;
double Y = 1e7;
double rho = 2;
double B = 1e-2;
double S = 1e4;
double Kc = 1e7;
double mu = 0.3;
double Kf = 0.3;
/*-------------------*/

Camera camera(glm::vec3(12, 11, 5), glm::vec3(0,0,1),-132,-17);
Cloth cloth(cloth_width, cloth_height, R, L, mu, rho, Y, B, S, Kc, Kf);

void init()
{
	glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
	glutInitWindowSize(600, 600);
	glutInitWindowPosition(100, 100);
	glutCreateWindow("Yarn");
	glEnable(GL_DEPTH_TEST);

	glEnable(GL_BLEND);
	glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
	glEnable(GL_POINT_SMOOTH);
	glEnable(GL_LINE_SMOOTH);
	glEnable(GL_POLYGON_SMOOTH);

	glClearColor(0.2f, 0.2f, 0.2f, 1.0);
}

void reshape(int width, int height)
{
	glViewport(0, 0, (GLsizei)width, (GLsizei)height);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0f, (GLfloat)width / (GLfloat)height, 0.05f, 100.0f);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
}

void display()
{
	glClearColor(0.2f, 0.2f, 0.2f, 1.0f);
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	gluLookAt(camera.Position.x, camera.Position.y, camera.Position.z,
		camera.Position.x + camera.Front.x, camera.Position.y + camera.Front.y, camera.Position.z + camera.Front.z,
		camera.Up.x, camera.Up.y, camera.Up.z);

	glLineWidth(2.0);
	glColor3d(1, 0, 0);
	glBegin(GL_LINES);
	glVertex3d(0, 0, 0);
	glVertex3d(10, 0, 0);
	glEnd();

	glColor3d(0, 1, 0);
	glBegin(GL_LINES);
	glVertex3d(0, 0, 0);
	glVertex3d(0, 10, 0);
	glEnd();

	glColor3d(0, 0, 1);
	glBegin(GL_LINES);
	glVertex3d(0, 0, 0);
	glVertex3d(0, 0, 10);
	glEnd();

	cloth.step(0.001);
	cloth.draw();

	glutPostRedisplay();
	glutSwapBuffers();
}

void mymouse(int button, int state, int x, int y)
{
	if (state = GLUT_UP)
	{
		firstMouse = true;
	}
}

void mouseMotion(int x, int y)
{
	if (firstMouse)
	{
		mouseX = x;
		mouseY = y;
		firstMouse = false;
	}

	double xoffset = x - mouseX;
	double yoffset = y - mouseY;

	mouseX = x;
	mouseY = y;

	camera.rotate(xoffset, yoffset);
	glutPostRedisplay();
}

void mykeyboard(unsigned char key, int x, int y)
{
	switch (key)
	{
		case 'w': camera.translate(FORWARD, 0.08); break;
		case 'a': camera.translate(LEFT, 0.08); break;
		case 's': camera.translate(BACKWARD, 0.08); break;
		case 'd': camera.translate(RIGHT, 0.08); break;
		case 'm': cloth.step(0.0001);
	}
	glutPostRedisplay();
}

void run(int argc, char* argv[])
{
	double test = 1e100;
	cout <<"TEST : "  << test << endl;
	glutInit(&argc, (char**)argv);
	init();
	glutDisplayFunc(display);
	glutReshapeFunc(reshape);
	glutKeyboardFunc(mykeyboard);
	glutMouseFunc(mymouse);
	glutMotionFunc(mouseMotion);
	glutMainLoop();
}


#include "lennard_jones_system.h"
#include "mc_sampler.h"
// C++ Standard Library
#include <iomanip>

// Boost Bind
#include <boost/bind.hpp>

// Boost Format
#include <boost/format.hpp>

namespace po = boost::program_options;

/**
 * Private Methods:
 */

void LennardJonesSystem::set_size(double s) {
  size_half = s/2;
}

void LennardJonesSystem::set_bins(size_t nb) {
  n_bins = nb;
  Q.resize(n_bins, n_bins);
  Qd.resize(n_bins, n_bins);

  std::fill(Q.data().begin(), Q.data().end(), 0);
  std::fill(Qd.data().begin(), Qd.data().end(), 0.0);
}

void LennardJonesSystem::set_particles(size_t N) {
  particles.resize(N);
  for (size_t i = 0; i < particles.size(); i++) {
    for (size_t j = 0; j < 3; j++) {
      particles[i][j] = size * dist01(rng.rng);
    }
  }

  ran_particle = boost::uniform_smallint<size_t>(0, N-1);
}

void LennardJonesSystem::calculate_dos_exact_norm() {
  std::string format = "%1%/data/lennard_jones_2/exact_%2%.dat";
  std::string fn = str( boost::format(format)
      % boost::any_cast<std::string>(settings["executable_path"])
      % n_bins
      );
  std::ifstream in(fn.c_str());

  dos_exact_norm.resize(n_bins);
  dos_exact_norm *= 0;
  bin_width = (e_max-e_min)/n_bins;
  for (size_t i = 0; i < n_bins; i++) {
    double energy;
    double dos;
    in >> energy >> dos;
    if (energy > -0.07 && energy < 0.07) {
      dos_exact_norm[i] = 0;
    } else {
      dos_exact_norm[i] = dos;
    }
  }
  dos_exact_norm /= boost::numeric::ublas::sum(dos_exact_norm);
}

void LennardJonesSystem::setup_output() {
  std::vector<std::string> sampler_string;
  // initialize list of available samplers
  sampler_string.push_back("BM");
  sampler_string.push_back("WL");
  sampler_string.push_back("QB");
  sampler_string.push_back("QA");
  sampler_string.push_back("TM");
  sampler_string.push_back("1t");

  std::string format;
  switch (sampler) {
    case 1:
      format = "lj_%1%_%2%S_%3%R_%4%M_%10%dr_%6$0.2ff%7%%8%.out";
      break;
    case 2:
    case 3:
    case 4:
    case 5:
      format = "lj_%1%_%2%S_%3%R_%4%M_%10%dr%7%%8%.out";
      break;
    case 0:
      format = "lj_%1%_%2%S_%3%R_%4%M_%10%dr_%5$0.2fT_%9%Sd%7%%8%.out";
      break;
    default:
      throw std::runtime_error("Unknown smapler");
  }
  boost::format fmt(format);
  fmt.exceptions(boost::io::all_error_bits ^ (
        boost::io::too_many_args_bit | boost::io::too_few_args_bit
        ));
  fn_template = str( fmt
      % sampler_string[sampler]
      % steps
      % "%1%"
      % n_bins
      % boost::any_cast<double>(settings["temperature"])
      % boost::any_cast<double>(settings["flatness"])
      % (tag.size() > 0 ? "_" : "")
      % tag
      % boost::any_cast<size_t>(settings["schedule"])
      % delta_r
    );

  out << "#";
  out << "\n# sampler:      " << sampler << " - " << sampler_string[sampler];
  out << "\n# steps:        " << steps;
  out << "\n# n_bins:       " << n_bins;
  out << "\n# temperature:  " << boost::any_cast<double>(settings["temperature"]);
  out << "\n# flatness:     " << boost::any_cast<double>(settings["flatness"]);
  out << "\n# one-over-t-c: " << boost::any_cast<double>(settings["one-over-t-c"]);
  out << "\n# e_min:        " << e_min;
  out << "\n# e_max:        " << e_max;
}

/**
 * Public Methods
 */

/**
 * Constructor
 */
LennardJonesSystem::LennardJonesSystem(settings_t &s)
  : SimulationSystem(s), u_sphere_dist(3) {}

po::options_description LennardJonesSystem::get_program_options() {
  po::options_description desc("LennardJones System Options");
  desc.add_options()
    (
      "lj-size",
      po::value<double>(&size)->default_value(5)->notifier(boost::bind(&LennardJonesSystem::set_size, this, _1)),
      "Box length"
    ) (
      "lj-num-particles",
      po::value<size_t>(&num_particles)->default_value(2),
      "Number of particles"
    ) (
      "lj-bins",
      po::value<size_t>(&n_bins)->default_value(100)->notifier(boost::bind(&LennardJonesSystem::set_bins, this, _1)),
      "Number of particles"
    ) (
      "lj-sigma",
      po::value<double>(&sigma)->default_value(1),
      "LJ sigma"
    ) (
      "lj-epsilon",
      po::value<double>(&epsilon)->default_value(1),
      "LJ epsilon"
    ) (
      "lj-cutoff-radius",
      po::value<double>(&cutoff_radius)->default_value(-1),
      "LJ cutoff radius, default: -1 (half of box size)"
    ) (
      "lj-emin",
      po::value<double>(&e_min)->default_value(-1),
      "LJ E_min"
    ) (
      "lj-emax",
      po::value<double>(&e_max)->default_value(3),
      "LJ E_max"
    ) (
      "lj-delta-r",
      po::value<double>(&delta_r)->default_value(1),
      "LJ Delta r"
    );
  return desc;
}

void LennardJonesSystem::parse_arguments(boost::program_options::variables_map &) {}

void LennardJonesSystem::setup() {
  SimulationSystem::setup();

  try {
    sampler       = boost::any_cast<size_t>(settings["sampler"]);
  } catch(const boost::bad_any_cast &e) {
    std::cerr << "Error while extracting settings in "
      << __FILE__ << ":" << __LINE__ << "\nExecption: "
      << e.what() << std::endl;
    throw e;
  }

  // set_size should have been called at this point
  // and thus n_bins should have a value
  settings["macrostates"] = n_bins;
  size_t num_unvisited_energies = 1;
  settings["num_unvisited_energies"] = num_unvisited_energies;

  //read_exact_dos();
  setup_output();

  // precalculate constants
  sigma6           = sigma*sigma*sigma*sigma*sigma*sigma;
  four_epsilon     = 4*epsilon;
  cutoff_radius    = (cutoff_radius < 0 ? size / 2 : cutoff_radius);
  cutoff_radius_sq = cutoff_radius*cutoff_radius;

  box_dimensions[0] = box_dimensions[1] = box_dimensions[2] = size;

  set_particles(num_particles);
  calculate_dos_exact_norm();
}

bool LennardJonesSystem::run() {
  switch (sampler) {
  case 0:
    mc_loop<BoltzmannSampler>();
    break;
  case 1:
    mc_loop<WangLandauSampler>();
    break;
  case 2:
    mc_loop<QualityMeasureBSampler>();
    break;
  case 3:
    mc_loop<QualityMeasureASampler>();
    break;
  case 4:
    mc_loop<TransitionMatrixSampler>();
    break;
  case 5:
    mc_loop<WangLandau1tSampler>();
    break;
  default:
    std::cerr << "Error: unknown sampler" << std::endl;
    return false;
  }

  return true;
}

/* vim: set ts=2 sw=2 sts=2 tw=0 expandtab :*/


#include <iostream>
#include <boost/lexical_cast.hpp>

#include <ast.hpp>
#include <util.hpp>
#include <exception.hpp>
#include <abstraction.hpp>
#include <simplify.hpp>
#include <ast/hash.hpp>

namespace ila
{

    // ---------------------------------------------------------------------- //
    NodeRef::NodeRef()
      : node(new Node())
    {
        throw PyILAException(PyExc_RuntimeError, "Cannot create node's directly. Use the context object.");
    }

    NodeRef::NodeRef(Node* n)
      : node(n)
    {
    }

    NodeRef::NodeRef(const nptr_t& ptr)
      : node(ptr)
    {
    }

    NodeRef::NodeRef(const NodeRef& nr)
      : node(nr.node)
    {
    }

    NodeRef::~NodeRef()
    {
        // stupid cout to help debug. need a better solution.
        // std::cout << "destroying: " << *node.get() << std::endl;
    }

    NodeRef& NodeRef::operator=(const NodeRef& other)
    {
        if(this != &other) {
            node = other.node;
        }
        return *this;
    }

    // ---------------------------------------------------------------------- //
    std::string NodeRef::getName() const
    {
        return node->name;
    }

    NodeType NodeRef::getType() const
    {
        return node->type;
    }

    bool NodeRef::equal(NodeRef* other) const
    {
        return node->equal(other->node.get());
    }

    // ---------------------------------------------------------------------- //
    NodeRef* NodeRef::complement() const
    {
        return _unOp(BoolOp::NOT, BitvectorOp::COMPLEMENT, "complement");
    }

    NodeRef* NodeRef::negate() const
    {
        return _unOp(BoolOp::INVALID, BitvectorOp::NEGATE, "negate");
    }

    NodeRef* NodeRef::logicalAnd(NodeRef* other) const
    {
        return _binOp(BoolOp::AND, BitvectorOp::AND, "and", other);
    }

    NodeRef* NodeRef::logicalAndInt(int r) const
    {
        return _binOp(BitvectorOp::AND, r);
    }

    NodeRef* NodeRef::logicalAndRInt(int l) const
    {
        return _binOpR(BitvectorOp::AND, l);
    }

    NodeRef* NodeRef::logicalOr(NodeRef* other) const
    {
        return _binOp(BoolOp::OR, BitvectorOp::OR, "or", other);
    }

    NodeRef* NodeRef::logicalOrInt(int r) const
    {
        return _binOp(BitvectorOp::OR, r);
    }

    NodeRef* NodeRef::logicalOrRInt(int l) const
    {
        return _binOpR(BitvectorOp::OR, l);
    }

    NodeRef* NodeRef::logicalXor(NodeRef* other) const
    {
        return _binOp(BoolOp::XOR, BitvectorOp::XOR, "xor", other);
    }

    NodeRef* NodeRef::logicalXorInt(int r) const
    {
        return _binOp(BitvectorOp::XOR, r);
    }

    NodeRef* NodeRef::logicalXorRInt(int l) const
    {
        return _binOpR(BitvectorOp::XOR, l);
    }

    // add //
    NodeRef* NodeRef::add(NodeRef* other) const
    {
        return _binOp(BitvectorOp::ADD, other);
    }

    NodeRef* NodeRef::addInt(int r) const
    {
        return _binOp(BitvectorOp::ADD, r);
    }

    NodeRef* NodeRef::raddInt(int r) const
    {
        return _binOpR(BitvectorOp::ADD, r);
    }

    // sub //
    NodeRef* NodeRef::sub(NodeRef* other) const
    {
        return _binOp(BitvectorOp::SUB, other);
    }

    NodeRef* NodeRef::subInt(int r) const
    {
        return _binOp(BitvectorOp::SUB, r);
    }

    NodeRef* NodeRef::rsubInt(int r) const
    {
        return _binOpR(BitvectorOp::SUB, r);
    }

    // udiv //
    NodeRef* NodeRef::udiv(NodeRef* other) const
    {
        return _binOp(BitvectorOp::UDIV, other);
    }

    NodeRef* NodeRef::udivInt(int r) const
    {
        return _binOp(BitvectorOp::UDIV, r);
    }

    NodeRef* NodeRef::rudivInt(int r) const
    {
        return _binOpR(BitvectorOp::UDIV, r);
    }

    // urem //
    NodeRef* NodeRef::urem(NodeRef* r)
    {
        return _binOp(BitvectorOp::UREM, r);
    }

    NodeRef* NodeRef::uremInt(int r)
    {
        return _binOp(BitvectorOp::UREM, r);
    }

    NodeRef* NodeRef::ruremInt(int l)
    {
        return _binOpR(BitvectorOp::UREM, l);
    }

    // shl
    NodeRef* NodeRef::shl(NodeRef* other) const
    {
        return _binOp(BitvectorOp::SHL, other);
    }

    NodeRef* NodeRef::shlInt(int r) const
    {
        return _binOp(BitvectorOp::SHL, r);
    }

    NodeRef* NodeRef::rshlInt(int r) const
    {
        return _binOpR(BitvectorOp::SHL, r);
    }

    // shr //
    NodeRef* NodeRef::lshr(NodeRef* other) const
    {
        return _binOp(BitvectorOp::LSHR, other);
    }

    NodeRef* NodeRef::lshrInt(int r) const
    {
        return _binOp(BitvectorOp::LSHR, r);
    }

    NodeRef* NodeRef::rlshrInt(int r) const
    {
        return _binOpR(BitvectorOp::LSHR, r);
    }

    // mul //
    NodeRef* NodeRef::mul(NodeRef* other) const
    {
        return _binOp(BitvectorOp::MUL, other);
    }

    NodeRef* NodeRef::mulInt(int r) const
    {
        return _binOp(BitvectorOp::MUL, r);
    }

    NodeRef* NodeRef::rmulInt(int r) const
    {
        return _binOpR(BitvectorOp::MUL, r);
    }

    // eq/neq //
    NodeRef* NodeRef::eq(NodeRef& other) const
    {
        return _cmpOp(BoolOp::EQUAL, other, false);
    }

    NodeRef* NodeRef::neq(NodeRef& other) const
    {
        return _cmpOp(BoolOp::DISTINCT, other, false);
    }

    // lt/gt //
    NodeRef* NodeRef::ult(NodeRef& other) const
    {
        return _cmpOp(BoolOp::ULT, other, true);
    }

    NodeRef* NodeRef::ugt(NodeRef& other) const
    {
        return _cmpOp(BoolOp::UGT, other, true);
    }

    // le/ge //
    NodeRef* NodeRef::ule(NodeRef& other) const
    {
        return _cmpOp(BoolOp::ULE, other, true);
    }

    NodeRef* NodeRef::uge(NodeRef& other) const
    {
        return _cmpOp(BoolOp::UGE, other, true);
    }

    // slice //
    NodeRef* NodeRef::slice(int hi, int lo) const
    {
        return _extractOp(this, hi, lo);
    }

    void NodeRef::setRefName(const std::string & refName)
    {
        this->node->setRefName( refName );
    }

    NodeRef* NodeRef::getItem(NodeRef* idx) const
    {
        if (node->type.isBitvector()) {
            return _binOp(BitvectorOp::GET_BIT, idx);
        } else if (node->type.isMem()) {
            return _binOp(BitvectorOp::READMEM, idx);
        } else {
            throw PyILAException(
                PyExc_TypeError,
                "Incorrect types for indexing operator.");
            return NULL;
        }
    }

    NodeRef* NodeRef::getItemInt(int idx) const
    {
        return _extractOp(this, idx, idx); // TODO
    }

    NodeRef* NodeRef::eqInt(int r) const
    {
        return _cmpOp(BoolOp::EQUAL, r);
    }

    NodeRef* NodeRef::neqInt(int r) const
    {
        return _cmpOp(BoolOp::DISTINCT, r);
    }

    NodeRef* NodeRef::ultInt(int r) const
    {
        return _cmpOp(BoolOp::ULT, r);
    }

    NodeRef* NodeRef::ugtInt(int r) const
    {
        return _cmpOp(BoolOp::UGT, r);
    }

    NodeRef* NodeRef::uleInt(int r) const
    {
        return _cmpOp(BoolOp::ULE, r);
    }

    NodeRef* NodeRef::ugeInt(int r) const
    {
        return _cmpOp(BoolOp::UGE, r);
    }

    py::object NodeRef::value() const
    {
        return node->getValue();
    }

    // ---------------------------------------------------------------------- //
    // static functions.

    NodeRef* NodeRef::load(NodeRef* mem, NodeRef* addr)
    {
        const NodeType& mt = mem->node->type;
        const NodeType& at = addr->node->type;
        if (!mt.isMem()                     ||
            !at.isBitvector(mt.addrWidth))
        {
            throw PyILAException(PyExc_TypeError,
                "Type error in arguments.");
            return NULL;
        }
        return new NodeRef(new BitvectorOp(
            BitvectorOp::READMEM,
            mem->node, addr->node));
    }

    NodeRef* NodeRef::loadblock(NodeRef* mem, NodeRef* addr, int chunks)
    {
        const NodeType& mt = mem->node->type;
        const NodeType& at = addr->node->type;
        if (!mt.isMem()                     ||
            !at.isBitvector(mt.addrWidth))
        {
            throw PyILAException(PyExc_TypeError,
                "Type error in arguments.");
            return NULL;
        }
        if (chunks <= 0) {
            throw PyILAException(PyExc_ValueError,
                "Invalid number of blocks.");
            return NULL;
        }

        return new NodeRef(new BitvectorOp(
            BitvectorOp::READMEMBLOCK,
            mem->node, addr->node, chunks,
            LITTLE_E));
    }

    NodeRef* NodeRef::loadblockB(NodeRef* mem, NodeRef* addr, int chunks)
    {
        const NodeType& mt = mem->node->type;
        const NodeType& at = addr->node->type;
        if (!mt.isMem()                     ||
            !at.isBitvector(mt.addrWidth))
        {
            throw PyILAException(PyExc_TypeError,
                "Type error in arguments.");
            return NULL;
        }
        if (chunks <= 0) {
            throw PyILAException(PyExc_ValueError,
                "Invalid number of blocks.");
            return NULL;
        }

        return new NodeRef(new BitvectorOp(
            BitvectorOp::READMEMBLOCK,
            mem->node, addr->node, chunks,
            BIG_E));
    }

    NodeRef* NodeRef::store(NodeRef* mem, NodeRef* addr, NodeRef* data)
    {
        const NodeType& mt = mem->node->type;
        const NodeType& at = addr->node->type;
        const NodeType& dt = data->node->type;
        if (!mt.isMem()                     ||
            !at.isBitvector(mt.addrWidth)   ||
            !dt.isBitvector(mt.dataWidth))
        {
            throw PyILAException(PyExc_TypeError,
                "Type error in arguments.");
            return NULL;
        }
        return new NodeRef(new MemOp(
            MemOp::STORE, mem->node, addr->node, data->node));
    }

    NodeRef* NodeRef::storeblock(NodeRef* mem, NodeRef* addr, NodeRef* data)
    {
        const NodeType& mt = mem->node->type;
        const NodeType& at = addr->node->type;
        const NodeType& dt = addr->node->type;
        if (!mt.isMem()                     ||
            !at.isBitvector(mt.addrWidth)   ||
            !dt.isBitvector()               ||
            dt.bitWidth % mt.dataWidth != 0)
        {
            throw PyILAException(PyExc_TypeError,
                "Type error in arguments.");
            return NULL;
        }
        return new NodeRef(new MemOp(
            MemOp::STOREBLOCK,
            mem->node, addr->node, data->node,
            LITTLE_E));
    }

    NodeRef* NodeRef::storeblockB(NodeRef* mem, NodeRef* addr, NodeRef* data)
    {
        const NodeType& mt = mem->node->type;
        const NodeType& at = addr->node->type;
        const NodeType& dt = addr->node->type;
        if (!mt.isMem()                     ||
            !at.isBitvector(mt.addrWidth)   ||
            !dt.isBitvector()               ||
            dt.bitWidth % mt.dataWidth != 0)
        {
            throw PyILAException(PyExc_TypeError,
                "Type error in arguments.");
            return NULL;
        }
        return new NodeRef(new MemOp(
            MemOp::STOREBLOCK,
            mem->node, addr->node, data->node,
            BIG_E));
    }

    NodeRef* NodeRef::logicalXnor(NodeRef* l, NodeRef* r)
    {
        return _binOp(BoolOp::XNOR, BitvectorOp::XNOR, "xnor", l, r);
    }

    NodeRef* NodeRef::logicalNand(NodeRef* l, NodeRef* r)
    {
        return _binOp(BoolOp::NAND, BitvectorOp::NAND, "nand", l, r);
    }

    NodeRef* NodeRef::logicalNor(NodeRef* l, NodeRef* r)
    {
        return _binOp(BoolOp::NOR, BitvectorOp::NOR, "nor", l, r);
    }

    // sdiv //
    NodeRef* NodeRef::sdiv(NodeRef* l, NodeRef* r)
    {
        return _binOp(BitvectorOp::SDIV, l, r);
    }

    NodeRef* NodeRef::sdivInt(NodeRef* l, int r)
    {
        return _binOp(BitvectorOp::SDIV, l, r);
    }

    NodeRef* NodeRef::rsdivInt(int l, NodeRef* r)
    {
        return _binOpR(BitvectorOp::SDIV, l, r);
    }

    // smod //
    NodeRef* NodeRef::smod(NodeRef* l, NodeRef* r)
    {
        return _binOp(BitvectorOp::SMOD, l, r);
    }

    NodeRef* NodeRef::smodInt(NodeRef* l, int r)
    {
        return _binOp(BitvectorOp::SMOD, l, r);
    }

    NodeRef* NodeRef::rsmodInt(int l, NodeRef* r)
    {
        return _binOpR(BitvectorOp::SMOD, l, r);
    }

    // srem //
    NodeRef* NodeRef::srem(NodeRef* l, NodeRef* r)
    {
        return _binOp(BitvectorOp::SREM, l, r);
    }

    NodeRef* NodeRef::sremInt(NodeRef* l, int r)
    {
        return _binOp(BitvectorOp::SREM, l, r);
    }

    NodeRef* NodeRef::rsremInt(int l, NodeRef* r)
    {
        return _binOpR(BitvectorOp::SREM, l, r);
    }

    NodeRef* NodeRef::ashr(NodeRef* l, NodeRef* r)
    {
        return _binOp(BitvectorOp::ASHR, l, r);
    }

    NodeRef* NodeRef::ashrInt(NodeRef* l, int r)
    {
        return _binOp(BitvectorOp::ASHR, l, r);
    }

    NodeRef* NodeRef::rashrInt(int l, NodeRef* r)
    {
        return _binOp(BitvectorOp::ASHR, r, l);
    }

    NodeRef* NodeRef::concat(NodeRef* hi, NodeRef* lo)
    {
        return _binOp(BitvectorOp::CONCAT, hi, lo);
    }

    NodeRef* NodeRef::concatList(const py::list& l)
    {
        nptr_vec_t args;
        // number of arguments.
        if (py::len(l) < 2) {
            throw PyILAException(
                PyExc_RuntimeError, "Must have two arguments.");
            return NULL;
        }
        // must all be nodes.
        for (unsigned i=0; i != py::len(l); i++) {
            py::extract<NodeRef&> ni(l[i]);
            if (ni.check()) {
                args.push_back(ni().node);
            } else {
                throw PyILAException(
                    PyExc_TypeError, "Argument to concat must be a node.");
                return NULL;
            }
        }
        // must all be bitvectors.
        for (unsigned i=0; i !=args.size(); i++) {
            if (!args[i]->type.isBitvector()) {
                throw PyILAException(
                    PyExc_TypeError, "Argument to concat must be a bitvector.");
                return NULL;
            }
        }

        // now finally create the expression.
        ILA_ASSERT(args.size() >= 2, "Must have two arguments.");
        nptr_t expr;
        for (int i=args.size()-2; i >= 0; i--) {
            const nptr_t& b1 = args[i];
            const nptr_t& b2 = args[i+1];
            if (i == (int)args.size()-2) {
                nptr_t arg(new BitvectorOp(
                    BitvectorOp::CONCAT,
                    b1, b2));
                expr = arg;
            } else {
                nptr_t arg(new BitvectorOp(
                    BitvectorOp::CONCAT,
                    b1, expr));
                expr = arg;
            }
        }
        return new NodeRef(expr);
    }

    NodeRef* NodeRef::lrotate(NodeRef* obj, int par)
    {
        return _binOp(BitvectorOp::LROTATE, obj, par);
    }

    NodeRef* NodeRef::rrotate(NodeRef* obj, int par)
    {
        return _binOp(BitvectorOp::RROTATE, obj, par);
    }

    NodeRef* NodeRef::slt(NodeRef& l, NodeRef& r)
    {
        return _cmpOp(BoolOp::SLT, l, r, true);
    }

    NodeRef* NodeRef::sgt(NodeRef& l, NodeRef& r)
    {
        return _cmpOp(BoolOp::SGT, l, r, true);
    }

    NodeRef* NodeRef::sle(NodeRef& l, NodeRef& r)
    {
        return _cmpOp(BoolOp::SLE, l, r, true);
    }

    NodeRef* NodeRef::sge(NodeRef& l, NodeRef& r)
    {
        return _cmpOp(BoolOp::SGE, l, r, true);
    }

    NodeRef* NodeRef::sltInt(NodeRef& l, int r)
    {
        return _cmpOp(BoolOp::SLT, l, r);
    }

    NodeRef* NodeRef::sgtInt(NodeRef& l, int r)
    {
        return _cmpOp(BoolOp::SGT, l, r);
    }

    NodeRef* NodeRef::sleInt(NodeRef& l, int r)
    {
        return _cmpOp(BoolOp::SLE, l, r);
    }

    NodeRef* NodeRef::sgeInt(NodeRef& l, int r)
    {
        return _cmpOp(BoolOp::SGE, l, r);
    }

    NodeRef* NodeRef::zero_extend(NodeRef* obj, int outWidth)
    {
        return _binOp(BitvectorOp::Z_EXT, obj, outWidth);
    }

    NodeRef* NodeRef::sign_extend(NodeRef* obj, int outWidth)
    {
        return _binOp(BitvectorOp::S_EXT, obj, outWidth);
    }

    NodeRef* NodeRef::nonzero(NodeRef* obj)
    {
        return obj->neqInt(0);
    }

    NodeRef* NodeRef::imply(NodeRef& p, NodeRef& q)
    {
        return _binOp(BoolOp::IMPLY, p, q);
    }

    NodeRef* NodeRef::ite(NodeRef& cond, NodeRef& trueExp, NodeRef& falseExp)
    {
        return _triOp(BoolOp::IF, BitvectorOp::IF, MemOp::ITE, cond, trueExp, falseExp);
    }

    NodeRef* NodeRef::appfunc0(NodeRef* fun)
    {
        nptr_vec_t args = {fun->node};
        return _naryOp(BitvectorOp::APPLY_FUNC, args);
    }

    NodeRef* NodeRef::appfunc1(NodeRef* fun, NodeRef* arg)
    {
        nptr_vec_t args = {fun->node, arg->node};
        return _naryOp(BitvectorOp::APPLY_FUNC, args);
    }

    NodeRef* NodeRef::appfunc2(NodeRef* fun, NodeRef* arg0, NodeRef* arg1)
    {
        nptr_vec_t args = {fun->node, arg0->node, arg1->node};
        return _naryOp(BitvectorOp::APPLY_FUNC, args);
    }

    NodeRef* NodeRef::appfuncL(NodeRef* fun, const py::list& l)
    {
        nptr_vec_t args;
        args.push_back(fun->node);
        for (unsigned i=0; i != py::len(l); i++) {
            py::extract<NodeRef&> ni(l[i]);
            if (ni.check()) {
                args.push_back(ni().node);
            } else {
                throw PyILAException(
                    PyExc_TypeError,
                    "Argument to apply must be a node.");
            }
        }
        return _naryOp(BitvectorOp::APPLY_FUNC, args);
    }

    NodeRef* NodeRef::choice2(const std::string& name, NodeRef* e1, NodeRef* e2)
    {
        nptr_vec_t args = { e1->node, e2->node };
        return _choice(name, args);
    }

    NodeRef* NodeRef::choice3(
        const std::string& name, NodeRef* e1, NodeRef* e2, NodeRef* e3)
    {
        nptr_vec_t args = { e1->node, e2->node, e3->node };
        return _choice(name, args);
    }

    NodeRef* NodeRef::choice4(
        const std::string& name, NodeRef* e1, NodeRef* e2, NodeRef* e3, NodeRef* e4)
    {
        nptr_vec_t args = { e1->node, e2->node, e3->node, e4->node };
        return _choice(name, args);
    }

    NodeRef* NodeRef::choice5(const std::string& name,
                              NodeRef* e1, NodeRef* e2,
                              NodeRef* e3, NodeRef* e4, NodeRef* e5)
    {
        nptr_vec_t args =
            { e1->node, e2->node, e3->node, e4->node, e5->node };
        return _choice(name, args);
    }

    NodeRef* NodeRef::choice6(const std::string& name,
                              NodeRef* e1, NodeRef* e2, NodeRef* e3,
                              NodeRef* e4, NodeRef* e5, NodeRef* e6)
    {
        nptr_vec_t args =
            { e1->node, e2->node, e3->node, e4->node, e5->node, e6->node };
        return _choice(name, args);
    }

    NodeRef* NodeRef::choiceL(const std::string& name, const py::list& l)
    {
        nptr_vec_t args;
        if (py::len(l) < 2) {
            throw PyILAException(
                PyExc_RuntimeError,
                "Must have at least two choices.");
            return NULL;
        }

        for (unsigned i=0; i != py::len(l); i++) {
            py::extract<NodeRef&> ni(l[i]);
            if (ni.check()) {
                args.push_back(ni().node);
            } else {
                throw PyILAException(
                    PyExc_TypeError,
                    "Argument to choice must be a node.");
            }
        }
        return _choice(name, args);
    }

    NodeRef* NodeRef::inRange(const std::string& name,
                              NodeRef* lo, NodeRef* hi)
    {
        return new NodeRef(new BVInRange(name, lo->node, hi->node));
    }

    NodeRef* NodeRef::readSlice(const std::string& name, NodeRef* bv, int w)
    {
        if (!bv->node->type.isBitvector() || bv->node->type.bitWidth <= w || w <= 0) {
            throw PyILAException(
                PyExc_TypeError,
                "Argument to readslice must be a bitvector of width greater than result width.");
            return NULL;
        }
        return new NodeRef(
            ReadSlice::createReadSlice(name, bv->node, w, 1));
    }

    NodeRef* NodeRef::readChunk(const std::string& name, NodeRef* bv, int w)
    {
        if (!bv->node->type.isBitvector() || bv->node->type.bitWidth <= w || w <= 0) {
            throw PyILAException(
                PyExc_TypeError,
                "Argument to readchunk must be a bitvector of width greater than result width.");
            return NULL;
        }
        if (!bv->node->type.bitWidth % w != 0) {
            throw PyILAException(
                PyExc_TypeError,
                "Width of argument to readchunk must multiple of chunk size");
            return NULL;
        }

        return new NodeRef(
            ReadSlice::createReadSlice(name, bv->node, w, w));
    }

    NodeRef* NodeRef::writeSlice(const std::string& name, NodeRef* bv, NodeRef* wr)
    {
        if (!bv->node->type.isBitvector() || !wr->node->type.isBitvector() ||
            bv->node->type.bitWidth <= wr->node->type.bitWidth) {

            throw PyILAException(
                PyExc_TypeError,
                "Incorrect types to writeslice arguments.");
            return NULL;
        }
        return new NodeRef(
            WriteSlice::createWriteSlice(name, bv->node, wr->node, 1));
    }

    NodeRef* NodeRef::writeChunk(const std::string& name, NodeRef* bv, NodeRef* wr)
    {
        if (!bv->node->type.isBitvector() || !wr->node->type.isBitvector() ||
            bv->node->type.bitWidth <= wr->node->type.bitWidth) {

            throw PyILAException(
                PyExc_TypeError,
                "Incorrect types to writeslice arguments.");
            return NULL;
        }

        if (!bv->node->type.bitWidth % wr->node->type.bitWidth != 0) {
            throw PyILAException(
                PyExc_TypeError,
                "Bitwidth of bitvector not a multiple of data bitwidth.");
        }
        return new NodeRef(
            WriteSlice::createWriteSlice(name,
                bv->node, wr->node, wr->node->type.bitWidth));
    }

    NodeRef* NodeRef::simplify(NodeRef& n_assump, NodeRef& n_exp)
    {
        const nptr_t& assump = n_assump.node;
        const nptr_t& exp = n_exp.node;

        ITESimplifier ite_simp(assump);
        nptr_t simp_exp = ite_simp.simplify(exp.get());

        return new NodeRef(simp_exp);
    }

    size_t NodeRef::hash() const
    {
        return hash_value(*this->node.get());
    }

    // ---------------------------------------------------------------------- //
    NodeRef* NodeRef::_unOp(
        BoolOp::Op opBool, BitvectorOp::Op opBv, const char* opName) const
    {
        if (opBv != BitvectorOp::INVALID && node->type.isBitvector()) {
            return new NodeRef(new BitvectorOp(opBv, node));
        } else if (opBool != BoolOp::INVALID && node->type.isBool()) {
            return new NodeRef(new BoolOp(opBool, node));
        } else {
            throw PyILAException(PyExc_TypeError, std::string("Incorrect type for ") +
                                 opName);
            return NULL;
        }
    }

    NodeRef* NodeRef::_binOp(
        BoolOp::Op boolOp,
        BitvectorOp::Op bvOp,
        const char* opName,
        NodeRef* other) const
    {
        if (boolOp != BoolOp::INVALID && node->type.isBool()) {
            return new NodeRef(new BoolOp(
                        boolOp, node, other->node));
        } else if (bvOp != BitvectorOp::INVALID && node->type.isBitvector()) {
            return new NodeRef(new BitvectorOp(
                        bvOp, node, other->node));
        } else {
            throw PyILAException(PyExc_TypeError,
                                 std::string("Incorrect type for ")
                                 + opName);
        }
    }

    NodeRef* NodeRef::_binOp(BitvectorOp::Op op, NodeRef* other) const
    {
        if (node->type.isBitvector()) {
            return new NodeRef(new BitvectorOp(
                        op, node, other->node));
        } else if (node->type.isMem() && other->node->type.isBitvector()) {
            return new NodeRef(new BitvectorOp(
                        op, node, other->node));
        } else {
            throw PyILAException(PyExc_TypeError,
                                 "Incorrect type for " +
                                 BitvectorOp::operatorNames[op]);
        }
    }

    NodeRef* NodeRef::_binOp(BitvectorOp::Op op, int r) const
    {
        if (node->type.isBitvector()) {
            nptr_t node_r(
                new BitvectorConst(r, node->type.bitWidth));

            return new NodeRef(new BitvectorOp(
                        op, node, node_r));
        } else {
            throw PyILAException(PyExc_TypeError,
                                 "Incorrect type for " +
                                 BitvectorOp::operatorNames[op]);
        }
    }

    NodeRef* NodeRef::_binOpR(BitvectorOp::Op op, int r) const
    {
        if (node->type.isBitvector()) {
            nptr_t node_l(
                new BitvectorConst(r, node->type.bitWidth));

            return new NodeRef(new BitvectorOp(
                        op, node_l, node));
        } else {
            throw PyILAException(PyExc_TypeError,
                                 "Incorrect type for " +
                                 BitvectorOp::operatorNames[op]);
        }
    }

    // bvtype = true means this operator can only be applied
    // on bitvector type objects.
    NodeRef* NodeRef::_cmpOp(BoolOp::Op op,
                             NodeRef& other,
                             bool bvtype) const
    {
        if (node->type == other.node->type &&
                   (!bvtype || node->type.isBitvector())) {
            return new NodeRef(
                        new BoolOp(op, node, other.node));
        } else {
            throw PyILAException(PyExc_TypeError,
                                  "Incorrect type for " +
                                  BoolOp::operatorNames[op]);
            return NULL;
        }
    }

    NodeRef* NodeRef::_cmpOp(BoolOp::Op op, int r) const
    {
        if (node->type.isBitvector()) {
            nptr_t node_r(
                new BitvectorConst(r, node->type.bitWidth));
            return new NodeRef(new BoolOp(op, node, node_r));
        } else {
            throw PyILAException(PyExc_TypeError,
                                  "Incorrect type for " +
                                  BoolOp::operatorNames[op]);
            return NULL;
        }
        return NULL;
    }

    // ---------------------------------------------------------------------- //
    // static methods

    // bvtype = true means this operator can only be applied
    // on bitvector type objects.
    NodeRef* NodeRef::_cmpOp(BoolOp::Op op,
                             NodeRef& l, NodeRef& r,
                             bool bvonly)
    {
        if (l.node->type == r.node->type &&
                   (!bvonly || l.node->type.isBitvector())) {
            return new NodeRef(
                        new BoolOp(op, l.node, r.node));
        } else {
            throw PyILAException(PyExc_TypeError,
                                  "Incorrect type for " +
                                  BoolOp::operatorNames[op]);
            return NULL;
        }
    }
    // Only bitvector can compare with int
    NodeRef* NodeRef::_cmpOp(BoolOp::Op op, NodeRef& l, int r)
    {
        if (l.node->type.isBitvector()) {
            nptr_t node_r(
                new BitvectorConst(r, l.node->type.bitWidth));
            return new NodeRef(new BoolOp(op, l.node, node_r));
        } else {
            throw PyILAException(PyExc_TypeError,
                                  "Incorrect type for " +
                                  BoolOp::operatorNames[op]);
            return NULL;
        }
        return NULL;
    }

    NodeRef* NodeRef::_binOp(
        BoolOp::Op boolOp,
        BitvectorOp::Op bvOp,
        const char* opName,
        NodeRef* l,
        NodeRef* r)
    {
        if (boolOp != BoolOp::INVALID && l->node->type.isBool()) {
            return new NodeRef(new BoolOp(
                        boolOp, l->node, r->node));
        } else if (bvOp != BitvectorOp::INVALID && l->node->type.isBitvector()) {
            return new NodeRef(new BitvectorOp(
                        bvOp, l->node, r->node));
        } else {
            throw PyILAException(PyExc_TypeError,
                                 std::string("Incorrect type for ")
                                 + opName);
            return NULL;
        }
    }

    NodeRef* NodeRef::_binOp(BoolOp::Op op, NodeRef& l, NodeRef& r)
    {
        if (l.node->type.isBool() && r.node->type.isBool()) {
            return new NodeRef(new BoolOp(
                        op, l.node, r.node));
        } else {
            throw PyILAException(PyExc_TypeError,
                                 "Incorrect type for " +
                                 BoolOp::operatorNames[op]);
            return NULL;
        }
    }

    NodeRef* NodeRef::_binOp(BitvectorOp::Op op, NodeRef* l, NodeRef* r)
    {
        if (l->node->type.isBitvector()) {
            return new NodeRef(new BitvectorOp(op, l->node, r->node));
        } else {
            throw PyILAException(PyExc_TypeError,
                                 "Incorrect type for " +
                                 BitvectorOp::operatorNames[op]);
            return NULL;
        }
    }

    NodeRef* NodeRef::_binOp(BitvectorOp::Op op, NodeRef* l, int r)
    {
        if (l->node->type.isBitvector()) {
            if (op >= BitvectorOp::Op::LROTATE &&
                op <= BitvectorOp::Op::S_EXT)
            {
                return new NodeRef(new BitvectorOp(op, l->node, r));
            } else {
                nptr_t node_r(new BitvectorConst(r, l->node->type.bitWidth));
                return new NodeRef(new BitvectorOp(op, l->node, node_r));
            }
        } else {
            throw PyILAException(PyExc_TypeError,
                                 "Incorrect type for " +
                                 BitvectorOp::operatorNames[op]);
            return NULL;
        }
    }

    NodeRef* NodeRef::_binOpR(BitvectorOp::Op op, int l, NodeRef* r)
    {
        if (r->node->type.isBitvector()) {
            nptr_t node_l(new BitvectorConst(l, r->node->type.bitWidth));

            return new NodeRef(new BitvectorOp(op, node_l, r->node));
        } else {
            throw PyILAException(PyExc_TypeError,
                                 "Incorrect type for " +
                                 BitvectorOp::operatorNames[op]);
            return NULL;
        }
    }

    NodeRef* NodeRef::_triOp(BoolOp::Op boolOp, BitvectorOp::Op bvOp, MemOp::Op memOp,
                             NodeRef& cond, NodeRef& trueExp, NodeRef& falseExp)
    {
        if (trueExp.node->type != falseExp.node->type ||
            !cond.node->type.isBool())
        {
            throw PyILAException(PyExc_TypeError, "Incorrect type for ITE");
            return NULL;
        } else {
            nptr_vec_t args_;
            args_.push_back(cond.node);
            args_.push_back(trueExp.node);
            args_.push_back(falseExp.node);
            if (trueExp.node->type.isBool() && boolOp != BoolOp::INVALID) {
                return new NodeRef(
                    new BoolOp(boolOp, args_));
            } else if (trueExp.node->type.isBitvector() && bvOp != BitvectorOp::INVALID) {
                return new NodeRef(
                    new BitvectorOp(bvOp, args_));
            } else if (trueExp.node->type.isMem() && memOp == MemOp::ITE) {
                return new NodeRef(
                    new MemOp(memOp, cond.node, trueExp.node, falseExp.node));
            } else {
                throw PyILAException(PyExc_TypeError, "Incorrect type for ITE");
                return NULL;
            }
        }
        return NULL;
    }

    NodeRef* NodeRef::_naryOp(BitvectorOp::Op bvOp, nptr_vec_t& args)
    {
        if (args.size() < 1) {
            throw PyILAException(PyExc_TypeError, "No function specified.");
            return NULL;
        }
        if (!args[0]->type.isFunc()) {
            throw PyILAException(PyExc_TypeError,
                    "Incorrect type for apply function.");
            return NULL;
        } else {
            return new NodeRef(new BitvectorOp(BitvectorOp::APPLY_FUNC, args));
        }
        return NULL;
    }

    NodeRef* NodeRef::_extractOp(const NodeRef* obj, int beg, int end)
    {
        if (!obj->node->type.isBitvector())
        {
            throw PyILAException(PyExc_TypeError, "Incorrect type for extract.");
            return NULL;
        } else {
            return new NodeRef(
                new BitvectorOp(BitvectorOp::EXTRACT, obj->node, beg, end));
        }
    }

    NodeRef* NodeRef::_choice(const std::string& name,
                              const nptr_vec_t& args)
    {

        NodeType t = Choice::getChoiceType(args);
        if (!t) {
            throw PyILAException(PyExc_TypeError,
                "Type mismatch in choice arguments.");
            return NULL;
        } else {
            if (t.isBool()) {
                return new NodeRef(new BoolChoice(name, args));
            } else if (t.isBitvector()) {
                return new NodeRef(new BitvectorChoice(name, args));
            } else if (t.isMem()) {
                return new NodeRef(new MemChoice(name, args));
            } else {
                throw PyILAException(PyExc_ValueError,
                    "Unable to create choice of specified type.");
                return NULL;
            }
        }
    }

    // ---------------------------------------------------------------------- //
    std::ostream& operator<<(std::ostream& out, const NodeRef& n)
    {
        return (out << *n.node.get());
    }

    // ---------------------------------------------------------------------- //
}


// GMTL is (C) Copyright 2001-2010 by Allen Bierbaum
// Distributed under the GNU Lesser General Public License 2.1 with an
// addendum covering inlined code. (See accompanying files LICENSE and
// LICENSE.addendum or http://www.gnu.org/copyleft/lesser.txt)

// Includes ====================================================================
#include <boost/python.hpp>
#include <gmtl-CoordOps.h>

// Using =======================================================================
using namespace boost::python;

// Declarations ================================================================
namespace  {

BOOST_PYTHON_FUNCTION_OVERLOADS(isEqual_overloads_2_3, gmtl::isEqual, 2, 3)

}// namespace

// Module ======================================================================
void _Export_gmtl_CoordOps_h()
{
    def("isEqual", (bool (*)(const gmtl::Coord<gmtl::Vec<double, 3>,gmtl::EulerAngle<double, gmtl::XYZ> > &, const gmtl::Coord<gmtl::Vec<double, 3>,gmtl::EulerAngle<double, gmtl::XYZ> > &, double))&gmtl::isEqual, isEqual_overloads_2_3());
    def("isEqual", (bool (*)(const gmtl::Coord<gmtl::Vec<double, 4>,gmtl::AxisAngle<double> > &, const gmtl::Coord<gmtl::Vec<double, 4>,gmtl::AxisAngle<double> > &, double))&gmtl::isEqual, isEqual_overloads_2_3());
    def("isEqual", (bool (*)(const gmtl::Coord<gmtl::Vec<float, 4>,gmtl::AxisAngle<float> > &, const gmtl::Coord<gmtl::Vec<float, 4>,gmtl::AxisAngle<float> > &, float))&gmtl::isEqual, isEqual_overloads_2_3());
    def("isEqual", (bool (*)(const gmtl::Coord<gmtl::Vec<double, 4>,gmtl::Quat<double> > &, const gmtl::Coord<gmtl::Vec<double, 4>,gmtl::Quat<double> > &, double))&gmtl::isEqual, isEqual_overloads_2_3());
    def("isEqual", (bool (*)(const gmtl::Coord<gmtl::Vec<float, 4>,gmtl::Quat<float> > &, const gmtl::Coord<gmtl::Vec<float, 4>,gmtl::Quat<float> > &, float))&gmtl::isEqual, isEqual_overloads_2_3());
    def("isEqual", (bool (*)(const gmtl::Coord<gmtl::Vec<float, 3>,gmtl::EulerAngle<float, gmtl::ZYX> > &, const gmtl::Coord<gmtl::Vec<float, 3>,gmtl::EulerAngle<float, gmtl::ZYX> > &, float))&gmtl::isEqual, isEqual_overloads_2_3());
    def("isEqual", (bool (*)(const gmtl::Coord<gmtl::Vec<double, 3>,gmtl::EulerAngle<double, gmtl::ZXY> > &, const gmtl::Coord<gmtl::Vec<double, 3>,gmtl::EulerAngle<double, gmtl::ZXY> > &, double))&gmtl::isEqual, isEqual_overloads_2_3());
    def("isEqual", (bool (*)(const gmtl::Coord<gmtl::Vec<double, 3>,gmtl::EulerAngle<double, gmtl::ZYX> > &, const gmtl::Coord<gmtl::Vec<double, 3>,gmtl::EulerAngle<double, gmtl::ZYX> > &, double))&gmtl::isEqual, isEqual_overloads_2_3());
    def("isEqual", (bool (*)(const gmtl::Coord<gmtl::Vec<double, 3>,gmtl::Quat<double> > &, const gmtl::Coord<gmtl::Vec<double, 3>,gmtl::Quat<double> > &, double))&gmtl::isEqual, isEqual_overloads_2_3());
    def("isEqual", (bool (*)(const gmtl::Coord<gmtl::Vec<float, 3>,gmtl::Quat<float> > &, const gmtl::Coord<gmtl::Vec<float, 3>,gmtl::Quat<float> > &, float))&gmtl::isEqual, isEqual_overloads_2_3());
    def("isEqual", (bool (*)(const gmtl::Coord<gmtl::Vec<double, 4>,gmtl::EulerAngle<double, gmtl::ZXY> > &, const gmtl::Coord<gmtl::Vec<double, 4>,gmtl::EulerAngle<double, gmtl::ZXY> > &, double))&gmtl::isEqual, isEqual_overloads_2_3());
    def("isEqual", (bool (*)(const gmtl::Coord<gmtl::Vec<double, 4>,gmtl::EulerAngle<double, gmtl::ZYX> > &, const gmtl::Coord<gmtl::Vec<double, 4>,gmtl::EulerAngle<double, gmtl::ZYX> > &, double))&gmtl::isEqual, isEqual_overloads_2_3());
    def("isEqual", (bool (*)(const gmtl::Coord<gmtl::Vec<double, 4>,gmtl::EulerAngle<double, gmtl::XYZ> > &, const gmtl::Coord<gmtl::Vec<double, 4>,gmtl::EulerAngle<double, gmtl::XYZ> > &, double))&gmtl::isEqual, isEqual_overloads_2_3());
    def("isEqual", (bool (*)(const gmtl::Coord<gmtl::Vec<float, 4>,gmtl::EulerAngle<float, gmtl::ZXY> > &, const gmtl::Coord<gmtl::Vec<float, 4>,gmtl::EulerAngle<float, gmtl::ZXY> > &, float))&gmtl::isEqual, isEqual_overloads_2_3());
    def("isEqual", (bool (*)(const gmtl::Coord<gmtl::Vec<float, 4>,gmtl::EulerAngle<float, gmtl::ZYX> > &, const gmtl::Coord<gmtl::Vec<float, 4>,gmtl::EulerAngle<float, gmtl::ZYX> > &, float))&gmtl::isEqual, isEqual_overloads_2_3());
    def("isEqual", (bool (*)(const gmtl::Coord<gmtl::Vec<double, 3>,gmtl::AxisAngle<double> > &, const gmtl::Coord<gmtl::Vec<double, 3>,gmtl::AxisAngle<double> > &, double))&gmtl::isEqual, isEqual_overloads_2_3());
    def("isEqual", (bool (*)(const gmtl::Coord<gmtl::Vec<float, 3>,gmtl::AxisAngle<float> > &, const gmtl::Coord<gmtl::Vec<float, 3>,gmtl::AxisAngle<float> > &, float))&gmtl::isEqual, isEqual_overloads_2_3());
    def("isEqual", (bool (*)(const gmtl::Coord<gmtl::Vec<float, 3>,gmtl::EulerAngle<float, gmtl::XYZ> > &, const gmtl::Coord<gmtl::Vec<float, 3>,gmtl::EulerAngle<float, gmtl::XYZ> > &, float))&gmtl::isEqual, isEqual_overloads_2_3());
    def("isEqual", (bool (*)(const gmtl::Coord<gmtl::Vec<float, 3>,gmtl::EulerAngle<float, gmtl::ZXY> > &, const gmtl::Coord<gmtl::Vec<float, 3>,gmtl::EulerAngle<float, gmtl::ZXY> > &, float))&gmtl::isEqual, isEqual_overloads_2_3());
    def("isEqual", (bool (*)(const gmtl::Coord<gmtl::Vec<float, 4>,gmtl::EulerAngle<float, gmtl::XYZ> > &, const gmtl::Coord<gmtl::Vec<float, 4>,gmtl::EulerAngle<float, gmtl::XYZ> > &, float))&gmtl::isEqual, isEqual_overloads_2_3());
}


/*
 * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale,
 * Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
 * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
*/

#ifndef KINDR_ROTATIONS_EIGEN_ANGLEAXIS_HPP_
#define KINDR_ROTATIONS_EIGEN_ANGLEAXIS_HPP_

#include <cmath>

#include <Eigen/Geometry>

#include "kindr/common/common.hpp"
#include "kindr/common/assert_macros_eigen.hpp"
#include "kindr/rotations/RotationBase.hpp"
#include "kindr/rotations/eigen/RotationEigenFunctions.hpp"

namespace kindr {
namespace rotations {
namespace eigen_impl {

/*! \class AngleAxis
 * \brief Implementation of an angle axis rotation based on Eigen::AngleAxis
 *
 *  The following two typedefs are provided for convenience:
 *   - \ref eigen_impl::AngleAxisAD "AngleAxisAD" for active rotation and primitive type double
 *   - \ref eigen_impl::AngleAxisAF "AngleAxisAF" for active rotation and primitive type float
 *   - \ref eigen_impl::AngleAxisPD "AngleAxisPD" for passive rotation and primitive type double
 *   - \ref eigen_impl::AngleAxisPF "AngleAxisPF" for passive rotation and primitive type float
 *
 *  \tparam PrimType_ the primitive type of the data (double or float)
 *  \tparam Usage_ the rotation usage which is either active or passive
 *
 *  \ingroup rotations
 */
template<typename PrimType_, enum RotationUsage Usage_>
class AngleAxis : public AngleAxisBase<AngleAxis<PrimType_, Usage_>, Usage_> {
 private:
  /*! \brief The base type.
   */
  typedef Eigen::AngleAxis<PrimType_> Base;

  /*! Data container
   */
  Base angleAxis_;

 public:
  /*! \brief The implementation type.
   *  The implementation type is always an Eigen object.
   */
  typedef Base Implementation;
  /*! \brief The primitive type.
   *  Float/Double
   */
  typedef PrimType_ Scalar;
  /*! \brief The axis type is a 3D vector.
   */
  typedef Eigen::Matrix<PrimType_, 3, 1> Vector3;

  /*! \brief All four parameters stored in a vector [angle; axis]
   */
  typedef Eigen::Matrix<PrimType_, 4, 1> Vector4;

  /*! \brief Default constructor using identity rotation.
   */
  AngleAxis()
    : angleAxis_(Base::Identity()) {
  }

  /*! \brief Constructor using four scalars.
   *  In debug mode, an assertion is thrown if the rotation vector has not unit length.
   *  \param angle     rotation angle
   *  \param v1      first entry of the rotation axis vector
   *  \param v2      second entry of the rotation axis vector
   *  \param v3      third entry of the rotation axis vector
   */
  AngleAxis(Scalar angle, Scalar v1, Scalar v2, Scalar v3)
    : angleAxis_(angle,Vector3(v1,v2,v3)) {
    KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, this->axis().norm(), static_cast<Scalar>(1), static_cast<Scalar>(1e-4), "Input rotation axis has not unit length.");
  }

  /*! \brief Constructor using angle and axis.
   * In debug mode, an assertion is thrown if the rotation vector has not unit length.
   * \param angle   rotation angle
   * \param axis     rotation vector with unit length (Eigen vector)
   */
  AngleAxis(Scalar angle, const Vector3& axis)
    : angleAxis_(angle,axis) {
    KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, this->axis().norm(), static_cast<Scalar>(1), static_cast<Scalar>(1e-4), "Input rotation axis has not unit length.");
  }

  /*! \brief Constructor using a 4x1matrix.
   * In debug mode, an assertion is thrown if the rotation vector has not unit length.
   * \param vector     4x1-matrix with [angle; axis]
   */
  AngleAxis(const Vector4& vector)
    : angleAxis_(vector(0),vector.template block<3,1>(1,0)) {
    KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, this->axis().norm(), static_cast<Scalar>(1), static_cast<Scalar>(1e-4), "Input rotation axis has not unit length.");
  }

  /*! \brief Constructor using Eigen::AngleAxis.
   *  In debug mode, an assertion is thrown if the rotation vector has not unit length.
   *  \param other   Eigen::AngleAxis<PrimType_>
   */
  explicit AngleAxis(const Base& other) // explicit on purpose
    : angleAxis_(other) {
    KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, this->axis().norm(), static_cast<Scalar>(1), static_cast<Scalar>(1e-4), "Input rotation axis has not unit length.");
  }

  /*! \brief Constructor using another rotation.
   *  \param other   other rotation
   */
  template<typename OtherDerived_>
  inline explicit AngleAxis(const RotationBase<OtherDerived_, Usage_>& other)
    : angleAxis_(internal::ConversionTraits<AngleAxis, OtherDerived_>::convert(other.derived()).toImplementation()) {
  }

  /*! \brief Assignment operator using another rotation.
   *  \param other   other rotation
   *  \returns reference
   */
  template<typename OtherDerived_>
  AngleAxis& operator =(const RotationBase<OtherDerived_, Usage_>& other) {
    this->toImplementation() = internal::ConversionTraits<AngleAxis, OtherDerived_>::convert(other.derived()).toImplementation();
    return *this;
  }

  /*! \brief Parenthesis operator to convert from another rotation.
   *  \param other   other rotation
   *  \returns reference
   */
  template<typename OtherDerived_>
  AngleAxis& operator ()(const RotationBase<OtherDerived_, Usage_>& other) {
    this->toImplementation() = internal::ConversionTraits<AngleAxis, OtherDerived_>::convert(other.derived()).toImplementation();
    return *this;
  }

  /*! \brief Returns the inverse of the rotation.
   *  \returns the inverse of the rotation
   */
  AngleAxis inverted() const {
    Base inverse = this->toImplementation().inverse();
    inverse.angle() = -inverse.angle();
    inverse.axis() = -inverse.axis();
    return AngleAxis(inverse);
  }

  /*! \brief Inverts the rotation.
   *  \returns reference
   */
  AngleAxis& invert() {
    *this = this->inverted();
    return *this;
  }

  /*! \brief Cast to the implementation type.
   *  \returns the implementation for direct manipulation (recommended only for advanced users)
   */
  inline Implementation& toImplementation() {
    return static_cast<Implementation&>(angleAxis_);
  }

  /*! \brief Cast to the implementation type.
   *  \returns the implementation for direct manipulation (recommended only for advanced users)
   */
  inline const Implementation& toImplementation() const {
    return static_cast<const Implementation&>(angleAxis_);
  }

  /*! \brief Returns the rotation angle.
   *  \returns rotation angle (scalar)
   */
  inline Scalar angle() const {
    return angleAxis_.angle();
  }

  /*! \brief Sets the rotation angle.
   */
  inline void setAngle(Scalar angle) {
    angleAxis_.angle() = angle;
  }

  /*! \brief Returns the rotation axis.
   *  \returns rotation axis (vector)
   */
  inline const Vector3& axis() const {
    return angleAxis_.axis();
  }

  /*! \brief Sets the rotation axis.
   */
  inline void setAxis(const Vector3& axis) {
    angleAxis_.axis() = axis;
    KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, this->axis().norm(), static_cast<Scalar>(1), static_cast<Scalar>(1e-4), "Input rotation axis has not unit length.");
  }

  /*! \brief Sets the rotation axis.
   */
  inline void setAxis(Scalar v1, Scalar v2, Scalar v3) {
    angleAxis_.axis() = Vector3(v1,v2,v3);
    KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, this->axis().norm(), static_cast<Scalar>(1), static_cast<Scalar>(1e-4), "Input rotation axis has not unit length.");
  }

  /*! \brief Sets angle-axis from a 4x1-matrix
   */
  inline void setVector(const Vector4& vector) {
    this->setAngle(vector(0));
    this->setAxis(vector.template block<3,1>(1,0));
  }

  /*! \returns the angle and axis in a 4x1 vector [angle; axis].
   */
  inline Vector4 vector() const {
    Vector4 vector;
    vector(0) = angle();
    vector.template block<3,1>(1,0) = axis();
    return vector;
  }

  /*! \brief Sets the rotation to identity.
   *  \returns reference
   */
  AngleAxis& setIdentity() {
    this->setAngle(static_cast<Scalar>(0));
    this->setAxis(static_cast<Scalar>(1), static_cast<Scalar>(0), static_cast<Scalar>(0));
    return *this;
  }

  /*! \brief Returns a unique angle axis rotation with angle in [0,pi].
   *  This function is used to compare different rotations.
   *  \returns copy of the angle axis rotation which is unique
   */
  AngleAxis getUnique() const {
    AngleAxis aa(kindr::common::floatingPointModulo(angle()+M_PI,2*M_PI)-M_PI, axis()); // first wraps angle into [-pi,pi)
    if(aa.angle() > 0)  {
      return aa;
    } else if(aa.angle() < 0) {
      if(aa.angle() != -M_PI) {
        return AngleAxis(-aa.angle(),-aa.axis());
      } else { // angle == -pi, so axis must be viewed further, because -pi,axis does the same as -pi,-axis

        if(aa.axis()[0] < 0) {
          return AngleAxis(-aa.angle(),-aa.axis());
        } else if(aa.axis()[0] > 0) {
          return AngleAxis(-aa.angle(),aa.axis());
        } else { // v1 == 0

          if(aa.axis()[1] < 0) {
            return AngleAxis(-aa.angle(),-aa.axis());
          } else if(aa.axis()[1] > 0) {
            return AngleAxis(-aa.angle(),aa.axis());
          } else { // v2 == 0

            if(aa.axis()[2] < 0) { // v3 must be -1 or 1
              return AngleAxis(-aa.angle(),-aa.axis());
            } else  {
              return AngleAxis(-aa.angle(),aa.axis());
            }
          }
        }
      }
    } else { // angle == 0
      return AngleAxis();
    }
  }

  /*! \brief Modifies the angle axis rotation such that the lies angle in [0,pi).
   *  \returns reference
   */
  AngleAxis& setUnique() {
    *this = getUnique();
    return *this;
  }

  /*! \brief Concenation operator.
   *  This is explicitly specified, because Eigen provides also an operator*.
   *  \returns the concenation of two rotations
   */
  using AngleAxisBase<AngleAxis<PrimType_, Usage_>, Usage_>::operator*;

  /*! \brief Used for printing the object with std::cout.
   *  \returns std::stream object
   */
  friend std::ostream& operator << (std::ostream& out, const AngleAxis& a) {
    out << a.angle() << ", " << a.axis().transpose();
    return out;
  }
};

//! \brief Active angle axis rotation with double primitive type
typedef AngleAxis<double, RotationUsage::ACTIVE>  AngleAxisAD;
//! \brief Active angle axis rotation with float primitive type
typedef AngleAxis<float,  RotationUsage::ACTIVE>  AngleAxisAF;
//! \brief Passive angle axis rotation with double primitive type
typedef AngleAxis<double, RotationUsage::PASSIVE> AngleAxisPD;
//! \brief Passive angle axis rotation with float primitive type
typedef AngleAxis<float,  RotationUsage::PASSIVE> AngleAxisPF;

} // namespace eigen_impl

namespace internal {

template<typename PrimType_, enum RotationUsage Usage_>
class get_scalar<eigen_impl::AngleAxis<PrimType_, Usage_>> {
 public:
  typedef PrimType_ Scalar;
};

template<typename PrimType_, enum RotationUsage Usage_>
class get_matrix3X<eigen_impl::AngleAxis<PrimType_, Usage_>>{
 public:
  typedef int  IndexType;

  template <IndexType Cols>
  using Matrix3X = Eigen::Matrix<PrimType_, 3, Cols>;
};

template<typename PrimType_>
class get_other_usage<eigen_impl::AngleAxis<PrimType_, RotationUsage::ACTIVE>> {
 public:
  typedef eigen_impl::AngleAxis<PrimType_, RotationUsage::PASSIVE> OtherUsage;
};

template<typename PrimType_>
class get_other_usage<eigen_impl::AngleAxis<PrimType_, RotationUsage::PASSIVE>> {
 public:
  typedef eigen_impl::AngleAxis<PrimType_, RotationUsage::ACTIVE> OtherUsage;
};

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Conversion Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
template<typename DestPrimType_, typename SourcePrimType_, enum RotationUsage Usage_>
class ConversionTraits<eigen_impl::AngleAxis<DestPrimType_, Usage_>, eigen_impl::AngleAxis<SourcePrimType_, Usage_>> {
 public:
  inline static eigen_impl::AngleAxis<DestPrimType_, Usage_> convert(const eigen_impl::AngleAxis<SourcePrimType_, Usage_>& a) {
    return eigen_impl::AngleAxis<DestPrimType_, Usage_>(a.toImplementation().template cast<DestPrimType_>());
  }
};

template<typename DestPrimType_, typename SourcePrimType_, enum RotationUsage Usage_>
class ConversionTraits<eigen_impl::AngleAxis<DestPrimType_, Usage_>, eigen_impl::RotationVector<SourcePrimType_, Usage_>> {
 public:
  inline static eigen_impl::AngleAxis<DestPrimType_, Usage_> convert(const eigen_impl::RotationVector<SourcePrimType_, Usage_>& rotationVector) {
    typedef typename eigen_impl::RotationVector<SourcePrimType_, Usage_>::Scalar Scalar;

    const eigen_impl::RotationVector<DestPrimType_, Usage_> rv(rotationVector);

    if (rv.toImplementation().norm() < common::internal::NumTraits<Scalar>::dummy_precision()) {
      return eigen_impl::AngleAxis<DestPrimType_, Usage_>();
    }
    return eigen_impl::AngleAxis<DestPrimType_, Usage_>(rv.toImplementation().norm(), rv.toImplementation().normalized());
  }
};

template<typename DestPrimType_, typename SourcePrimType_, enum RotationUsage Usage_>
class ConversionTraits<eigen_impl::AngleAxis<DestPrimType_, Usage_>, eigen_impl::RotationQuaternion<SourcePrimType_, Usage_>> {
 public:
  inline static eigen_impl::AngleAxis<DestPrimType_, Usage_> convert(const eigen_impl::RotationQuaternion<SourcePrimType_, Usage_>& q) {
    return eigen_impl::AngleAxis<DestPrimType_, Usage_>(eigen_impl::eigen_internal::getAngleAxisFromQuaternion<SourcePrimType_, DestPrimType_>(q.toImplementation()));
  }
};

template<typename DestPrimType_, typename SourcePrimType_, enum RotationUsage Usage_>
class ConversionTraits<eigen_impl::AngleAxis<DestPrimType_, Usage_>, eigen_impl::RotationMatrix<SourcePrimType_, Usage_>> {
 public:
  inline static eigen_impl::AngleAxis<DestPrimType_, Usage_> convert(const eigen_impl::RotationMatrix<SourcePrimType_, Usage_>& rotationMatrix) {
//    return eigen_impl::AngleAxis<DestPrimType_, Usage_>(eigen_impl::getAngleAxisFromRotationMatrix<SourcePrimType_, DestPrimType_>(rotationMatrix.toImplementation()));
//    return eigen_impl::AngleAxis<DestPrimType_, Usage_>(eigen_impl::eigen_internal::getAngleAxisFromRotationMatrix<SourcePrimType_, DestPrimType_>(rotationMatrix.toImplementation()));

//   if (Usage_ == RotationUsage::ACTIVE) {
//     return eigen_impl::AngleAxis<DestPrimType_, Usage_>(eigen_impl::eigen_internal::getAngleAxisFromRotationMatrix<SourcePrimType_, DestPrimType_>(rotationMatrix.matrix()));
//
//   }
//   if (Usage_ == RotationUsage::PASSIVE) {
//     return eigen_impl::AngleAxis<DestPrimType_, Usage_>(eigen_impl::eigen_internal::getAngleAxisFromRotationMatrix<SourcePrimType_, DestPrimType_>(rotationMatrix.toImplementation()));
//
//   }
//
   return eigen_impl::AngleAxis<DestPrimType_, Usage_>(eigen_impl::eigen_internal::getAngleAxisFromRotationMatrix<SourcePrimType_, DestPrimType_>(rotationMatrix.toImplementation()));

  }
};

template<typename DestPrimType_, typename SourcePrimType_, enum RotationUsage Usage_>
class ConversionTraits<eigen_impl::AngleAxis<DestPrimType_, Usage_>, eigen_impl::EulerAnglesXyz<SourcePrimType_, Usage_>> {
 public:
  inline static eigen_impl::AngleAxis<DestPrimType_, Usage_> convert(const eigen_impl::EulerAnglesXyz<SourcePrimType_, Usage_>& xyz) {
    return eigen_impl::AngleAxis<DestPrimType_, Usage_>(eigen_impl::eigen_internal::getAngleAxisFromRpy<SourcePrimType_, DestPrimType_>(xyz.toImplementation()));
  }
};

template<typename DestPrimType_, typename SourcePrimType_, enum RotationUsage Usage_>
class ConversionTraits<eigen_impl::AngleAxis<DestPrimType_, Usage_>, eigen_impl::EulerAnglesZyx<SourcePrimType_, Usage_>> {
 public:
  inline static eigen_impl::AngleAxis<DestPrimType_, Usage_> convert(const eigen_impl::EulerAnglesZyx<SourcePrimType_, Usage_>& zyx) {
    return eigen_impl::AngleAxis<DestPrimType_, Usage_>(eigen_impl::eigen_internal::getAngleAxisFromYpr<SourcePrimType_, DestPrimType_>(zyx.toImplementation()));
  }
};

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Multiplication Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Rotation Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Comparison Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
template<typename PrimType_, enum RotationUsage Usage_>
class ComparisonTraits<eigen_impl::AngleAxis<PrimType_, Usage_>, eigen_impl::AngleAxis<PrimType_, Usage_>> {
 public:
  inline static bool isEqual(const eigen_impl::AngleAxis<PrimType_, Usage_>& a, const eigen_impl::AngleAxis<PrimType_, Usage_>& b){
    const double tolPercent = 0.01;
    return common::eigen::compareRelative(a.angle(), b.angle(), tolPercent) &&
        common::eigen::compareRelative(a.axis().x(), b.axis().x(), tolPercent) &&
        common::eigen::compareRelative(a.axis().y(), b.axis().y(), tolPercent) &&
        common::eigen::compareRelative(a.axis().z(), b.axis().z(), tolPercent);
  }
};

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Fixing Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
template<typename PrimType_, enum RotationUsage Usage_>
class FixingTraits<eigen_impl::AngleAxis<PrimType_, Usage_>> {
 public:
  inline static void fix(eigen_impl::AngleAxis<PrimType_, Usage_>& aa) {
    aa.setAxis(aa.axis().normalized());
  }
};

} // namespace internal
} // namespace rotations
} // namespace kindr

#endif /* KINDR_ROTATIONS_EIGEN_ANGLEAXIS_HPP_ */


/*
 * Copyright 2009-2011 The VOTCA Development Team (http://www.votca.org)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

#include <iostream>
#include <fstream>
#include <vector>
#include <math.h>
#include <string>
#include <boost/lexical_cast.hpp>
#include "analysistool.h"
#include <votca/tools/histogram.h>
#include <votca/csg/version.h>
#include "bondedstatistics.h"
#include "tabulatedpotential.h"

using namespace std;
using namespace boost;

TabulatedPotential::TabulatedPotential()
{
    _tab_smooth1 = _tab_smooth2 = 0;
    _T = 300;
}

void TabulatedPotential::Register(map<string, AnalysisTool *> &lib)
{
    lib["tab"] = this;
    lib["hist"] = this;
}

void TabulatedPotential::Command(BondedStatistics &bs, string cmd, vector<string> &args)
{
    if(args[0] == "set") {
        if(cmd == "hist") SetOption(_hist_options, args);
        else if(cmd == "tab") {
            if(!SetOption(_tab_options, args)) {
                if(args.size() >2) {
                    if(args[1] == "smooth_pdf")
                        _tab_smooth1 = lexical_cast<int>(args[2]);
                    else if(args[1] == "smooth_pot")
                        _tab_smooth2 = lexical_cast<int>(args[2]);
                    else if(args[1] == "T")
                        _T = lexical_cast<double>(args[2]);
                    else {
                        cout << "unknown option " << args[2] << endl;
                        return;
                    }
                }
            }
            if(args.size() <=2) {
                cout << "smooth_pdf: " << _tab_smooth1 << endl;
                cout << "smooth_pot: " << _tab_smooth2 << endl;
                cout << "T: " << _T << endl;
            }
        }
    }
    else if(args.size() >= 2) {
        if(cmd == "hist") WriteHistogram(bs, args);
        else if(cmd == "tab") WritePotential(bs, args);
    }
    else cout << "wrong number of arguments" << endl;
}

void TabulatedPotential::Help(string cmd, vector<string> &args)
{
    if(args.size() == 0) {
        if(cmd == "tab") {
            cout << "tab <file> <selection>\n"
                 << "Calculate tabulated potential by inverting the distribution function. "
                    "Statistics is calculated using all interactions in selection.\n"
                    "see also: help tab set\n\n"
                    "example:\ntab set scale bond\ntab U_bond.txt *:bond:*\n";
        }
        if(cmd == "hist") {
            cout << "hist <file> <selection>\n"
                 << "Calculate distribution function for selection. "
                    "Statistics is calculated using all interactions in selection.\n"
                    "see also: help hist set\n\n"
                    "example:hist U_bond.txt *:bond:*\n";
        }
        return;
    }
    if(args[0] == "set") {
        if(args.size() == 1) {
            cout << cmd << " set <option> <value>\n"
                 << "set option for this command. Use \"" << cmd << " set\""
                    " for a list of available options. To get help on a specific option use e.g.\n"
                    << cmd << " set periodic\n";
            return;
        }
        if(args[1] == "n") {
            cout << cmd << "set n <integer>\n"
                 << "set number of bins for table\n";
            return;
        }
        if(args[1] == "min") {
            cout << cmd << "set min <value>\n"
                 << "minimum value of interval for histogram (see also periodic, extend)\n";
            return;
        }
        if(args[1] == "max") {
            cout << cmd << "set max <value>\n"
                 << "maximum value of interval for histogram (see also periodic, extend)\n";
            return;
        }
        if(args[1] == "periodic") {
            cout << cmd << "set periodic <value>\n"
                << "can be 1 for periodic interval (e.g. dihedral) or 0 for "
                    "non-periodic (e.g. bond)\n";
            return;
        }
        if(args[1] == "auto") {
            cout << cmd << "set auto <value>\n"
                 "can be 1 for automatically determine the interval for the "
                 "table (min, max, extend will be ignored) or 0 to use min/max as specified\n";
            return;
        }
        if(args[1] == "extend") {
            cout << cmd << "set extend <value>\n"
                 "should only be used with auto=0. Can be 1 for extend the interval "
                 "if values are out of bounds (min/max) "
                 "or 0 to ignore values which are out of the interal\n";
            return;
        }
        if(args[1] == "scale") {
            cout << cmd << "set scale <value>\n"
                "volume normalization of pdf. Can be no (no scaling), bond "
                "(1/r^2) or angle ( 1/sin(phi) ). See VOTCA manual, section "
                "theoretical background for details\n";
            return;
        }
        if(args[1] == "normalize") {
            cout << cmd << "set normalize <value>\n"
                 "can be 1 for a normalized histogram or 0 to skip normalization\n";
            return;
        }

        if(cmd == "tab") {
            if(args[1] == "smooth_pdf") {
                cout << "tab set smooth_pdf <value>\n"
                 "Perform so many smoothing iterations on the distribution function before inverting the potential\n";
                return;
            }
            if(args[1] == "smooth_pot") {
                cout << "tab set smooth_pot <value>\n"
                 "Perform so many smoothing iterations on tabulated potential after inverting the potential\n";
                return;
            }
            if(args[1] == "T") {
                cout << "tab set T <value>\n"
                 "Temperature in Kelvin the simulation was performed\n";
                return;
            }
        }
    }

    cout << "no help text available" << endl;
}

bool TabulatedPotential::SetOption(Histogram::options_t &op, const vector<string> &args)
{
    if(args.size() >2) {
        if(args[1] == "n")
            op._n = lexical_cast<int>(args[2]);
        else if(args[1] == "min") {
            op._min = lexical_cast<double>(args[2]);
        }
        else if(args[1] == "max")
            op._max = lexical_cast<double>(args[2]);
        else if(args[1] == "periodic")
            op._periodic = lexical_cast<bool>(args[2]);
        else if(args[1] == "auto")
            op._auto_interval = lexical_cast<bool>(args[2]);
        else if(args[1] == "extend")
            op._extend_interval = lexical_cast<bool>(args[2]);
        else if(args[1] == "normalize")
            op._normalize = lexical_cast<bool>(args[2]);
        else if(args[1] == "scale") {
            if(args[2]=="no" || args[2]=="bond" || args[2]=="angle")
                op._scale = args[2];
            else {
                cout << "scale can be: no, bond or angle\n";
            }
        }
        else {
            return false;
        }
    }
    else {
        cout << "n: " << op._n << endl;
        cout << "min: " << op._min << endl;
        cout << "max: " << op._max << endl;
        cout << "periodic: " << op._periodic << endl;
        cout << "auto: " << op._auto_interval << endl;
        cout << "extend: " << op._extend_interval << endl;
        cout << "scale: " << op._scale << endl;
        cout << "normalize: " << op._normalize << endl;
    }
    return true;
}

void TabulatedPotential::WriteHistogram(BondedStatistics &bs, vector<string> &args)
{
    ofstream out;
    DataCollection<double>::selection *sel = NULL;

    for(size_t i=1; i<args.size(); i++)
        sel = bs.BondedValues().select(args[i], sel);
    Histogram h(_hist_options);
    h.ProcessData(sel);
    out.open(args[0].c_str());
/*    out << "# histogram, created csg version " <<  VERSION_STR  << endl;
    out << "# n = " << _hist_options._n << endl;
    out << "# min = " << _hist_options._min << endl;
    out << "# max = " << _hist_options._max << endl;
    out << "# periodic = " << _hist_options._periodic << endl;
    out << "# auto = " << _hist_options._auto_interval << endl;
    out << "# extend = " << _hist_options._extend_interval << endl;
    out << "# scale = " << _hist_options._scale << endl;*/
    out << h ;
    out.close();
    cout << "histogram created using " << sel->size() << " data-rows, written to " << args[0] << endl;
    delete sel;
}

void TabulatedPotential::CalcForce(vector<double> &U, vector<double> &F, double dx, bool bPeriodic)
{
    size_t n=U.size();
    double f = 0.5/dx;
    F.resize(n);
    if(bPeriodic)
        F[n-1] = F[0] = -(U[1] - U[n-2])*f;
    else {
        F[0] = -(U[1] - U[0])*2*f;
        F[n-1] = -(U[n-1] - U[n-2])*2*f;
    }
    for(size_t i=1; i<n-1; i++)
        F[i] = -(U[i+1] - U[i-1])*f;
}

void TabulatedPotential::WritePotential(BondedStatistics &bs, vector<string> &args)
{
   ofstream out;
    DataCollection<double>::selection *sel = NULL;

    for(size_t i=1; i<args.size(); i++)
        sel = bs.BondedValues().select(args[i], sel);
    Histogram h(_tab_options);
    h.ProcessData(sel);
    for(int i=0; i<_tab_smooth1; ++i)
        Smooth(h.getPdf(), _tab_options._periodic);
    BoltzmannInvert(h.getPdf(), _T);
    for(int i=0; i<_tab_smooth2; ++i)
        Smooth(h.getPdf(), _tab_options._periodic);
    out.open(args[0].c_str());

/*       out << "# tabulated potential, created csg version " VERSION_STR  << endl;
    out << "# n = " << _tab_options._n << endl;
    out << "# min = " << _tab_options._min << endl;
    out << "# max = " << _tab_options._max << endl;
    out << "# periodic = " << _tab_options._periodic << endl;
    out << "# auto = " << _tab_options._auto_interval << endl;
    out << "# extend = " << _tab_options._extend_interval << endl;
    out << "# scale = " << _tab_options._scale << endl;
    out << "# smooth_pdf = " << _tab_smooth1 << endl;
    out << "# smooth_pot = " << _tab_smooth2 << endl;
    out << "# T = " << _T << endl;*/

    vector<double> F;

    CalcForce(h.getPdf(), F, h.getInterval(), _tab_options._periodic);
    for(int i=0; i<h.getN(); i++) {
        out << h.getMin() + h.getInterval()*((double)i) << " " << h.getPdf()[i] << " " << F[i] << endl;
    }
    out.close();
    cout << "histogram created using " << sel->size() << " data-rows, written to " << args[0] << endl;
    delete sel;
}

void TabulatedPotential::Smooth(vector<double> &data, bool bPeriodic)
{
    double old[3];
    int n=data.size();
    if(bPeriodic) {
        old[0] = data[n-3];
        old[1] = data[n-2];
    }
    else {
        old[0] = data[0];
        old[1] = data[0];
    }
    size_t i;
    for(i=0; i<data.size()-2; i++) {
        old[2] = data[i];
        data[i] = (old[0] + 2.*old[1] + 3.*data[i] + 2.*data[i+1] + data[i+2])/9.;
        old[0]=old[1];
        old[1]=old[2];;
    }
    if(bPeriodic) {
        data[i] = (old[0] + 2.*old[1] + 3.*data[i] + 2.*data[i+1] + data[0])/9.;
        old[0]=old[1];old[1]=data[i];
        data[n-1] = data[0];
    }
    else {
        data[i] = (old[0] + 2.*old[1] + 3.*data[i] + 3.*data[i+1])/9.;
        old[0]=old[1];old[1]=data[i];
        i++;
        data[i] = (old[0] + 2.*old[1] + 6.*data[i])/9.;
    }
}

void TabulatedPotential::BoltzmannInvert(vector<double> &data, double T)
{
    double _min, _max;

    _min = numeric_limits<double>::max();
    _max = numeric_limits<double>::min();

    for(size_t i=0; i<data.size(); i++) {
        _max = max(data[i], _max);
        if(data[i] > 0) _min = min(data[i], _min);
    }
    _max = -8.3109*T*log(_max)*0.001;
    _min = -8.3109*T*log(_min)*0.001-_max;

    for(size_t i=0; i<data.size(); i++) {
        if(data[i] == 0) data[i] = _min;
        else
            data[i] = -8.3109*T*log(data[i])*0.001 - _max;
    }
}


#include "SVGPath.h"
#include "SVGParser.h"

#include <pugixml.hpp>

#include <boost/lexical_cast.hpp>

#include <fstream>
#include <iostream>

namespace
{
	Vector parseTranslate(const pugi::xml_node &g_node)
	{
		Vector translate;
		std::string transformAttribute = g_node.attribute("transform").as_string("translate(0,0)");
		std::string::size_type beg = transformAttribute.find("translate(") + 10;
		std::string::size_type end = transformAttribute.find(")", beg);
		transformAttribute = transformAttribute.substr(beg, end - beg);

		std::string::size_type comma = transformAttribute.find(",");
		translate.x = boost::lexical_cast<float>(transformAttribute.substr(0, comma));
		translate.y = boost::lexical_cast<float>(transformAttribute.substr(comma + 1));
		return translate;
	}

	VectorList parsePath(const pugi::xml_node &path_node, const Vector &translate, int flags)
	{
		VectorList result;
		std::string pathString = path_node.attribute("d").as_string();
		std::istringstream pathStream(pathString);

		SVGParser::SVGPath svgPath = SVGParser::parsePath(pathStream);

		for (SVGParser::SVGCommand *cmd : svgPath)
		{
			switch (cmd->getType())
			{
			case SVGParser::SVG_MOVE_TO_ABS:
			case SVGParser::SVG_LINE_TO_ABS:
				{
					float x = (cmd->x + translate.x);
					float y = (cmd->y + translate.y);

					if (flags & SVGPath::ROUND)
					{
						x = std::floor(x + 0.5f);
						y = std::floor(y + 0.5f);
					}

					result.emplace_back(x,y);
					break;
				}
			case SVGParser::SVG_CLOSE_PATH:
				break; // Do nothing
			default:
				std::cerr << "Unhandled svg command" << std::endl;
				break;
			}
		}

		return result;
	}
}

SVGPath::SVGPath(int flags) : flags(flags)
{
}
SVGPath::~SVGPath()
{
	close();
}

bool SVGPath::openFile(const std::string &filename)
{
	std::fstream file(filename, std::ios::in);
	if (!file.is_open())
	{
		std::cerr << "Could not open file" << std::endl;
		return false;
	}

	pugi::xml_document doc;
	pugi::xml_parse_result result = doc.load(file);
	if (result.status != pugi::status_ok)
	{
		std::cerr << "Parse error: " << result.description() << std::endl;
		return false;
	}

	pugi::xml_node g_node = doc.select_single_node("/svg/g").node();
	pugi::xml_node path_node = g_node.child("path");

	Vector translate = parseTranslate(g_node);
	path = parsePath(path_node, translate, flags);

	return true;
}
void SVGPath::close()
{
	path.clear();
}


//==================================================================================================
/*
  Copyright 2017 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
//! [replace_if]
#include <boost/simd/algorithm/replace_if.hpp>
#include <boost/simd/pack.hpp>
#include <iostream>
#include <vector>

namespace bs =  boost::simd;

struct p
{

  template < typename T >
  auto operator()(const T & t) const -> decltype((t >= 3.5f)&&(t < 5.0f))
  {
    return (t >= 3.5f)&&(t < 5.0f);
  }
};

int main()
{
  namespace bs =  boost::simd;
  std::vector<float> d{2.0f, 2.5f, 4.0f, 3.5f, 4.0f, 4.0f, 5.0f, 4.0f};
  d[7] = 42.0f;
  bs::replace_if( d.data(),d.data()+7, p(),  0.0f);
  std::cout << "d =  (" ;
  for( auto e : d) std::cout << e << " ";
  std::cout << ") " << std::endl;

  std::vector<float> d1{4.0f, 2.5f, 4.0f, 3.5f, 4.0f, 4.0f, 5.0f, 4.0f};
  bs::replace_if( d1.data()+1,d1.data()+8, p(),  0.0f);
  std::cout << "d1 =  (" ;
  for( auto e : d1) std::cout << e << " ";
  std::cout << ") " << std::endl;

  return 0;
 }
//! [replace_if]


///////////////////////////////////////////////////////////////
//  Copyright Christopher Kormanyos 2002 - 2011.
//  Copyright 2011 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_
//
// This work is based on an earlier work:
// "Algorithm 910: A Portable C++ Multiple-Precision System for Special-Function Calculations",
// in ACM TOMS, {VOL 37, ISSUE 4, (February 2011)} (C) ACM, 2011. http://doi.acm.org/10.1145/1916461.1916469

#ifdef _MSC_VER
#  define _SCL_SECURE_NO_WARNINGS
#endif

#include <boost/detail/lightweight_test.hpp>
#include <boost/array.hpp>
#include "test.hpp"

#include <boost/multiprecision/gmp.hpp>

int main()
{
   using namespace boost::multiprecision;
   //
   // Test interconversions between GMP supported backends:
   //
   mpf_t mpf;
   mpz_t mpz;
   mpq_t mpq;
   mpf_init2(mpf, 100);
   mpf_set_ui(mpf, 2u);
   mpz_init(mpz);
   mpz_set_ui(mpz, 2u);
   mpq_init(mpq);
   mpq_set_ui(mpq, 2u, 1u);

   BOOST_TEST(mpf_float(mpf) == 2);
   BOOST_TEST(mpf_float_50(mpf) == 2);
   BOOST_TEST(mpf_float(mpz) == 2);
   BOOST_TEST(mpf_float_50(mpz) == 2);
   BOOST_TEST(mpf_float(mpq) == 2);
   BOOST_TEST(mpf_float_50(mpq) == 2);

   mpf_float f0;
   mpf_float_50 f50;
   f0 = mpf;
   BOOST_TEST(f0 == 2);
   f0 = 0;
   f0 = mpz;
   BOOST_TEST(f0 == 2);
   f0 = 0;
   f0 = mpq;
   BOOST_TEST(f0 == 2);

   f50 = mpf;
   BOOST_TEST(f50 == 2);
   f50 = 0;
   f50 = mpz;
   BOOST_TEST(f50 == 2);
   f50 = 0;
   f50 = mpq;
   BOOST_TEST(f50 == 2);

   f50 = 4;
   f0 = f50;
   BOOST_TEST(f0 == 4);
   f0 = 3;
   f50 = f0;
   BOOST_TEST(f50 == 3);
   f50 = 4;
   BOOST_TEST(mpf_float(f50) == 4);
   BOOST_TEST(mpf_float_50(f0) == 3);

   mpz_int iz(2);
   mpq_rational rat(2);
   f50 = iz;
   BOOST_TEST(f50 == 2);
   f0 = iz;
   BOOST_TEST(f0 == 2);
   BOOST_TEST(mpf_float(iz) == 2);
   BOOST_TEST(mpf_float_50(iz) == 2);
   BOOST_TEST(mpf_float(rat) == 2);
   BOOST_TEST(mpf_float_50(rat) == 2);

   BOOST_TEST(mpz_int(mpf) == 2);
   BOOST_TEST(mpz_int(mpz) == 2);
   BOOST_TEST(mpz_int(mpq) == 2);
   iz = 3;
   iz = mpz_int(mpf);  // explicit conversion only
   BOOST_TEST(iz == 2);
   iz = 3;
   iz = mpz;
   BOOST_TEST(iz == 2);
   iz = 4;
   iz = mpz_int(mpq);  // explicit conversion only
   BOOST_TEST(iz == 2);
   f0 = 2;
   f50 = 2;

   BOOST_TEST(mpz_int(f0) == 2);
   BOOST_TEST(mpz_int(f50) == 2);
   rat = 2;
   BOOST_TEST(mpz_int(rat) == 2);
   iz = 3;
   iz = static_cast<mpz_int>(f0);
   BOOST_TEST(iz == 2);
   iz = 3;
   iz = static_cast<mpz_int>(f50);
   BOOST_TEST(iz == 2);
   iz = 3;
   iz = static_cast<mpz_int>(rat);
   BOOST_TEST(iz == 2);

   BOOST_TEST(mpq_rational(mpz) == 2);
   BOOST_TEST(mpq_rational(mpq) == 2);
   BOOST_TEST(mpq_rational(iz) == 2);
   rat = 3;
   rat = mpz;
   BOOST_TEST(rat == 2);
   rat = 3;
   rat = mpq;
   BOOST_TEST(rat == 2);
   rat = 3;
   rat = iz;
   BOOST_TEST(rat == 2);

   iz = numerator(rat);
   BOOST_TEST(iz == 2);
   iz = denominator(rat);
   BOOST_TEST(iz == 1);

   //
   // Conversions involving precision only,
   // note that mpf_t precisions are only approximate:
   //
   mpf_float::default_precision(30);
   f50 = 2;
   mpf_float_100   f100(3);
   mpf_float       f0a(4);
   mpf_float       f0b(f100);
   BOOST_TEST(f0a.precision() >= 30);
   BOOST_TEST(f0b.precision() >= 100);
   f0a = f100;
   BOOST_TEST(f0a == 3);
   BOOST_TEST(f0a.precision() >= 100);

   f100 = f50;
   BOOST_TEST(f100 == 2);

   return boost::report_errors();
}


/*
 */
#include "sparta/utils/SpartaTester.hpp"
#include <boost/dynamic_bitset.hpp>

TEST_INIT;

static void testOperatorEqual()
{
    const boost::dynamic_bitset<> a(sizeof(uint32_t) * 8, 0xdeadbeef);
    const boost::dynamic_bitset<> b(sizeof(uint32_t) * 8, 0xdeadbeef);
    const boost::dynamic_bitset<> c(sizeof(uint32_t) * 8, 0xabcdabcd);

    EXPECT_TRUE(a == b);
    EXPECT_TRUE(a != c);
}

static void testCopyConstructor()
{
    const boost::dynamic_bitset<> a(sizeof(uint32_t) * 8, 0xdeadbeef);
    const boost::dynamic_bitset<> b(a);

    EXPECT_TRUE(a == b);
}

static void testAssignmentConstructor()
{
    const uint32_t a_value = 0xdeadbeef;
    const uint32_t b_value = 0xabcdabcd;
    boost::dynamic_bitset<> a(sizeof(uint32_t) * 8, a_value);
    boost::dynamic_bitset<> b(sizeof(uint32_t) * 8, b_value);

    EXPECT_TRUE(a != b);
    a = b;
    EXPECT_TRUE(a == b);
}

static void testOperatorLeftShift()
{
    const boost::dynamic_bitset<> a(sizeof(uint32_t) * 8, 0xf0f0f0f0);
    for (size_t i = 0; i < sizeof(uint32_t) * 8; i++) {
        const boost::dynamic_bitset<> b(sizeof(uint32_t) * 8, uint32_t(0xf0f0f0f0) << i);

        EXPECT_TRUE((a << i) == b);
    }
}

static void testOperatorLeftShiftAssign()
{
    boost::dynamic_bitset<> a(sizeof(uint32_t) * 8, 0xf0f0f0f0);
    for (size_t i = 0; i < sizeof(uint32_t) * 8; i++) {
        const boost::dynamic_bitset<> b(sizeof(uint32_t) * 8, uint32_t(0xf0f0f0f0) << i);

        EXPECT_TRUE(a == b);
        a <<= 1;
    }
}

static void testOperatorRightShift()
{
    boost::dynamic_bitset<> a(sizeof(uint32_t) * 8, 0xf0f0f0f0);
    for (size_t i = 0; i < sizeof(uint32_t) * 8; i++) {
        const boost::dynamic_bitset<> b(sizeof(uint32_t) * 8, uint32_t(0xf0f0f0f0) >> i);

        EXPECT_TRUE((a >> i) == b);
    }
}

static void testOperatorRightShiftAssign()
{
    boost::dynamic_bitset<> a(sizeof(uint32_t) * 8, 0xf0f0f0f0);
    for (size_t i = 0; i < sizeof(uint32_t)*8; i++) {
        const boost::dynamic_bitset<> b(sizeof(uint32_t) * 8, uint32_t(0xf0f0f0f0) >> i);
        // std::cout << "i == " << i << " " << b << " " << a << std::endl;

        EXPECT_TRUE(a == b);
        a >>= 1;
    }
}

static void testOperatorAnd()
{
    const uint32_t a = 0x12345678;
    const uint32_t b = 0x87654321;

    const boost::dynamic_bitset<> aa(sizeof(uint32_t) * 8, a);
    const boost::dynamic_bitset<> bb(sizeof(uint32_t) * 8, b);
    const boost::dynamic_bitset<> cc(sizeof(uint32_t) * 8, a & b);

    EXPECT_TRUE((aa & bb) == cc);
}

static void testOperatorAndAssign()
{
    const uint32_t a = 0x12345678;
    const uint32_t b = 0x87654321;

    boost::dynamic_bitset<> aa(sizeof(uint32_t) * 8, a);
    const boost::dynamic_bitset<> bb(sizeof(uint32_t) * 8, b);
    const boost::dynamic_bitset<> cc(sizeof(uint32_t) * 8, a & b);

    aa &= bb;
    EXPECT_TRUE(aa == cc);
}

static void testOperatorOr()
{
    uint32_t a = 0x12345678;
    uint32_t b = 0x87654321;

    const boost::dynamic_bitset<> aa(sizeof(uint32_t) * 8, a);
    const boost::dynamic_bitset<> bb(sizeof(uint32_t) * 8, b);
    const boost::dynamic_bitset<> cc(sizeof(uint32_t) * 8, a | b);

    EXPECT_TRUE((aa | bb) == cc);
}

static void testOperatorOrAssign()
{
    uint32_t a = 0x12345678;
    uint32_t b = 0x87654321;

    boost::dynamic_bitset<> aa(sizeof(uint32_t) * 8, a);
    const boost::dynamic_bitset<> bb(sizeof(uint32_t) * 8, b);
    const boost::dynamic_bitset<> cc(sizeof(uint32_t) * 8, a | b);

    aa |= bb;
    EXPECT_TRUE(aa == cc);
}

static void testOperatorNegate()
{
    const boost::dynamic_bitset<> a(sizeof(uint32_t) * 8, uint32_t(0xdeadbeef));
    const boost::dynamic_bitset<> b(sizeof(uint32_t) * 8, uint32_t(~0xdeadbeef));

    EXPECT_TRUE(~a == b);
}

int main()
{
    /* This is important for performance */
    for(uint32_t i = 0; i < 1000000; ++i)
    {

        testOperatorEqual();

        testOperatorLeftShift();
        testOperatorLeftShiftAssign();

        testOperatorRightShift();
        testOperatorRightShiftAssign();

        testOperatorAnd();
        testOperatorAndAssign();

        testOperatorOr();
        testOperatorOrAssign();

        testOperatorNegate();
    }

    REPORT_ERROR;
    return ERROR_CODE;
}


#include "Day21-FractalArt.h"

#include "FractalImage.h"
#include "ImageEnhancer.h"

#include <AdventOfCodeCommon/DisableLibraryWarningsMacros.h>

__BEGIN_LIBRARIES_DISABLE_WARNINGS
#include <boost/algorithm/string.hpp>
__END_LIBRARIES_DISABLE_WARNINGS

namespace AdventOfCode
{
namespace Year2017
{
namespace Day21
{

SmallSquare createSquareFromString(const std::string& matrixString)
{
    std::vector<std::string> rows;
    boost::split(rows, matrixString, boost::is_any_of("/"));

    DynamicBoolMatrix square{rows.size(), rows.size()};

    for (size_t j = 0; j < rows.size(); ++j)
    {
        for (size_t i = 0; i < rows.size(); ++i)
        {
            square(j, i) = (rows[j][i] == '#');
        }
    }

    return square;
}

ImageEnhancer::EnhancementMap createEnhancementMapFromRuleLines(const std::vector<std::string>& enhancementRuleLines)
{
    ImageEnhancer::EnhancementMap enhancementMap;

    for (const auto& line : enhancementRuleLines)
    {
        std::vector<std::string> matrixStrings;
        boost::split(matrixStrings, line, boost::is_any_of("=> "), boost::token_compress_on);
        if (matrixStrings.size() != 2)
        {
            throw std::runtime_error("Expected 2 matrices.");
        }

        SmallSquare from = createSquareFromString(matrixStrings[0]);
        SmallSquare to = createSquareFromString(matrixStrings[1]);

        enhancementMap.emplace(std::move(from), std::move(to));
    }

    return enhancementMap;
}

unsigned numPixelsOnAfterEnhancements(const std::vector<std::string>& enhancementRuleLines, unsigned numIterations)
{
    ImageEnhancer::EnhancementMap enhancementMap = createEnhancementMapFromRuleLines(enhancementRuleLines);

    FractalImage fractalImage{std::move(enhancementMap)};
    fractalImage.enhanceRepeatedly(numIterations);
    return fractalImage.numPixelsOn();
}

unsigned numPixelsOnSmallIterations(const std::vector<std::string>& enhancementRuleLines, unsigned numIterations)
{
    return numPixelsOnAfterEnhancements(enhancementRuleLines, numIterations);
}

unsigned numPixelsOnLargeIterations(const std::vector<std::string>& enhancementRuleLines, unsigned numIterations)
{
    return numPixelsOnAfterEnhancements(enhancementRuleLines, numIterations);
}

}
}
}


#ifndef MAIA_UTILITIES_GLTF_H_INCLUDED
#define MAIA_UTILITIES_GLTF_H_INCLUDED

#include <cstddef>
#include <optional>
#include <string>
#include <unordered_map>
#include <variant>
#include <vector>

#include <Eigen/Geometry>
#include <nlohmann/json.hpp>

namespace Maia::Utilities::glTF
{
	enum class Component_type
	{
		Byte = 5120,
		Unsigned_byte = 5121,
		Short = 5122,
		Unsigned_short = 5123,
		Unsigned_int = 5125,
		Float = 5126
	};

	std::uint8_t size_of(Component_type component_type);

	struct Accessor
	{
		enum class Type
		{
			Scalar,
			Vector2,
			Vector3,
			Vector4,
			Matrix2x2,
			Matrix3x3,
			Matrix4x4
		};

		std::optional<std::size_t> buffer_view_index;
		Component_type component_type;
		std::size_t count;
		Type type;
		std::optional<Eigen::Vector3f> max;
		std::optional<Eigen::Vector3f> min;
	};

	void from_json(nlohmann::json const& json, Accessor& value);
	void to_json(nlohmann::json& json, Accessor const& value);
	std::uint8_t size_of(Accessor::Type accessor_type);

	struct Buffer
	{
		std::optional<std::string> uri;
		std::size_t byte_length;
	};

	void from_json(nlohmann::json const& json, Buffer& value);
	void to_json(nlohmann::json& json, Buffer const& value);

	struct Buffer_view
	{
		std::size_t buffer_index;
		std::size_t byte_offset{ 0 };
		std::size_t byte_length;
	};

	void from_json(nlohmann::json const& json, Buffer_view& value);
	void to_json(nlohmann::json& json, Buffer_view const& value);

	struct PbrMetallicRoughness
	{
		Eigen::Vector4f base_color_factor{ 1.0f, 1.0f, 1.0f, 1.0f };
		float metallic_factor{ 1.0f };
		float roughness_factor{ 1.0f };
	};

	void from_json(nlohmann::json const& json, PbrMetallicRoughness& value);
	void to_json(nlohmann::json& json, PbrMetallicRoughness const& value);

	struct Material
	{
		std::optional<std::string> name;
		PbrMetallicRoughness pbr_metallic_roughness{};
		Eigen::Vector3f emissive_factor{ 0.0f, 0.0f, 0.0f };
		std::string alpha_mode{ "OPAQUE" };
		float alpha_cutoff{ 0.5f };
		bool double_sided{ false };
	};

	void from_json(nlohmann::json const& json, Material& value);
	void to_json(nlohmann::json& json, Material const& value);

	struct Primitive
	{
		std::unordered_map<std::string, std::size_t> attributes;
		std::optional<std::size_t> indices_index;
		std::optional<std::size_t> material_index;
	};

	void from_json(nlohmann::json const& json, Primitive& value);
	void to_json(nlohmann::json& json, Primitive const& value);

	struct Mesh
	{
		std::vector<Primitive> primitives;
		std::optional<std::string> name;
	};

	void from_json(nlohmann::json const& json, Mesh& value);
	void to_json(nlohmann::json& json, Mesh const& value);

	struct Camera
	{
		enum class Type
		{
			Orthographic,
			Perspective
		};

		struct Orthographic
		{
			float horizontal_magnification;
			float vertical_magnification;
			float near_z;
			float far_z;
		};

		struct Perspective
		{
			std::optional<float> aspect_ratio;
			float vertical_field_of_view;
			float near_z;
			std::optional<float> far_z;
		};

		Type type;
		std::optional<std::string> name;
		std::variant<Orthographic, Perspective> projection;
	};

	struct Node
	{
		std::optional<std::string> name;

		std::optional<std::size_t> mesh_index;
		std::optional<std::size_t> camera_index;
		std::optional<std::vector<std::size_t>> child_indices;

		Eigen::Quaternionf rotation{ Eigen::Quaternionf::Identity() };
		Eigen::Vector3f scale{ Eigen::Vector3f::Ones() };
		Eigen::Vector3f translation{ Eigen::Vector3f::Zero() };
	};

	void from_json(nlohmann::json const& json, Node& value);
	void to_json(nlohmann::json& json, Node const& value);

	struct Scene
	{
		std::optional<std::string> name;
		std::optional<std::vector<std::size_t>> nodes;
	};

	void from_json(nlohmann::json const& json, Scene& value);
	void to_json(nlohmann::json& json, Scene const& value);

	struct Gltf
	{
		std::optional<std::vector<Accessor>> accessors;
		std::optional<std::vector<Buffer>> buffers;
		std::optional<std::vector<Buffer_view>> buffer_views;
		std::optional<std::vector<Camera>> cameras;
		std::optional<std::vector<Material>> materials;
		std::optional<std::vector<Mesh>> meshes;
		std::optional<std::vector<Node>> nodes;
		std::optional<std::size_t> scene_index;
		std::optional<std::vector<Scene>> scenes;
	};

	void from_json(nlohmann::json const& json, Gltf& value);
	void to_json(nlohmann::json& json, Gltf const& value);

}

#endif