// Copyright 2005-2014 Daniel James.
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
//  Based on Peter Dimov's proposal
//  http://www.open-std.org/JTC1/SC22/WG21/docs/papers/2005/n1756.pdf
//  issue 6.18.
//
//  This also contains public domain code from MurmurHash. From the
//  MurmurHash header:
//
// MurmurHash3 was written by Austin Appleby, and is placed in the public
// domain. The author hereby disclaims copyright to this source code.
//
// Copyright 2021 Ion Gaztanaga
// Refactored the original boost/container_hash/hash.hpp to avoid
// any heavy std header dependencies to just combine two hash
// values represented in a std::size_t type.

#ifndef BOOST_INTRUSIVE_DETAIL_HASH_COMBINE_HPP
#define BOOST_INTRUSIVE_DETAIL_HASH_COMBINE_HPP

#ifndef BOOST_CONFIG_HPP
#  include <boost/config.hpp>
#endif

#if defined(BOOST_HAS_PRAGMA_ONCE)
#  pragma once
#endif

#include <boost/cstdint.hpp>

#if defined(_MSC_VER)
#  include <stdlib.h>
#  define BOOST_INTRUSIVE_HASH_ROTL32(x, r) _rotl(x,r)
#else
#  define BOOST_INTRUSIVE_HASH_ROTL32(x, r) (x << r) | (x >> (32 - r))
#endif

namespace boost {
namespace intrusive {
namespace detail {

template <typename SizeT>
inline void hash_combine_size_t(SizeT& seed, SizeT value)
{
   seed ^= value + 0x9e3779b9 + (seed<<6) + (seed>>2);
}

inline void hash_combine_size_t(boost::uint32_t& h1,
         boost::uint32_t k1)
{
   const uint32_t c1 = 0xcc9e2d51;
   const uint32_t c2 = 0x1b873593;

   k1 *= c1;
   k1 = BOOST_INTRUSIVE_HASH_ROTL32(k1,15);
   k1 *= c2;

   h1 ^= k1;
   h1 = BOOST_INTRUSIVE_HASH_ROTL32(h1,13);
   h1 = h1*5+0xe6546b64;
}

   // Don't define 64-bit hash combine on platforms without 64 bit integers,
   // and also not for 32-bit gcc as it warns about the 64-bit constant.
   #if !defined(BOOST_NO_INT64_T) && \
       !(defined(__GNUC__) && ULONG_MAX == 0xffffffff)
   inline void hash_combine_size_t(boost::uint64_t& h,
            boost::uint64_t k)
   {
      const boost::uint64_t m = UINT64_C(0xc6a4a7935bd1e995);
      const int r = 47;

      k *= m;
      k ^= k >> r;
      k *= m;

      h ^= k;
      h *= m;

      // Completely arbitrary number, to prevent 0's
      // from hashing to 0.
      h += 0xe6546b64;
   }

   #endif // BOOST_NO_INT64_T

}  //namespace detail {
}  //namespace intrusive {
}  //namespace boost {

#endif   //BOOST_INTRUSIVE_DETAIL_HASH_COMBINE_HPP


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2013 Alec Jacobson <alecjacobson@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "massmatrix.h"
#include "massmatrix_intrinsic.h"
#include "edge_lengths.h"
#include "normalize_row_sums.h"
#include "sparse.h"
#include "doublearea.h"
#include "repmat.h"
#include <Eigen/Geometry>
#include <iostream>

template <typename DerivedV, typename DerivedF, typename Scalar>
IGL_INLINE void igl::massmatrix(
  const Eigen::MatrixBase<DerivedV> & V,
  const Eigen::MatrixBase<DerivedF> & F,
  const MassMatrixType type,
  Eigen::SparseMatrix<Scalar>& M)
{
  using namespace Eigen;
  using namespace std;

  const int n = V.rows();
  const int m = F.rows();
  const int simplex_size = F.cols();

  MassMatrixType eff_type = type;
  // Use voronoi of for triangles by default, otherwise barycentric
  if(type == MASSMATRIX_TYPE_DEFAULT)
  {
    eff_type = (simplex_size == 3?MASSMATRIX_TYPE_VORONOI:MASSMATRIX_TYPE_BARYCENTRIC);
  }

  // Not yet supported
  assert(type!=MASSMATRIX_TYPE_FULL);

  if(simplex_size == 3)
  {
    // Triangles
    // edge lengths numbered same as opposite vertices
    Matrix<Scalar,Dynamic,3> l;
    igl::edge_lengths(V,F,l);
    return massmatrix_intrinsic(l,F,type,M);
  }else if(simplex_size == 4)
  {
    Matrix<int,Dynamic,1> MI;
    Matrix<int,Dynamic,1> MJ;
    Matrix<Scalar,Dynamic,1> MV;
    assert(V.cols() == 3);
    assert(eff_type == MASSMATRIX_TYPE_BARYCENTRIC);
    MI.resize(m*4,1); MJ.resize(m*4,1); MV.resize(m*4,1);
    MI.block(0*m,0,m,1) = F.col(0);
    MI.block(1*m,0,m,1) = F.col(1);
    MI.block(2*m,0,m,1) = F.col(2);
    MI.block(3*m,0,m,1) = F.col(3);
    MJ = MI;
    // loop over tets
    for(int i = 0;i<m;i++)
    {
      // http://en.wikipedia.org/wiki/Tetrahedron#Volume
      Matrix<Scalar,3,1> v0m3,v1m3,v2m3;
      v0m3.head(V.cols()) = V.row(F(i,0)) - V.row(F(i,3));
      v1m3.head(V.cols()) = V.row(F(i,1)) - V.row(F(i,3));
      v2m3.head(V.cols()) = V.row(F(i,2)) - V.row(F(i,3));
      Scalar v = fabs(v0m3.dot(v1m3.cross(v2m3)))/6.0;
      MV(i+0*m) = v/4.0;
      MV(i+1*m) = v/4.0;
      MV(i+2*m) = v/4.0;
      MV(i+3*m) = v/4.0;
    }
    sparse(MI,MJ,MV,n,n,M);
  }else
  {
    // Unsupported simplex size
    assert(false && "Unsupported simplex size");
  }
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
// generated by autoexplicit.sh
template void igl::massmatrix<Eigen::Matrix<double, -1, -1, 1, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, double>(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 1, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, igl::MassMatrixType, Eigen::SparseMatrix<double, 0, int>&);
// generated by autoexplicit.sh
template void igl::massmatrix<Eigen::Matrix<double, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 4, 0, -1, 4>, double>(Eigen::MatrixBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 4, 0, -1, 4> > const&, igl::MassMatrixType, Eigen::SparseMatrix<double, 0, int>&);
// generated by autoexplicit.sh
template void igl::massmatrix<Eigen::Matrix<double, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 3, 0, -1, 3>, double>(Eigen::MatrixBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 3, 0, -1, 3> > const&, igl::MassMatrixType, Eigen::SparseMatrix<double, 0, int>&);
template void igl::massmatrix<Eigen::Matrix<double, -1, 3, 1, -1, 3>, Eigen::Matrix<int, -1, 3, 1, -1, 3>, double>(Eigen::MatrixBase<Eigen::Matrix<double, -1, 3, 1, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 3, 1, -1, 3> > const&, igl::MassMatrixType, Eigen::SparseMatrix<double, 0, int>&);
template void igl::massmatrix<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, double>(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, igl::MassMatrixType, Eigen::SparseMatrix<double, 0, int>&);
#endif


#include "random"
#include "set"
#include "string"
#include "vector"

#define EIGEN_NO_STATIC_ASSERT 1

#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wdeprecated-register"
#include <Eigen/Core>
#include "/usr/local/include/g2o/core/base_binary_edge.h"
#include "/usr/local/include/g2o/core/base_unary_edge.h"
#include "/usr/local/include/g2o/core/block_solver.h"
#include "/usr/local/include/g2o/solvers/cholmod/linear_solver_cholmod.h"
#pragma clang diagnostic pop

#include "/usr/local/include/pcg_extras.hpp"
#include "/usr/local/include/pcg_random.hpp"

#include "types.hpp"

template class Eigen::Matrix<double, -1, -1, 1, -1, -1>;
template class Eigen::Matrix<double, 3, 1, 0, 3, 1>;
template class Eigen::Matrix<double, 3, 3, 0, 3, 3>;
template class Eigen::Matrix<double, 6, 1, 0, 6, 1>;
template class Eigen::Matrix<double, 6, 6, 0, 6, 6>;
template class g2o::BaseBinaryEdge<6, Eigen::Vector3d, class VertexPositionVelocity3D, class VertexPositionVelocity3D>;
template class g2o::BaseUnaryEdge<3, Eigen::Vector3d, class VertexPositionVelocity3D>;
template class g2o::BlockSolver<g2o::BlockSolverTraits<6, 6> >;
template class g2o::LinearSolverCholmod<g2o::BlockSolver<g2o::BlockSolverTraits<6, 6> >::PoseMatrixType>;
template class pcg_detail::engine<unsigned int, unsigned long, struct pcg_detail::xsh_rr_mixin<unsigned int, unsigned long>, true, class pcg_detail::specific_stream<unsigned long>, struct pcg_detail::default_multiplier<uint64_t> >;
template class pcg_extras::seed_seq_from<class std::random_device>;
template class std::basic_string<char>;
template class std::normal_distribution<double>;
template class std::set<class g2o::HyperGraph::Vertex *, struct std::less<class g2o::HyperGraph::Vertex *>, class std::allocator<class g2o::HyperGraph::Vertex *> >;


#include "math/util/prime.h"

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE( prime )

BOOST_AUTO_TEST_CASE( maybe_prime ) {
  BOOST_TEST_MESSAGE("  Testing " << math::NPRIMES << " primes");
  for (unsigned int i = 0; i < math::NPRIMES; ++i) {
    BOOST_CHECK(math::maybe_prime(math::primes[i]));
  }
  //Field::base_type maxVal = static_cast<Field::base_type>(Field::getSize());
  //BOOST_CHECK(math::maybe_prime(maxVal));
}
BOOST_AUTO_TEST_SUITE_END()

bool init_unit_test() {
  return true;
}


#include <EigenFaceVector.h>
#include <EigenFace.h>

#include <QBuffer>
#include <QDomElement>
#include <QIODevice>
#include <QStringList>

#include <Return.h>

#include <math.h>

/*
		EigenFaceVector	VectorId;
		int PersonKey, FaceKey;
		qreal Residual;
		QVector<qreal> coefficients;
*/

EigenFaceVector::EigenFaceVector(void)
{
	clear();
}

EigenFaceVector::EigenFaceVector(const QByteArray & ba)
{
	clear();
	EigenFaceVector efv;
	efv.read(ba);
	if (efv.isValid())
		*this = efv;
} // c'tor

void EigenFaceVector::clear(void)
{
	VectorKey = 0;
	PersonKey = FaceKey = 0;
	Residual = 0.0;
	coefficients.clear();
}

qreal EigenFaceVector::distance(const EigenFaceVector & other,
								 const int max,
								 QVector<qreal> weights)
{
	if ( ! isValid() || ! other.isValid())
		return -1.0;

	// TODO: Should this be calculated outside?
	int n = qMin(coefficients.size(), other.coefficients.size());
	if (max && max < n) n = max;
	if ( ! weights.isEmpty())
		n = qMin(n, weights.size());
	else
		weights.fill(1.0, n);

	qreal sumSquare = 0.0;
	for (int i = 0; i < n; ++i)
	{
		qreal d = coefficients[i] - other.coefficients[i];
		sumSquare += weights[i] * d * d;
	}
	return sqrt(sumSquare);
} // distance()

Return EigenFaceVector::read(const QDomElement & de)
{
	Return rtn;

	clear();

	int n = de.attribute("Count").toInt();
	if (n <= 0)
		return Return(EigenFace::ReturnNoData);

	VectorKey = de.attribute("VectorId").toInt();
	PersonKey = de.attribute("PersonKey").toInt();
	FaceKey = de.attribute("FaceKey").toInt();
	Residual = de.attribute("Residual").toDouble();

	QString txtString = de.text();
	QStringList coefs = txtString.simplified().split(" ", QString::SkipEmptyParts);
	if (n != coefs.size())
		return Return(EigenFace::ReturnInvalidStructure, "Bad Count");
	coefficients.fill(0.0, n);
	for (int i = 0; i < n; ++i)
		coefficients[i] = coefs[i].toDouble();

	return rtn;
} // read()

Return EigenFaceVector::write(QDomElement * de) const
{
	if ( ! isValid())
		return Return(EigenFace::ReturnInvalidStructure, "Vector");

	int n = coefficients.size();
	de->setAttribute("Count", n);
	de->setAttribute("VectorId", VectorKey);
	de->setAttribute("PersonKey", PersonKey);
	de->setAttribute("FaceKey", FaceKey);
	de->setAttribute("Residual", Residual);

	QString txtString;
	txtString.reserve(n * 16);
	for (int x = 0; x < n; ++x)
	{
		if (0 == x % 8)
			txtString.append("\n");
		txtString.append(QString::number(coefficients.at(x), 'f', 12) + " ");
	}

	QDomText txtNode = de->ownerDocument().createTextNode(txtString);
	de->appendChild(txtNode);

	return Return();
} // write(de)

Return EigenFaceVector::write(QByteArray * ba) const
{
	Return rtn;

	if ( ! ba)
		return Return(EigenFace::ReturnStatusNull);
	if ( ! isValid())
		return Return(EigenFace::ReturnInvalidStructure, "Vector");

	QBuffer buf;
	rtn = write(&buf);
	if (rtn.isError())
		return rtn;

	*ba = buf.data();
	return Return();
} // write(ba)

Return EigenFaceVector::write(QIODevice * io) const
{
	if ( ! isValid())
		return Return(EigenFace::ReturnInvalidStructure, "Vector");

	// write stream id
	int i = StreamId;
	io->write((char *)&i, sizeof(i));

	// write basic data
	io->write((char *)&VectorKey, sizeof(VectorKey));
	io->write((char *)&PersonKey, sizeof(PersonKey));
	io->write((char *)&FaceKey, sizeof(FaceKey));
	io->write((char *)&Residual, sizeof(Residual));

	i = coefficients.size();
	io->write((char *)&i, sizeof(i));
	if (i * sizeof(qreal) != io->write((char *)coefficients.data(), i * sizeof(qreal)))
		return Return::errorString(io->errorString());

	return Return();
} // write(io)

Return EigenFaceVector::read(const QByteArray & ba)
{
	QBuffer buf;
	buf.setData(ba);
	return read(&buf);
}

Return EigenFaceVector::read(QIODevice * io)
{
	int i;

	clear();

	// Check Stream Id
	io->read((char *)&i, sizeof(i));
	if (StreamId != i)
		return Return(EigenFace::ReturnInvalidStructure, "EFV:StreamId");

	// Read basic data
	io->read((char *)&VectorKey, sizeof(VectorKey));
	io->read((char *)&PersonKey, sizeof(PersonKey));
	io->read((char *)&FaceKey, sizeof(FaceKey));
	io->read((char *)&Residual, sizeof(Residual));

	// Read data vector
	io->read((char *)&i, sizeof(i));
	coefficients.fill(i, 0.0);
	if (i * sizeof(qreal) != io->read((char *)coefficients.data(), i * sizeof(qreal)))
		return Return::errorString(io->errorString());

	if (isValid())
		return Return();
	else
		return Return(EigenFace::ReturnInvalidStructure, "MaskedArray");
} // read(io)


#include "QPCommon.h"
#include <Eigen/StdVector>
#include "drakeUtil.h"
#include "controlUtil.h"

using namespace std;

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
	if (nrhs<1) mexErrMsgTxt("usage: mask=getActiveSupportsmex(ptr,x,support_data,contact_sensor,contact_threshold,terrain_height))");
	if (nlhs<1) mexErrMsgTxt("please take at least one output");

	const mxArray* pm;

	// first get the ptr back from matlab
  NewQPControllerData *pdata = (NewQPControllerData*) getDrakeMexPointer(prhs[0]);

  int nq = pdata->r->num_positions;
  int nv = pdata->r->num_velocities;

  int narg=1;
  double *q_ptr = mxGetPrSafe(prhs[narg]);
  double *qd_ptr = &q_ptr[nq];
  Map<VectorXd> q(q_ptr, nq);
  Map<VectorXd> qd(qd_ptr, nv);
  narg++;

  const mxArray* supp_data = prhs[narg];
  narg++;

  pm = prhs[narg];
  Map<VectorXd> contact_force_detected(mxGetPrSafe(pm), mxGetNumberOfElements(pm), 1);
  narg++;

  pm = prhs[narg];
  double contact_threshold = mxGetScalar(pm);
  narg++;

  pm = prhs[narg];
  double terrain_height = mxGetScalar(pm);
  narg++;

  Matrix<bool, Dynamic, 1> b_contact_force_detected = Matrix<bool, Dynamic, 1>::Zero(contact_force_detected.size());
  for (int i = 0; i < contact_force_detected.size(); i++) {
    b_contact_force_detected(i) = (contact_force_detected(i) != 0);
  }

  vector<SupportStateElement,Eigen::aligned_allocator<SupportStateElement>> available_supports = parseSupportData(supp_data);
  Matrix<bool, Dynamic, 1> b_mask = getActiveSupportMask(pdata->r, pdata->map_ptr, q, qd, available_supports, b_contact_force_detected, contact_threshold, terrain_height);

  VectorXd active_supp_mask = VectorXd::Zero(b_mask.size());
  for (int i = 0; i < b_mask.size(); i++) {
    if (b_mask(i)) {
      active_supp_mask(i) = 1.0;
    }
  }
  plhs[0] = eigenToMatlab(active_supp_mask);
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file SubgraphPreconditioner.cpp
 * @date Dec 31, 2009
 * @author: Frank Dellaert
 */

#include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <boost/foreach.hpp>

using namespace std;

namespace gtsam {

  /* ************************************************************************* */
  static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) {
    GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph());
    BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg) {
      JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf);
      if( !jf ) {
        jf = boost::make_shared<JacobianFactor>(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
      }
      result->push_back(jf);
    }
    return result;
  }

  /* ************************************************************************* */
  SubgraphPreconditioner::SubgraphPreconditioner(const sharedFG& Ab2,
      const sharedBayesNet& Rc1, const sharedValues& xbar) :
    Ab2_(convertToJacobianFactors(*Ab2)), Rc1_(Rc1), xbar_(xbar), b2bar_(new Errors(-Ab2_->gaussianErrors(*xbar))) {
  }

  /* ************************************************************************* */
  // x = xbar + inv(R1)*y
  VectorValues SubgraphPreconditioner::x(const VectorValues& y) const {
    return *xbar_ + Rc1_->backSubstitute(y);
  }

  /* ************************************************************************* */
  double SubgraphPreconditioner::error(const VectorValues& y) const {
    Errors e(y);
    VectorValues x = this->x(y);
    Errors e2 = Ab2()->gaussianErrors(x);
    return 0.5 * (dot(e, e) + dot(e2,e2));
  }

  /* ************************************************************************* */
  // gradient is y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar),
  VectorValues SubgraphPreconditioner::gradient(const VectorValues& y) const {
    VectorValues x = Rc1()->backSubstitute(y); /* inv(R1)*y */
    Errors e = (*Ab2()*x - *b2bar());               /* (A2*inv(R1)*y-b2bar) */
    VectorValues v = VectorValues::Zero(x);
    Ab2()->transposeMultiplyAdd(1.0, e, v);           /* A2'*(A2*inv(R1)*y-b2bar) */
    return y + Rc1()->backSubstituteTranspose(v);
  }

  /* ************************************************************************* */
  // Apply operator A, A*y = [I;A2*inv(R1)]*y = [y; A2*inv(R1)*y]
  Errors SubgraphPreconditioner::operator*(const VectorValues& y) const {

    Errors e(y);
    VectorValues x = Rc1()->backSubstitute(y);   /* x=inv(R1)*y */
    Errors e2 = *Ab2() * x;                              /* A2*x */
    e.splice(e.end(), e2);
    return e;
  }

  /* ************************************************************************* */
  // In-place version that overwrites e
  void SubgraphPreconditioner::multiplyInPlace(const VectorValues& y, Errors& e) const {

    Errors::iterator ei = e.begin();
    for ( Key i = 0 ; i < y.size() ; ++i, ++ei ) {
      *ei = y[i];
    }

    // Add A2 contribution
    VectorValues x = Rc1()->backSubstitute(y);      // x=inv(R1)*y
    Ab2()->multiplyInPlace(x, ei);                  // use iterator version
  }

  /* ************************************************************************* */
  // Apply operator A', A'*e = [I inv(R1')*A2']*e = e1 + inv(R1')*A2'*e2
  VectorValues SubgraphPreconditioner::operator^(const Errors& e) const {

    Errors::const_iterator it = e.begin();
    VectorValues y = zero();
    for ( Key i = 0 ; i < y.size() ; ++i, ++it )
      y[i] = *it ;
    transposeMultiplyAdd2(1.0,it,e.end(),y);
    return y;
  }

  /* ************************************************************************* */
  // y += alpha*A'*e
  void SubgraphPreconditioner::transposeMultiplyAdd
    (double alpha, const Errors& e, VectorValues& y) const {

    Errors::const_iterator it = e.begin();
    for ( Key i = 0 ; i < y.size() ; ++i, ++it ) {
      const Vector& ei = *it;
      axpy(alpha, ei, y[i]);
    }
    transposeMultiplyAdd2(alpha, it, e.end(), y);
  }

  /* ************************************************************************* */
  // y += alpha*inv(R1')*A2'*e2
  void SubgraphPreconditioner::transposeMultiplyAdd2 (double alpha,
    Errors::const_iterator it, Errors::const_iterator end, VectorValues& y) const {

    // create e2 with what's left of e
    // TODO can we avoid creating e2 by passing iterator to transposeMultiplyAdd ?
    Errors e2;
    while (it != end) e2.push_back(*(it++));

    VectorValues x = VectorValues::Zero(y); // x = 0
    Ab2_->transposeMultiplyAdd(1.0,e2,x);   // x += A2'*e2
    axpy(alpha, Rc1_->backSubstituteTranspose(x), y); // y += alpha*inv(R1')*x
  }

  /* ************************************************************************* */
  void SubgraphPreconditioner::print(const std::string& s) const {
    cout << s << endl;
    Ab2_->print();
  }
} // nsamespace gtsam


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2018 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#include "graph_filtering.hh"

#include <boost/python.hpp>

#include "graph.hh"
#include "histogram.hh"
#include "graph_selectors.hh"
#include "graph_properties.hh"

#include "graph_corr_hist.hh"

using namespace std;
using namespace boost;
using namespace graph_tool;

typedef UnityPropertyMap<int,GraphInterface::edge_t> dummy_weight;

boost::python::object
get_vertex_combined_correlation_histogram(GraphInterface& gi,
                                          GraphInterface::deg_t deg1,
                                          GraphInterface::deg_t deg2,
                                          const vector<long double>& xbin,
                                          const vector<long double>& ybin)
{
    boost::python::object hist;
    boost::python::object ret_bins;

    std::array<vector<long double>,2> bins;
    bins[0] = xbin;
    bins[1] = ybin;

    run_action<>()(gi, get_correlation_histogram<GetCombinedPair>(hist, bins,
                                                                  ret_bins),
                   scalar_selectors(), scalar_selectors(),
                   boost::mpl::vector<dummy_weight>())
        (any(degree_selector(deg1)), any(degree_selector(deg2)),
         boost::any(dummy_weight()));

    return boost::python::make_tuple(hist, ret_bins);
}

using namespace boost::python;

void export_combined_vertex_correlations()
{
    def("vertex_combined_correlation_histogram",
        &get_vertex_combined_correlation_histogram);
}


#include "MexPackUnpack.h"
#include "mex.h"
#include <Eigen>
#include <typeindex>
#include <map>

using namespace MexPackUnpackTypes;
struct userStruct{
  int a;
  double b;
  EDRM c;
  std::string d;
};

struct userStruct2{
  int a;
  double b;
  std::string c;
  std::vector<userStruct> d;
};

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {

  MexUnpacker<std::vector<userStruct>> my_unpack(nrhs, prhs);

  try {

    auto [d] = my_unpack.unpackMex();

    std::map<std::type_index,std::vector<std::string> > my_name_map;

    my_name_map[typeid(userStruct)] = {"my_int","my_double","my_array", "my_string"};
    my_name_map[typeid(userStruct2)] = {"my_int","my_double", "my_string","my_vector"};

    MexPacker<userStruct2>my_pack(nlhs, plhs,my_name_map);

    userStruct2 output_struct{1,4.5,"doc",d};

    my_pack.PackMex(output_struct);

  } catch (std::string s) {
    mexPrintf(s.data());
  }

}


/**
 * Example use of boost::astar_search_no_init on an infinite, implicitly-defined graph.
 *
 * The graph type used here is XYGraph, representing an infinite grid of squares.  Each
 * square is connected to its eight neighbors; however, the example shows how to use
 * boost::filtered_graph to make the search take place only along orthogonal edges.
 */

#include <iostream>
#include <list>
#include <map>
#include <set>
#include <utility>

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/astar_search.hpp>
#include <boost/graph/filtered_graph.hpp>
#include <boost/operators.hpp>
#include <boost/ref.hpp>

namespace Direction
{
enum id
{
    MIN = 0,
    N = MIN, S, E, W, NW, NE, SE, SW, NONE
};
}

struct XY : public boost::additive<XY,
    boost::totally_ordered<XY,
    boost::equivalent<XY>
    > >
{
    typedef int X;
    typedef int Y;

    XY(X x = 0, Y y = 0);

    // Same square counts.
    bool adjacentTo(XY const& that) const;

    XY & operator=(XY const& that);
    XY & operator+=(XY const& that);

    bool operator<(XY const& that) const;

    X x;
    Y y;

    XY neighbor(Direction::id direction) const;
    std::set<XY> allNeighbors() const;
};

std::ostream & operator<<(std::ostream & os, XY const& xy);

struct neighbor_iterator;

/*
 * Model of:
 *  * Graph
 *  * IncidenceGraph
 */
struct XYGraph
{
    XYGraph();

    // Graph concept requirements
    typedef XY                                vertex_descriptor;
    typedef std::pair<XY, XY>                 edge_descriptor;
    typedef boost::undirected_tag             directed_category;
    typedef boost::disallow_parallel_edge_tag edge_parallel_category;
    typedef boost::incidence_graph_tag        traversal_category;

    // IncidenceGraph concept requirements
    typedef neighbor_iterator          out_edge_iterator;
    typedef int                        degree_size_type;
};

namespace boost
{
    template <> struct graph_traits<XYGraph>
    {
        typedef XYGraph G;

        typedef G::vertex_descriptor      vertex_descriptor;
        typedef G::edge_descriptor        edge_descriptor;
        typedef G::out_edge_iterator      out_edge_iterator;

        typedef G::directed_category      directed_category;
        typedef G::edge_parallel_category edge_parallel_category;
        typedef G::traversal_category     traversal_category;

        typedef G::degree_size_type       degree_size_type;

        typedef void in_edge_iterator;
        typedef void vertex_iterator;
        typedef void vertices_size_type;
        typedef void edge_iterator;
        typedef void edges_size_type;
    };
}

// IncidenceGraph concept requirements
std::pair<XYGraph::out_edge_iterator,
XYGraph::out_edge_iterator> out_edges(XYGraph::vertex_descriptor v, XYGraph const& g);
XYGraph::degree_size_type out_degree(XYGraph::vertex_descriptor v, XYGraph const& g);
XYGraph::vertex_descriptor source(XYGraph::edge_descriptor e, XYGraph const& g);
XYGraph::vertex_descriptor target(XYGraph::edge_descriptor e, XYGraph const& g);

// Iterator
struct neighbor_iterator :
    public boost::iterator_facade<neighbor_iterator,
                                  std::pair<XY,XY>,
                                  boost::forward_traversal_tag,
                                  std::pair<XY,XY> >
{
public:
    neighbor_iterator();
    neighbor_iterator(XY xy, Direction::id direction);

    neighbor_iterator & operator=(neighbor_iterator const& that);

    std::pair<XY,XY> operator*() const;
    void operator++();
    bool operator==(neighbor_iterator const& that) const;

    bool equal(neighbor_iterator const& that) const { return operator==(that); }
    void increment() { operator++(); }

private:
    XY xy;
    Direction::id direction;
};

struct orthogonal_only;
template <typename Graph> class distance_heuristic;

struct found_goal {}; // exception for termination

// visitor that terminates when we find the goal
class astar_goal_visitor : public boost::default_astar_visitor
{
public:
    astar_goal_visitor(XY goal) : m_goal(goal) {}

    virtual void examine_vertex(XY xy, XYGraph const& g) {
        std::cout << "Exploring " << xy << "..." << std::endl;
        if(xy == m_goal)
            throw found_goal();
    }
    virtual void examine_vertex(XY xy, boost::filtered_graph<XYGraph, orthogonal_only> const& g) {
        std::cout << "Exploring " << xy << "..." << std::endl;
        if(xy == m_goal)
            throw found_goal();
    }
private:
    XY m_goal;
};

template <typename K, typename V>
class default_map
{
public:
    typedef K key_type;
    typedef V data_type;
    typedef std::pair<K,V> value_type;

    default_map(V const& defaultValue)
        : defaultValue(defaultValue)
    {}

    V & operator[](K const& k)
    {
        if (m.find(k) == m.end())
        {
            m[k] = defaultValue;
        }
        return m[k];
    }

private:
    std::map<K,V> m;
    V const defaultValue;
};

struct PredecessorMap
{
    PredecessorMap() {}
    PredecessorMap(PredecessorMap const& that) : m(that.m) {}

    typedef XY key_type;
    typedef XY value_type;
    typedef XY & reference_type;
    typedef boost::read_write_property_map_tag category;

    XY & operator[](XY xy) { return m[xy]; }

    std::map<XY,XY> m;
};

XY get(PredecessorMap const& pm, XY xy)
{
    std::map<XY,XY>::const_iterator found = pm.m.find(xy);
    return (found != pm.m.end()) ? found->second : xy;
}

void put(PredecessorMap & pm, XY key, XY value)
{
    pm.m[key] = value;
}

// Filter used to traverse grid only along orthogonal (non-diagonal) edges.
struct orthogonal_only
{
    typedef std::pair<XY,XY> Edge;
    bool operator()(Edge const& edge) const
    {
        return edge.first.x == edge.second.x || edge.first.y == edge.second.y;
    }
};

// Euclidean distance heuristic (square root omitted)
template <typename Graph>
class distance_heuristic : public boost::astar_heuristic<Graph, int>
{
public:
    distance_heuristic(XY goal)
        : m_goal(goal) {}
    unsigned operator()(XY xy)
    {
        int dx = m_goal.x - xy.x;
        int dy = m_goal.y - xy.y;
        unsigned retval = static_cast<unsigned>(dx * dx + dy * dy);
        return retval;
    }
private:
    XY m_goal;
};

int main(int argc, char **argv)
{
    XYGraph baseGraph;
    boost::filtered_graph<XYGraph, orthogonal_only> g(baseGraph, orthogonal_only());
    //BOOST_CONCEPT_ASSERT((IncidenceGraphConcept< boost::filtered_graph<XYGraph, orthogonal_only> >));

    XY start(0,0);
    XY goal(5,7);

    std::cout << "Start vertex: " << start << std::endl;
    std::cout << "Goal vertex: " << goal << std::endl;

    PredecessorMap p;
    typedef boost::associative_property_map< default_map<XY,unsigned> > DistanceMap;
    typedef default_map<XY,unsigned> WrappedDistanceMap;
    WrappedDistanceMap wrappedMap =         WrappedDistanceMap(std::numeric_limits<unsigned>::max());
    wrappedMap[start] = 0;
    DistanceMap d = DistanceMap(wrappedMap);

    try {
        astar_search_no_init(g,
            start,
            distance_heuristic<XYGraph>(goal)
            , visitor(astar_goal_visitor(goal))
            . distance_map(d)
            . predecessor_map(boost::ref(p))
            . weight_map(boost::associative_property_map< default_map<std::pair<XY,XY>,unsigned> >(
                default_map<std::pair<XY,XY>,unsigned>(1)))
            . vertex_index_map(boost::associative_property_map< std::map<XY,unsigned> >(std::map<XY,unsigned>()))
            . rank_map(boost::associative_property_map< std::map<XY,unsigned> >(std::map<XY,unsigned>()))
            . color_map(boost::associative_property_map< std::map<XY,boost::default_color_type> >(
                std::map<XY,boost::default_color_type>()))
            . distance_compare(std::less<unsigned>())
            . distance_combine(std::plus<unsigned>())
            );
    } catch(found_goal const&) { // found a path to the goal
        std::list<XY> shortest_path;
        for(XY xy = goal;; xy = p[xy]) {
            shortest_path.push_front(xy);
            if(p[xy] == xy)
                break;
        }
        std::cout << "Shortest path from " << start << " to "
            << goal << ": ";
        std::list<XY>::iterator spi = shortest_path.begin();
        std::cout << start;
        for(++spi; spi != shortest_path.end(); ++spi)
            std::cout << " -> " << (*spi);
        std::cout << std::endl;
        return 0;
    }

    std::cout << "Didn't find a path from " << start << "to"
        << goal << "!" << std::endl;
    return 0;
}

XYGraph::XYGraph()
{}

std::pair<XYGraph::out_edge_iterator, XYGraph::out_edge_iterator>
out_edges(XYGraph::vertex_descriptor v,
          XYGraph const& g)
{
    return std::make_pair(
        XYGraph::out_edge_iterator(v, Direction::MIN),
        XYGraph::out_edge_iterator(v, Direction::NONE) );
}

XYGraph::degree_size_type
out_degree(XYGraph::vertex_descriptor v,
           XYGraph const& g)
{
    return v.allNeighbors().size();
}

XYGraph::vertex_descriptor
source(XYGraph::edge_descriptor e,
       XYGraph const& g)
{
    return e.first;
}

XYGraph::vertex_descriptor target(
    XYGraph::edge_descriptor e,
    XYGraph const& g)
{
    return e.second;
}

neighbor_iterator::neighbor_iterator()
{}

neighbor_iterator::neighbor_iterator(XY xy, Direction::id direction)
: xy(xy)
, direction(direction)
{
}

neighbor_iterator & neighbor_iterator::operator=(neighbor_iterator const& that)
{
    xy = that.xy;
    direction = that.direction;
    return *this;
}

std::pair<XY,XY> neighbor_iterator::operator*() const
{
    std::pair<XY,XY> const retval = std::make_pair(xy, xy.neighbor(direction));
    return retval;
}

void neighbor_iterator::operator++()
{
    direction = static_cast<Direction::id>(int(direction) + 1);
}

bool neighbor_iterator::operator==(neighbor_iterator const& that) const
{
    return xy == that.xy && direction == that.direction;
}

XY::XY(X x, Y y)
: x(x)
, y(y)
{
}

bool XY::adjacentTo(XY const& that) const
{
    return abs(x - that.x) <= 1 && abs(y - that.y) <= 1;
}

XY & XY::operator=(XY const& that)
{
    x = that.x;
    y = that.y;
    return *this;
}

XY & XY::operator+=(XY const& that)
{
    x += that.x;
    y += that.y;
    return *this;
}

bool XY::operator<(XY const& that) const
{
    return x < that.x || (x == that.x && y < that.y);
}

std::ostream & operator<<(std::ostream & os, XY const& xy)
{
    os << "(" << xy.x << "," << xy.y << ")";
    return os;
}

XY XY::neighbor(Direction::id direction) const
{
    using namespace Direction;

    int dx = 0, dy = 0;
    switch (direction)
    {
    case NW:
    case W:
    case SW:
        dx = -1;
        break;
    case NE:
    case E:
    case SE:
        dx = 1;
    }
    switch (direction)
    {
    case NW:
    case N:
    case NE:
        dy = -1;
        break;
    case SW:
    case S:
    case SE:
        dy = 1;
    }
    XY const neighbor(x + dx, y + dy);
    return neighbor;
}

std::set<XY> XY::allNeighbors() const
{
    std::set<XY> neighbors;

    for (int dx = -1; dx <= 1; ++dx)
        for (int dy = -1; dy <= 1; ++dy)
            neighbors.insert(XY(x+dx,y+dy));

    return neighbors;
}


#include <iostream>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/core/core.hpp>
#include <boost/timer.hpp>
#include <algorithm>

#include "core/Config.h"
#include "core/Feature.h"
#include "core/Featuredetection.h"
#include "core/Tracker.h"
#include "io/Zed_svo_wrapper.h"
#include "utils/SLAMTools.h"

using namespace std;

int main ( int argc, char** argv )
{
    if ( argc != 3 )
    {
        cout<<"usage: test_semi_direct_vo_g2o config_file svo_file"<<endl;
        return -1;
    }

    string path_to_config ( argv[1] );
    string svo_file ( argv[2] );

    zed_slam::Config::init ( path_to_config );
    zed_slam::Zed_svo_wrapper* svo_wrapper = new zed_slam::Zed_svo_wrapper ( svo_file );

    // remove the first frames where the camera does not move
    for (int i=0; i<40; i++)
    {
        svo_wrapper->next();
    }

    zed_slam::Frame::Ptr frame1 = svo_wrapper->next();
    if ( frame1 == nullptr )
        return -1;

    zed_slam::FeatureDetection featureDetection;
    featureDetection.detectFAST ( frame1 );

    zed_slam::FrameDrawer viewer;
    zed_slam::Tracker tracker;
    while (1)
    {
        zed_slam::Frame::Ptr frame2 = svo_wrapper->next();
        // set frame 2 same as frame 1
        frame2->T_f_w_ = frame1->T_f_w_;
        if (frame2 == nullptr)
            break;
        tracker.poseEstimationTwoFramesSemiDirectUsingG2O( frame1, frame2 );

        featureDetection.detectFAST( frame2, true );
        frame1 = frame2;
        viewer.addFrame(frame2);

        cv::imshow("image", frame2->color_);
        cv::waitKey(1);
    }
    viewer.stop();

    cv::waitKey(0);
    return 0;
}


/**
 * @file random_forest_test.cpp
 * @author Manish Kumar
 *
 * Test mlpackMain() of random_forest_main.cpp.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#define BINDING_TYPE BINDING_TYPE_TEST

#include <mlpack/core.hpp>
static const std::string testName = "RandomForest";

#include <mlpack/core/util/mlpack_main.hpp>
#include <mlpack/methods/random_forest/random_forest_main.cpp>
#include "test_helper.hpp"

#include <boost/test/unit_test.hpp>
#include "../test_tools.hpp"

using namespace mlpack;

struct RandomForestTestFixture
{
 public:
  RandomForestTestFixture()
  {
    // Cache in the options for this program.
    CLI::RestoreSettings(testName);
  }

  ~RandomForestTestFixture()
  {
    // Clear the settings.
    bindings::tests::CleanMemory();
    CLI::ClearSettings();
  }
};

BOOST_FIXTURE_TEST_SUITE(RandomForestMainTest, RandomForestTestFixture);

/**
 * Check that number of output points and number of input
 * points are equal and have appropriate number of classes.
 */
BOOST_AUTO_TEST_CASE(RandomForestOutputDimensionTest)
{
  arma::mat inputData;
  if (!data::Load("vc2.csv", inputData))
    BOOST_FAIL("Cannot load train dataset vc2.csv!");

  arma::Row<size_t> labels;
  if (!data::Load("vc2_labels.txt", labels))
    BOOST_FAIL("Cannot load labels for vc2_labels.txt");

  arma::mat testData;
  if (!data::Load("vc2_test.csv", testData))
    BOOST_FAIL("Cannot load test dataset vc2.csv!");

  size_t testSize = testData.n_cols;

  // Input training data.
  SetInputParam("training", std::move(inputData));
  SetInputParam("labels", std::move(labels));

  // Input test data.
  SetInputParam("test", std::move(testData));

  mlpackMain();

  // Check that number of output points are equal to number of input points.
  BOOST_REQUIRE_EQUAL(CLI::GetParam<arma::Row<size_t>>("predictions").n_cols,
                      testSize);
  BOOST_REQUIRE_EQUAL(CLI::GetParam<arma::mat>("probabilities").n_cols,
                      testSize);

  // Check number of output rows equals number of classes in case of
  // probabilities and 1 for predictions.
  BOOST_REQUIRE_EQUAL(CLI::GetParam<arma::Row<size_t>>("predictions").n_rows,
                      1);
  BOOST_REQUIRE_EQUAL(CLI::GetParam<arma::mat>("probabilities").n_rows, 3);
}

/**
 * Ensure that saved model can be used again.
 */
BOOST_AUTO_TEST_CASE(RandomForestModelReuseTest)
{
  arma::mat inputData;
  if (!data::Load("vc2.csv", inputData))
    BOOST_FAIL("Cannot load train dataset vc2.csv!");

  arma::Row<size_t> labels;
  if (!data::Load("vc2_labels.txt", labels))
    BOOST_FAIL("Cannot load labels for vc2_labels.txt");

  arma::mat testData;
  if (!data::Load("vc2_test.csv", testData))
    BOOST_FAIL("Cannot load test dataset vc2.csv!");

  size_t testSize = testData.n_cols;

  // Input training data.
  SetInputParam("training", std::move(inputData));
  SetInputParam("labels", std::move(labels));

  // Input test data.
  SetInputParam("test", testData);

  mlpackMain();

  arma::Row<size_t> predictions;
  arma::mat probabilities;
  predictions = std::move(CLI::GetParam<arma::Row<size_t>>("predictions"));
  probabilities = std::move(CLI::GetParam<arma::mat>("probabilities"));

  // Reset passed parameters.
  CLI::GetSingleton().Parameters()["training"].wasPassed = false;
  CLI::GetSingleton().Parameters()["labels"].wasPassed = false;
  CLI::GetSingleton().Parameters()["test"].wasPassed = false;

  // Input trained model.
  SetInputParam("test", std::move(testData));
  SetInputParam("input_model",
                CLI::GetParam<RandomForestModel*>("output_model"));

  mlpackMain();

  // Check that number of output points are equal to number of input points.
  BOOST_REQUIRE_EQUAL(CLI::GetParam<arma::Row<size_t>>("predictions").n_cols,
                      testSize);
  BOOST_REQUIRE_EQUAL(CLI::GetParam<arma::mat>("probabilities").n_cols,
                      testSize);

  // Check number of output rows equals number of classes in case of
  // probabilities and 1 for predicitions.
  BOOST_REQUIRE_EQUAL(CLI::GetParam<arma::Row<size_t>>("predictions").n_rows,
                      1);
  BOOST_REQUIRE_EQUAL(CLI::GetParam<arma::mat>("probabilities").n_rows, 3);

  // Check that initial predictions and predictions using saved model are same.
  CheckMatrices(predictions, CLI::GetParam<arma::Row<size_t>>("predictions"));
  CheckMatrices(probabilities, CLI::GetParam<arma::mat>("probabilities"));
}

/**
 * Make sure number of trees specified is always a positive number.
 */
BOOST_AUTO_TEST_CASE(RandomForestNumOfTreesTest)
{
  arma::mat inputData;
  if (!data::Load("vc2.csv", inputData))
    BOOST_FAIL("Cannot load train dataset vc2.csv!");

  arma::Row<size_t> labels;
  if (!data::Load("vc2_labels.txt", labels))
    BOOST_FAIL("Cannot load labels for vc2_labels.txt");

  SetInputParam("num_trees", (int) 0); // Invalid.

  Log::Fatal.ignoreInput = true;
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);
  Log::Fatal.ignoreInput = false;
}

/**
 * Make sure minimum leaf size specified is always a positive number.
 */
BOOST_AUTO_TEST_CASE(RandomForestMinimumLeafSizeTest)
{
  arma::mat inputData;
  if (!data::Load("vc2.csv", inputData))
    BOOST_FAIL("Cannot load train dataset vc2.csv!");

  arma::Row<size_t> labels;
  if (!data::Load("vc2_labels.txt", labels))
    BOOST_FAIL("Cannot load labels for vc2_labels.txt");

  SetInputParam("minimum_leaf_size", (int) 0); // Invalid.

  Log::Fatal.ignoreInput = true;
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);
  Log::Fatal.ignoreInput = false;
}

/**
 * Make sure only one of training data or pre-trained model is passed.
 */
BOOST_AUTO_TEST_CASE(RandomForestTrainingVerTest)
{
  arma::mat inputData;
  if (!data::Load("vc2.csv", inputData))
    BOOST_FAIL("Cannot load train dataset vc2.csv!");

  arma::Row<size_t> labels;
  if (!data::Load("vc2_labels.txt", labels))
    BOOST_FAIL("Cannot load labels for vc2_labels.txt");

  // Input training data.
  SetInputParam("training", std::move(inputData));
  SetInputParam("labels", std::move(labels));

  mlpackMain();

  // Input pre-trained model.
  SetInputParam("input_model",
                CLI::GetParam<RandomForestModel*>("output_model"));

  Log::Fatal.ignoreInput = true;
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);
  Log::Fatal.ignoreInput = false;
}

/**
 * Ensure that training accuracy changes as minimum_leaf_size decreases.
 */
BOOST_AUTO_TEST_CASE(RandomForestDiffMinLeafSizeTest)
{
  // Train for minimum leaf size 20.
  arma::mat inputData;
  if (!data::Load("vc2.csv", inputData))
    BOOST_FAIL("Cannot load train dataset vc2.csv!");

  arma::Row<size_t> labels;
  if (!data::Load("vc2_labels.txt", labels))
    BOOST_FAIL("Cannot load labels for vc2_labels.txt");

  bool success = false;
  for (size_t trial = 0; trial < 5; ++trial)
  {
    // Input training data.
    SetInputParam("training", inputData);
    SetInputParam("labels", labels);
    SetInputParam("minimum_leaf_size", (int) 20);

    mlpackMain();

    // Calculate training accuracy.
    arma::Row<size_t> predictions;
    CLI::GetParam<RandomForestModel*>("output_model")->rf.Classify(inputData,
        predictions);

    size_t correct = arma::accu(predictions == labels);
    double accuracy20 = (double(correct) / double(labels.n_elem) * 100);

    bindings::tests::CleanMemory();

    // Train for minimum leaf size 10.

    // Input training data.
    SetInputParam("training", inputData);
    SetInputParam("labels", labels);
    SetInputParam("minimum_leaf_size", (int) 10);

    mlpackMain();

    // Calculate training accuracy.
    CLI::GetParam<RandomForestModel*>("output_model")->rf.Classify(inputData,
         predictions);

    correct = arma::accu(predictions == labels);
    double accuracy10 = (double(correct) / double(labels.n_elem) * 100);

    bindings::tests::CleanMemory();

    // Train for minimum leaf size 1.

    // Input training data.
    SetInputParam("training", inputData);
    SetInputParam("labels", labels);
    SetInputParam("minimum_leaf_size", (int) 1);

    mlpackMain();

    // Calculate training accuracy.
    CLI::GetParam<RandomForestModel*>("output_model")->rf.Classify(inputData,
         predictions);

    correct = arma::accu(predictions == labels);
    double accuracy1 = (double(correct) / double(labels.n_elem) * 100);

    success = (accuracy1 != accuracy10 && accuracy10 != accuracy20);
    if (success)
      break;
  }

  BOOST_REQUIRE_EQUAL(success, true);
}

/**
 * Ensure that test accuracy changes as num_trees increases (we aren't
 * guaranteed that accuracy will go up).
 */
BOOST_AUTO_TEST_CASE(RandomForestDiffNumTreeTest)
{
  // Train for num_trees 1.
  arma::mat inputData;
  if (!data::Load("vc2.csv", inputData))
    BOOST_FAIL("Cannot load train dataset vc2.csv!");

  arma::Row<size_t> labels;
  if (!data::Load("vc2_labels.txt", labels))
    BOOST_FAIL("Cannot load labels for vc2_labels.txt");

  arma::mat testData;
  if (!data::Load("vc2_test.csv", testData))
    BOOST_FAIL("Cannot load test dataset vc2_test.csv!");

  arma::Row<size_t> testLabels;
  if (!data::Load("vc2_test_labels.txt", testLabels))
    BOOST_FAIL("Cannot load labels for vc2__test_labels.txt");

  bool success = false;
  for (size_t trial = 0; trial < 3; ++trial)
  {
    // Input training data.
    SetInputParam("training", inputData);
    SetInputParam("labels", labels);
    SetInputParam("num_trees", (int) 1);
    SetInputParam("minimum_leaf_size", (int) 1);

    mlpackMain();

    // Calculate training accuracy.
    arma::Row<size_t> predictions;
    CLI::GetParam<RandomForestModel*>("output_model")->rf.Classify(testData,
         predictions);
    bindings::tests::CleanMemory();

    size_t correct = arma::accu(predictions == testLabels);
    double accuracy1 = (double(correct) / double(testLabels.n_elem) * 100);

    // Train for num_trees 5.

    // Input training data.
    SetInputParam("training", inputData);
    SetInputParam("labels", labels);
    SetInputParam("num_trees", (int) 5);
    SetInputParam("minimum_leaf_size", (int) 1);

    mlpackMain();

    // Calculate training accuracy.
    CLI::GetParam<RandomForestModel*>("output_model")->rf.Classify(testData,
         predictions);
    bindings::tests::CleanMemory();

    correct = arma::accu(predictions == testLabels);
    double accuracy5 = (double(correct) / double(testLabels.n_elem) * 100);

    // Train for num_trees 10.

    // Input training data.
    SetInputParam("training", inputData);
    SetInputParam("labels", labels);
    SetInputParam("num_trees", (int) 10);
    SetInputParam("minimum_leaf_size", (int) 1);

    mlpackMain();

    // Calculate training accuracy.
    CLI::GetParam<RandomForestModel*>("output_model")->rf.Classify(testData,
         predictions);

    correct = arma::accu(predictions == testLabels);
    double accuracy10 = (double(correct) / double(testLabels.n_elem) * 100);

    // It's possible this might not work just because of randomness.  The VC2
    // dataset is not very large.
    success = (accuracy10 != accuracy5 || accuracy5 != accuracy1);
    if (success)
      break;
  }

  BOOST_REQUIRE_EQUAL(success, true);
}

BOOST_AUTO_TEST_SUITE_END();


#include "point_cloud.h"
#include "kd_tree.hpp"
#include "rgb_to_hsv.hpp"

#include <Eigen/Dense>

namespace pcs {

PointCloud::PointCloud(const Eigen::MatrixXd &points,
                       const Eigen::MatrixXd &colors) {
  points_.resize(points.rows());
  colors_.resize(colors.rows());
  for (auto i = 0u; i < points.rows(); ++i) {
    points_[i] = points.row(i);
  }
  for (auto i = 0u; i < colors.rows(); ++i) {
    colors_[i] = rgb_to_hsv(colors.row(i));
  }
}

PointCloud::PointCloud(const Eigen::MatrixXd &points) {
  points_.resize(points.rows());
  for (auto i = 0u; i < points.rows(); ++i) {
    points_[i] = points.row(i);
  }
}

void PointCloud::clear() {
  points_.clear();
  colors_.clear();
}

bool PointCloud::empty() const { return !has_points(); }

Eigen::Vector3d PointCloud::get_min_bound() const {
  if (!has_points()) {
    return Eigen::Vector3d(0.0, 0.0, 0.0);
  }
  auto itr_x =
      std::min_element(points_.begin(), points_.end(),
                       [](const Eigen::Vector3d &a, const Eigen::Vector3d &b) {
                         return a(0) < b(0);
                       });
  auto itr_y =
      std::min_element(points_.begin(), points_.end(),
                       [](const Eigen::Vector3d &a, const Eigen::Vector3d &b) {
                         return a(1) < b(1);
                       });
  auto itr_z =
      std::min_element(points_.begin(), points_.end(),
                       [](const Eigen::Vector3d &a, const Eigen::Vector3d &b) {
                         return a(2) < b(2);
                       });
  return Eigen::Vector3d((*itr_x)(0), (*itr_y)(1), (*itr_z)(2));
}

Eigen::Vector3d PointCloud::get_max_bound() const {
  if (!has_points()) {
    return Eigen::Vector3d(0.0, 0.0, 0.0);
  }
  auto itr_x =
      std::max_element(points_.begin(), points_.end(),
                       [](const Eigen::Vector3d &a, const Eigen::Vector3d &b) {
                         return a(0) < b(0);
                       });
  auto itr_y =
      std::max_element(points_.begin(), points_.end(),
                       [](const Eigen::Vector3d &a, const Eigen::Vector3d &b) {
                         return a(1) < b(1);
                       });
  auto itr_z =
      std::max_element(points_.begin(), points_.end(),
                       [](const Eigen::Vector3d &a, const Eigen::Vector3d &b) {
                         return a(2) < b(2);
                       });
  return Eigen::Vector3d((*itr_x)(0), (*itr_y)(1), (*itr_z)(2));
}

bool PointCloud::has_points() const { return points_.size() > 0; }

bool PointCloud::has_colors() const {
  return points_.size() > 0 && colors_.size() == points_.size();
}

double PointCloud::estimate_mean_distance() const {
  double mean_distance = 0;
  if (points_.size() < 2) {
    return mean_distance;
  }

  KDTree<3> tree(points_);
  std::vector<double> distances;
  distances.reserve(points_.size());
  for (const auto &point : points_) {
    auto neighbors = tree.find_nns(point, 2);
    distances.push_back(neighbors[1].second);
  }

  for (auto d : distances) {
    mean_distance += d;
  }
  mean_distance /= distances.size();

  return mean_distance;
}

void PointCloud::add_point(const Eigen::Vector3d &point) {
  points_.push_back(point);
}

void PointCloud::add_point_and_color(const Eigen::Vector3d &point,
                                     const Eigen::Vector3d &color) {
  points_.push_back(point);
  colors_.push_back(rgb_to_hsv(color));
}

void PointCloud::add_points(const Eigen::MatrixXd &points) {
  for (auto i = 0u; i < points.rows(); ++i) {
    points_.push_back(points.row(i));
  }
}

void PointCloud::add_points_and_colors(const Eigen::MatrixXd &points,
                                       const Eigen::MatrixXd &colors) {
  for (auto i = 0u; i < points.rows(); ++i) {
    points_.push_back(points.row(i));
    colors_.push_back(rgb_to_hsv(colors.row(i)));
  }
}

Eigen::Vector3d PointCloud::get_point(std::size_t idx) const {
  return points_[idx];
}

Eigen::Vector3d PointCloud::get_color(std::size_t idx) const {
  return hsv_to_rgb(colors_[idx]);
}

std::pair<Eigen::Vector3d, Eigen::Vector3d> PointCloud::get_point_and_color(
    std::size_t idx) const {
  return std::make_pair(points_[idx], hsv_to_rgb(colors_[idx]));
}
std::size_t PointCloud::num_points() const { return points_.size(); }

Eigen::MatrixXd PointCloud::get_points() const {
  Eigen::MatrixXd points = Eigen::MatrixXd::Zero(points_.size(), 3);
  for (auto i = 0u; i < points_.size(); ++i) {
    points.row(i) = points_[i];
  }
  return points;
}

Eigen::MatrixXd PointCloud::get_colors() const {
  Eigen::MatrixXd colors = Eigen::MatrixXd::Zero(colors_.size(), 3);
  for (auto i = 0u; i < colors_.size(); ++i) {
    colors.row(i) = hsv_to_rgb(colors_[i]);
  }
  return colors;
}

}  // namespace pcs


#include <iostream>
#include <fstream>
#include <vector>
#include <bitset>
#include <boost/dynamic_bitset.hpp>

std::vector<boost::dynamic_bitset<>> getBytesFromFile();

int main()
{
  //start somewhere, one must!
  std::cout << "Hello world" << std::endl;

  std::vector<boost::dynamic_bitset<>> codes = getBytesFromFile();

  std::size_t columns = codes[0].size();
  std::size_t rows = codes.size();
  for (size_t i = 0; i < rows; i++)
  {
    std::cout << codes[i] << std::endl;
  }

  std::vector<boost::dynamic_bitset<>> TCodes;
  for (int i = 0; i < columns; i++)
  {
    boost::dynamic_bitset code(rows);
    for (int j = 0; j < rows; j++)
    {
      code[(rows - 1) - j] = codes[j][(columns-1)-i];
    }
    TCodes.push_back(code);
  }

  boost::dynamic_bitset<> gammaBits(columns);
  for (size_t i = 0; i < columns; i++)
  {
    std::cout << TCodes[i] << std::endl;
    int amountOfBitsSet = TCodes[i].count();
    std::cout << "amountOfBits: " << amountOfBitsSet << " rows: " << rows << std::endl;
    if (amountOfBitsSet > rows / 2)
    {
      gammaBits[(columns-1)-i] = 1;
    }
    else
    {
      gammaBits[(columns-1)-i] = 0;
    }
  }
  int gamma = gammaBits.to_ulong();
  std::cout << gamma << std::endl;

  int epsilon = gammaBits.flip().to_ulong();
  std::cout << epsilon << std::endl;

  std::cout << "product: " << gamma * epsilon << std::endl;
  std::cout << "ow sheesh :s" << std::endl;
  return 0;
}

std::vector<boost::dynamic_bitset<>> getBytesFromFile()
{
  std::vector<boost::dynamic_bitset<>> temp;
  std::ifstream bytesFromFile;
  bytesFromFile.open("hello-world.txt");

  if (!bytesFromFile)
  {
    std::cout << "No file 'hello-world.txt' found! Did you name it correctly?" << std::endl;
    exit(1);
  }

  boost::dynamic_bitset<> x;
  while (bytesFromFile >> x)
  {
    temp.push_back(x);
  }
  bytesFromFile.close();
  return temp;
}

day 03/Marnix - C++/hello-world-1/hello-world.cpp


#include <cfloat>
#include <vector>
#include <algorithm>
#include <iostream>

#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/serialization/export.hpp>

#include "core/Common.h"
#include "math/Random.h"
#include "Approximator.h"
#include "ai/AI.h"
#include "TD.h"

namespace OpenNero
{

    /// called right before the agent is born
    bool TDBrain::initialize(const AgentInitInfo& init)
    {
        mInfo = init;
        this->fitness = mInfo.reward.getInstance();

        int bins = action_bins;

        if (num_tiles > 0)
        {
            AssertMsg(action_bins == 0, "action_bins must be 0 for num_tiles > 0");
            AssertMsg(state_bins == 0, "state_bins must be 0 for num_tiles > 0");
            mApproximator.reset(
                new TilesApproximator(mInfo, num_tiles, num_weights));
            bins = 7; // XXX force bins > 0 to discretize action_list below
        }
        else
        {
            AssertMsg(action_bins > 0, "action_bins > 0 for num_tiles == 0");
            AssertMsg(state_bins > 0, "action_bins > 0 for num_tiles == 0");
            mApproximator.reset(
                new TableApproximator(mInfo, action_bins, state_bins));
        }

        // Similar to FeatureVectorInfo::enumerate (from AI.cpp).
        //
        // We want to enumerate all possible actions in a discrete way, so that
        // we can store them in a policy table somehow. So, for each action
        // dimension, if it's discrete we just enumerate the integral values for
        // that dimension, and if it's continuous, we split it into "bins"
        // different values.
        //
        // An example: Suppose some action dimension is continuous and spans the
        // closed interval [-1, 1]. Additionally suppose we want to split
        // continuous action dimensions into 5 bins. We'd like to preserve the
        // actual range of action values, but only store the 5 discrete values
        // that equally partition this space, i.e., {-1, -.5, 0, .5, 1}. So we
        // traverse the action dimension from -1 to 1 (inclusive), adding
        // (hi - lo) / (bins - 1) == (1 - -1) / 4 == 0.5 each time.
        const FeatureVectorInfo& info = init.actions;
        action_list.clear();
        action_list.push_back(info.getInstance());
        for (size_t i = 0; i < info.size(); ++i)
        {
            const double lo = info.getMin(i), hi = info.getMax(i);
            const double inc = info.isDiscrete(i) ? 1.0f : (hi - lo) / (bins - 1);
            std::vector< Actions > new_action_list;
            std::vector< Actions >::const_iterator iter;
            for (iter = action_list.begin(); iter != action_list.end(); ++iter)
            {
                for (double a = lo; a <= hi; a += inc)
                {
                    FeatureVector v = *iter;
                    v[i] = a;
                    new_action_list.push_back(v);
                }
            }
            action_list = new_action_list;
        }

        return true;
    }

    /// called for agent to take its first step
    Actions TDBrain::start(const TimeType& time, const Observations& new_state)
    {
        epsilon_greedy(new_state);
        action = new_action;
        state = new_state;
        return action;
    }

    /// act based on time, sensor arrays, and last reward
    Actions TDBrain::act(const TimeType& time, const Observations& new_state, const Reward& reward)
    {
		AssertMsg(reward.size() == 1, "multi-objective rewards not supported");
        // select new action and estimate its value
        double new_Q = epsilon_greedy(new_state);
        double old_Q = mApproximator->predict(state, action);
        // Q(s_t, a_t) <- Q(s_t, a_t) + \alpha [r_{t+1} + \gamma Q(s_{t+1}, a_{t+1}) - Q(s_t, a_t)
        mApproximator->update(state, action, old_Q + mAlpha * (reward[0] + mGamma * new_Q - old_Q));
        action = new_action;
        state = new_state;
        return action;
    }

    /// called to tell agent about its last reward
    bool TDBrain::end(const TimeType& time, const Reward& reward)
    {
		AssertMsg(reward.size() == 1, "multi-objective rewards not supported");
		// Q(s_t, a_t) <- Q(s_t, a_t) + \alpha [r_{t+1} - Q(s_t, a_t)]
        // LOG_F_DEBUG("ai", "TD FINAL UPDATE s1: " << state << ", a1: " << action << ", r: " << reward);
        double old_Q = mApproximator->predict(state, action);
        mApproximator->update(state, action, old_Q + mAlpha * (reward[0] - old_Q));
        return true;
    }

    /// select action according to the epsilon-greedy policy
    double TDBrain::epsilon_greedy(const Observations& new_state)
    {
        // with chance epsilon, select random action
        if (RANDOM.randF() < mEpsilon)
        {
            new_action = mInfo.actions.getRandom();
            double value = predict(new_state);
            return value;
        }
        // enumerate all possible actions (actions must be discrete!)
        new_action = mInfo.actions.getInstance();
        // select the greedy action in random order
        std::random_shuffle(action_list.begin(), action_list.end());
        double max_value = -DBL_MAX;
        std::vector< Actions >::const_iterator iter;
        for (iter = action_list.begin(); iter != action_list.end(); ++iter)
        {
            double value = mApproximator->predict(new_state, *iter);
            if (value > max_value)
            {
                max_value = value;
                new_action = *iter;
            }
        }
        // Assuming if you choose max value, you will want to update with that as your prediction
        return max_value;
    }

    /// called right before the agent dies
    bool TDBrain::destroy()
    {
        return true;
    }

    /// serialize this brain to a text string
    std::string TDBrain::to_string() const
    {
        std::ostringstream oss;
        boost::archive::text_oarchive oa(oss);
        oa << *this;
        return oss.str();
    }

    /// deserialize this brain from a text string
    void TDBrain::from_string(const std::string& s)
    {
        try {
            std::istringstream iss(s);
            boost::archive::text_iarchive ia(iss);
            ia >> *this;
        } catch (boost::archive::archive_exception const& e) {
            LOG_F_ERROR("ai.rl", "unable to load agent because of error, " << e.what());
        }
    }
}

BOOST_CLASS_EXPORT(OpenNero::TDBrain)


/*
 * Copyright (c) 2015-2017, Princeton University, Johannes M Dieterich, Emily A Carter
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * 2. 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.
 *
 * 3. Neither the name of the copyright holder nor the names of its contributors may
 * be used to endorse or promote products derived from this software without specific
 * prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
 * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */
#include <armadillo>
#include <cmath>
#include "HelperFunctions.hpp"
using namespace arma;

const static double LOWDENSITYFORCUTOFF = 1E-2;
const static double HIGHDENSITYORCUTOFF = 100.0;
const static double HIPOWER = exp(100.0);

template<class GridComputer, template<class> class GridType>
void CutoffFunctions::WGCVacuumCutoff(unique_ptr<GridType<GridComputer> >& density, const double rhoV, const double rhoStep){

    const double ct = exp(rhoV/rhoStep);
    const double ctP1Sq = (ct+1.0)*(ct+1.0);
    const double rhoStepSq = rhoStep*rhoStep;
    const double t1 = (rhoStep+ct*rhoStep);
    const double t2 = (2.0*ctP1Sq*rhoStepSq);
    const double cutHi = (HIPOWER-1.0)/(HIPOWER+ct);

    cube* dens = density->getRealGrid();
    const size_t nSlices = dens->n_slices;
    const size_t nRows = dens->n_rows;
    const size_t nCols = dens->n_cols;
    #pragma omp parallel for default(none) shared(dens)
    for(size_t x = 0; x < nSlices; ++x){
        for(size_t col = 0; col < nCols; ++col){
            for(size_t row = 0; row < nRows; ++row){
                const double div = dens->at(row,col,x)/rhoStep;
                if(div < LOWDENSITYFORCUTOFF){
                    // the cutoff function is taylor expanded to 2nd order
                    const double d = dens->at(row,col,x);
                    dens->at(row,col,x) = d/t1 + (-1.0+ct)*d*d/t2;
                } else if(div > HIGHDENSITYORCUTOFF){
                    dens->at(row,col,x) = cutHi; // density is much bigger
                } else {
                    const double powDens = exp(div);
                    dens->at(row,col,x) = (powDens-1.0)/(powDens+ct);
                }
            }
        }
    }
    density->complete(dens);
}

double CutoffFunctions::vacuumCutoff(const double density, const double rhoV, const double rhoStep){

    // XXX not happy about this block, as it is a lot of recomputing but to save memory, this function may still be useful
    const double ct = exp(rhoV/rhoStep);
    const double ctP1Sq = (ct+1.0)*(ct+1.0);
    const double rhoStepSq = rhoStep*rhoStep;
    const double t1 = (rhoStep+ct*rhoStep);
    const double t2 = (2.0*ctP1Sq*rhoStepSq);
    const double cutHi = (HIPOWER-1.0)/(HIPOWER+ct);

    const double div = density/rhoStep;
    if(div < LOWDENSITYFORCUTOFF){
        // the cutoff function is taylor expanded to 2nd order
        return density/t1 + (-1.0+ct)*density*density/t2;
    } else if(div > HIGHDENSITYORCUTOFF){
        return cutHi; // density is much bigger
    } else {
        const double powDens = exp(div);
        return (powDens-1.0)/(powDens+ct);
    }
}

template<class GridComputer, template<class> class GridType>
void CutoffFunctions::WGCVacuumCutoffDeriv(unique_ptr<GridType<GridComputer> >& density, const double rhoV, const double rhoStep){

    const double ct = exp(rhoV/rhoStep);
    const double ctP1Sq = (ct+1.0)*(ct+1.0);
    const double rhoStepSq = rhoStep*rhoStep;
    const double hiCtSq = (HIPOWER+ct)*(HIPOWER+ct);
    const double t1 = (rhoStep+ct*rhoStep);
    const double t2 = (ctP1Sq*rhoStepSq);
    const double cutHi = HIPOWER*(1.0+ct)/(hiCtSq*rhoStep);

    cube* dens = density->getRealGrid();
    const size_t nSlices = dens->n_slices;
    const size_t nRows = dens->n_rows;
    const size_t nCols = dens->n_cols;
    #pragma omp parallel for default(none) shared(dens)
    for(size_t x = 0; x < nSlices; ++x){
        for(size_t col = 0; col < nCols; ++col){
            for(size_t row = 0; row < nRows; ++row){
                const double div = dens->at(row,col,x)/rhoStep;
                if(div < LOWDENSITYFORCUTOFF){
                    // the cutoff function is taylor expanded to 2nd order
                    const double d = dens->at(row,col,x);
                    dens->at(row,col,x) = 1.0/t1 + (-1.0+ct)*d/t2;
                } else if(div > HIGHDENSITYORCUTOFF){
                    dens->at(row,col,x) = cutHi; // density is much bigger
                } else {
                    const double powDens = exp(div);
                    const double powCt = powDens+ct;
                    const double powCtSq = powCt*powCt;
                    dens->at(row,col,x) = powDens*(1.0+ct)/(powCtSq*rhoStep);
                }
            }
        }
    }
}

double CutoffFunctions::vacuumCutoffDeriv(const double density, const double rhoV, const double rhoStep){

    // XXX not happy about this block, as it is a lot of recomputing but to save memory, this function may still be useful
    const double ct = exp(rhoV/rhoStep);
    const double ctP1Sq = (ct+1.0)*(ct+1.0);
    const double rhoStepSq = rhoStep*rhoStep;
    const double hiCtSq = (HIPOWER+ct)*(HIPOWER+ct);
    const double t1 = (rhoStep+ct*rhoStep);
    const double t2 = (ctP1Sq*rhoStepSq);
    const double cutHi = HIPOWER*(1.0+ct)/(hiCtSq*rhoStep);

    const double div = density/rhoStep;
    if(div < LOWDENSITYFORCUTOFF){
        // the cutoff function is taylor expanded to 2nd order
        return 1.0/t1 + (-1.0+ct)*density/t2;
    } else if(div > HIGHDENSITYORCUTOFF){
        return  cutHi; // density is much bigger
    } else {
        const double powDens = exp(div);
        const double powCt = powDens+ct;
        const double powCtSq = powCt*powCt;
        return powDens*(1.0+ct)/(powCtSq*rhoStep);
    }
}

double MathFunctions::lindhardResponse(const double eta, const double lambda, const double mu){

    if(eta < 0.0){return 0.0;}
    // limit for small eta
    else if(eta < 1e-10){
        const double lind = 1.0 - lambda + eta*eta * (1.0/3.0-3.0*mu);
        return lind;
    } else if(abs(eta-1.0) < 1e-10){
        const double lind = 2.0 - lambda - 3.0*mu + 20.0*(eta-1.0);
        return lind;
    } else if(eta > 3.65){
        // Taylor expansion for high eta
        const double etaSq = eta*eta;
        const double invEtaSq = 1.0/etaSq;
        const double lind = 3.0*(1.0-mu)*etaSq
            - lambda - 0.6
            + invEtaSq *  (-0.13714285714285712
            + invEtaSq * (-6.39999999999999875E-2
            + invEtaSq * (-3.77825602968460128E-2
            + invEtaSq * (-2.51824061652633074E-2
            + invEtaSq * (-1.80879839616166146E-2
            + invEtaSq * (-1.36715733124818332E-2
            + invEtaSq * (-1.07236045520990083E-2
            + invEtaSq * (-8.65192783339199453E-3
            + invEtaSq * (-7.1372762502456763E-3
            + invEtaSq * (-5.9945117538835746E-3
            + invEtaSq * (-5.10997527675418131E-3
            + invEtaSq * (-4.41060829979912465E-3
            + invEtaSq * (-3.84763737842981233E-3
            + invEtaSq * (-3.38745061493813488E-3
            + invEtaSq * (-3.00624946457977689E-3)))))))))))))));
        return lind;
    } else {
        const double lind = 1.0 / (0.5 + 0.25 * (1.-eta*eta) * log((1.0 + eta)
            / abs(1.0-eta))/eta) - 3.0 * mu * eta*eta - lambda;
        return lind;
    }
}

double MathFunctions::derivativeLindhardResponse(const double eta, const double mu){

    if(eta < 0.0){
        return 0.0;
    } else if(eta < 1e-10){
        return 2*eta*(1.0/3.0 - 3.0*mu);
    } else if(abs(eta-1.0) < 1e-10){
        return 40.0;
    } else {
        const double etaSq = eta*eta;
        const double oneMEta = 1-eta;
        const double onePEta = 1+eta;
        const double denom = (0.5 + 0.25*(1-eta*eta) * log(onePEta/abs(oneMEta))/eta);
        const double denomSq = denom*denom;
        const double gprim = ((etaSq + 1)*0.25 / etaSq * log(abs((1.0+eta)/(oneMEta))) - 0.5/eta) / denomSq - 6*eta*mu;

        return gprim;
    }
}


// Copyright 2009 The Trustees of Indiana University.

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

//  Authors: Nicholas Edmonds
//           Andrew Lumsdaine

#include <boost/random.hpp>
#include <boost/core/lightweight_test.hpp>

#include <boost/graph/rmat_graph_generator.hpp>
#include <boost/graph/small_world_generator.hpp>
#include <boost/graph/ssca_graph_generator.hpp>
#include <boost/graph/erdos_renyi_generator.hpp>
#include <boost/graph/mesh_graph_generator.hpp>

#include <boost/graph/adjacency_list.hpp>

using namespace boost;

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

    typedef rand48 RandomGenerator;

    typedef adjacency_list< vecS, vecS, directedS > Graph;

    RandomGenerator gen;

    size_t N = 100;
    size_t M = 1000;
    double p = 0.05;

    // Test Erdos-Renyi generator
    {
        erdos_renyi_iterator< RandomGenerator, Graph > start(gen, N, p);
        erdos_renyi_iterator< RandomGenerator, Graph > end;

        while (start != end)
            ++start;

        BOOST_TEST(start == end);
    }

    {
        sorted_erdos_renyi_iterator< RandomGenerator, Graph > start(gen, N, p);
        sorted_erdos_renyi_iterator< RandomGenerator, Graph > end;

        while (start != end)
            ++start;

        BOOST_TEST(start == end);
    }

    // Test Small World generator
    {
        small_world_iterator< RandomGenerator, Graph > start(gen, N, M, p);
        small_world_iterator< RandomGenerator, Graph > end;

        while (start != end)
            ++start;

        BOOST_TEST(start == end);
    }

    // Test SSCA generator
    {
        ssca_iterator< RandomGenerator, Graph > start(gen, N, 5, 0.5, 5, p);
        ssca_iterator< RandomGenerator, Graph > end;

        while (start != end)
            ++start;

        BOOST_TEST(start == end);
    }

    // Test Mesh generator
    {
        mesh_iterator< Graph > start(N, N);
        mesh_iterator< Graph > end;

        while (start != end)
            ++start;

        BOOST_TEST(start == end);
    }

    // Test R-MAT generator
    double a = 0.57, b = 0.19, c = 0.19, d = 0.05;

    {
        rmat_iterator< RandomGenerator, Graph > start(gen, N, M, a, b, c, d);
        rmat_iterator< RandomGenerator, Graph > end;

        while (start != end)
            ++start;

        BOOST_TEST(start == end);
    }

    {
        unique_rmat_iterator< RandomGenerator, Graph > start(
            gen, N, M, a, b, c, d);
        unique_rmat_iterator< RandomGenerator, Graph > end;

        while (start != end)
            ++start;

        BOOST_TEST(start == end);
    }

    {
        sorted_unique_rmat_iterator< RandomGenerator, Graph > start(
            gen, N, M, a, b, c, d);
        sorted_unique_rmat_iterator< RandomGenerator, Graph > end;

        while (start != end)
            ++start;

        BOOST_TEST(start == end);
    }

    {
        sorted_unique_rmat_iterator< RandomGenerator, Graph > start(
            gen, N, M, a, b, c, d, true);
        sorted_unique_rmat_iterator< RandomGenerator, Graph > end;

        while (start != end)
            ++start;

        BOOST_TEST(start == end);
    }

    return boost::report_errors();
}


#ifndef MLT_MODELS_CLASSIFIERS_OPTIMIZABLE_LINEAR_CLASSIFIER_HPP
#define MLT_MODELS_CLASSIFIERS_OPTIMIZABLE_LINEAR_CLASSIFIER_HPP

#include <type_traits>

#include <Eigen/Core>

#include "../optimizable_linear_model.hpp"
#include "linear_classifier.hpp"

namespace mlt {
namespace models {
namespace classifiers {
	template <class Loss, class Optimizer>
	class OptimizableLinearClassifier : public OptimizableLinearModel<LinearClassifier<OptimizableLinearClassifier<Loss, Optimizer>>, Loss, Optimizer> {
	public:
		template <class L, class O, class = enable_if<is_same<decay_t<L>, Loss>::value && is_convertible<decay_t<O>, Optimizer>::value>>
		explicit OptimizableLinearClassifier(L&& loss, O&& optimizer, double regularization,
			bool fit_intercept = true) : OptimizableLinearModel(forward<L>(loss), forward<O>(optimizer), regularization, fit_intercept) {}
	};
}
}
}
#endif


#include <igl/readOBJ.h>
#include <igl/png/readPNG.h>
#include <igl/opengl/glfw/imgui/ImGuiMenu.h>
#include <igl/opengl/glfw/Viewer.h>
#include <igl/unproject_onto_mesh.h>
#include <igl/decimate.h>
#include <imgui/imgui.h>
#include <sstream>
#include <vector>
#include <fstream>
#include <boost/filesystem.hpp>
#include "landmarks.h"

using namespace std;
using namespace Eigen;
using Viewer = igl::opengl::glfw::Viewer;
namespace fs = boost::filesystem;

bool load_mesh(int);

Viewer viewer;

int current_mesh;
std::vector<std::string> faces_to_label;
std::vector<int> labels; // Indices to verties in low resolution mesh.

// Viewer related point data.
Eigen::MatrixXd points(0, 3);
std::vector<std::string> point_labels;

// Vertex array, #V x 3
Eigen::MatrixXd V(0, 3);
Eigen::MatrixXd V_hd(0, 3);
// Face array, #F x 3
Eigen::MatrixXi F(0, 3);
Eigen::MatrixXi F_hd(0, 3);
// This will contain indices which map vertices of the low resolution mesh to high resolution.
Eigen::VectorXi v_map(0);
Eigen::VectorXi f_map(0);

Eigen::MatrixXd N(0, 3);
Eigen::MatrixXd TC(0, 2);
Eigen::MatrixXd FN(0, 1);
Eigen::MatrixXi FTC(0, 3);

using ColorMap = Eigen::Matrix<unsigned char, Eigen::Dynamic, Eigen::Dynamic>;
ColorMap R, G, B, A;

fs::path landmark_path(std::string mesh_path) {
  fs::path current_obj(mesh_path);
  fs::path parent_path = current_obj.parent_path();
  fs::path filename = current_obj.stem();
  filename += ".landmarks";
  return parent_path / filename;
}

void save_points() {
  fs::path label_path = landmark_path(faces_to_label[current_mesh]);
  assert(labels.size() == points.rows());
  landmarks::save_points(label_path, labels, V_hd);
}

void points_changed() {
  viewer.data().clear_labels();
  point_labels.clear();
  for (int i=0; i < points.rows(); i++) {
    std::stringstream ss;
    ss << i;
    point_labels.push_back(ss.str());
  }
  viewer.data().set_labels(points, point_labels);
  viewer.data().labels_strings = point_labels;
  viewer.data().labels_positions = points;
}

void load_landmarks(fs::path& filepath) {
  std::string file = filepath.string();
  auto pair = landmarks::read_landmarks(file);
  std::vector<Eigen::RowVector3d> vertices = pair.first;
  std::vector<int> read_labels = pair.second;
  points.resize(read_labels.size(), 3);
  labels.reserve(read_labels.size());
  for (int i=0; i < read_labels.size(); i++) {
    points.row(i) = vertices[i];
    labels.push_back(read_labels[i]);
  }
  points_changed();
}

void load_texture(std::string& mesh_path) {
  fs::path path(mesh_path);
  fs::path texture_path = path.parent_path() / "texture_map.png";
  igl::png::readPNG(texture_path.string(), R, G, B, A);
}

bool load_mesh(int mesh) {
  current_mesh = mesh;
  igl::readOBJ(faces_to_label[current_mesh], V_hd, TC, N, F_hd, FTC, FN);
  load_texture(faces_to_label[current_mesh]);

  igl::decimate(V_hd, F_hd, 10000, V, F, f_map, v_map);

  viewer.data().clear();
  viewer.data().set_mesh(V_hd, F_hd);
  viewer.data().set_normals(N);
  viewer.data().set_texture(R, G, B, A);
  viewer.data().set_uv(TC, FTC);
  viewer.data().set_normals(FN);
  viewer.core().align_camera_center(V);
  viewer.data().show_lines = false;
  viewer.data().show_texture = true;
  viewer.data().set_colors(Eigen::RowVector3d(1.0, 1.0, 1.0));
  viewer.core().lighting_factor = 0.0;

  points.resize(0, 3);
  labels.clear();

  std::string mesh_path = faces_to_label[current_mesh];
  fs::path point_path = landmark_path(mesh_path);

  if (fs::exists(point_path)) {
    load_landmarks(point_path);
    points_changed();
  }

  return true;
}

std::vector<std::string> get_paths(std::string& data_dir) {
  std::vector<std::string> files;
  for (auto & entry : fs::directory_iterator(data_dir)) {
    fs::path obj_path = entry.path() / "textured.obj";
    files.push_back(obj_path.string());
  }
  return files;
}

void remove_point() {
  labels.pop_back();
  Eigen::MatrixXd new_points(points.rows() - 1, 3);
  new_points.block(0, 0, new_points.rows(), 3) = points.block(0, 0, new_points.rows(), 3);
  points = new_points;
  points_changed();
  save_points();
}

void add_point(int vertex_index) {
  int index_on_high_res = v_map[vertex_index];
  labels.push_back(index_on_high_res);
  Eigen::MatrixXd new_points(points.rows() + 1, 3);
  new_points.block(0, 0, points.rows(), 3) = points;
  new_points.row(points.rows()) = V_hd.row(index_on_high_res);
  points = new_points;
  points_changed();
  save_points();
}

void prev_face() {
  current_mesh--;
  if (current_mesh == -1) current_mesh = faces_to_label.size() - 1;
  load_mesh(current_mesh);
}

void next_face() {
  current_mesh++;
  if (current_mesh == faces_to_label.size()) {
    std::exit(0);
  }
  load_mesh(current_mesh);
}

bool callback_key_down(Viewer& viewer, unsigned char key, int modifiers) {
  if (modifiers & IGL_MOD_CONTROL && key == 'Z') {
    if (labels.size() == 0) return false;
    std::cout << "Undo" << std::endl;
    remove_point();
    return true;
  }
  const char key_tab = '\002';
  if (key == 'X') {
    viewer.data().clear();
    viewer.data().set_mesh(V_hd, F_hd);
  } else if (key == 'C') {
    viewer.data().clear();
    viewer.data().set_mesh(V, F);
  }

  if (modifiers & IGL_MOD_SHIFT && key == key_tab) {
    prev_face();
  } else if (key == key_tab) {
    std::cout << "Moving to next face." << std::endl;
    next_face();
  }
  return false;
}

bool callback_mouse_down(Viewer& viewer, int button, int modifier) {
  if (button == (int) Viewer::MouseButton::Right) return false;
  int face_id, vertex_index;
  Vector3f barrycentric_coordinates;

  int down_mouse_x = viewer.current_mouse_x;
  int down_mouse_y = viewer.core().viewport(3) - viewer.current_mouse_y;
  if (igl::unproject_onto_mesh(Eigen::Vector2f(down_mouse_x, down_mouse_y), viewer.core().view,
                              viewer.core().proj, viewer.core().viewport, V, F, face_id, barrycentric_coordinates)) {
    barrycentric_coordinates.maxCoeff(&vertex_index);
    vertex_index = F(face_id, vertex_index);
    add_point(vertex_index);
    return true;
  }
  return false;
}

Eigen::RowVector3d point_colors(247. / 255., 72. / 255., 37. / 255.);

bool callback_pre_draw(Viewer& viewer) {
  viewer.data().clear_points();
  viewer.data().set_points(points, point_colors);
  return false;
}

void init_viewer(Viewer& viewer) {
  viewer.callback_key_down = callback_key_down;
  viewer.callback_mouse_down = callback_mouse_down;
  //viewer.callback_mouse_move = callback_mouse_move;
  //viewer.callback_mouse_up = callback_mouse_up;
  viewer.callback_pre_draw = callback_pre_draw;
  viewer.data().point_size = 25;
  viewer.core().set_rotation_type(igl::opengl::ViewerCore::ROTATION_TYPE_TRACKBALL);
  viewer.data().show_lines = false;
  viewer.data().label_color = RowVector4f(247., 72., 37., 255.) / 255.0;
  viewer.data().show_labels = true;

  igl::opengl::glfw::imgui::ImGuiMenu menu;
  menu.callback_draw_viewer_window = [&menu](){
    menu.draw_viewer_menu();

  };
  viewer.plugins.push_back(&menu);

  viewer.launch();
}

int main(int argc, char *argv[]) {
  std::string data_dir;
  if (argc != 2) {
    std::cout << "Please provide a data directory. ./labeler <data-dir>" << std::endl;
    exit(1);
  } else {
    data_dir = argv[1];
  }
  auto files = get_paths(data_dir);
  for (std::string &file : files ) {
    if (file.find(".obj") != std::string::npos) {
      faces_to_label.push_back(file);
    }
  }
  load_mesh(0);
  init_viewer(viewer);
}