/*
 * @file testEssentialMatrix.cpp
 * @brief Test EssentialMatrix class
 * @author Frank Dellaert
 * @date December 17, 2013
 */

#include <gtsam/geometry/EssentialMatrix.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/Testable.h>

#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
#include <sstream>

using namespace boost::placeholders;
using namespace std;
using namespace gtsam;

GTSAM_CONCEPT_TESTABLE_INST(EssentialMatrix)
GTSAM_CONCEPT_MANIFOLD_INST(EssentialMatrix)

//*************************************************************************
// Create two cameras and corresponding essential matrix E
Rot3 trueRotation = Rot3::Yaw(M_PI_2);
Point3 trueTranslation(0.1, 0, 0);
Unit3 trueDirection(trueTranslation);
EssentialMatrix trueE(trueRotation, trueDirection);

//*************************************************************************
TEST (EssentialMatrix, equality) {
  EssentialMatrix actual(trueRotation, trueDirection), expected(trueRotation, trueDirection);
  EXPECT(assert_equal(expected, actual));
}

//*************************************************************************
TEST(EssentialMatrix, FromRotationAndDirection) {
  Matrix actualH1, actualH2;
  EXPECT(assert_equal(
      trueE, EssentialMatrix::FromRotationAndDirection(trueRotation, trueDirection, actualH1, actualH2),
      1e-8));

  Matrix expectedH1 = numericalDerivative11<EssentialMatrix, Rot3>(
      boost::bind(EssentialMatrix::FromRotationAndDirection, _1, trueDirection, boost::none,
                  boost::none),
      trueRotation);
  EXPECT(assert_equal(expectedH1, actualH1, 1e-7));

  Matrix expectedH2 = numericalDerivative11<EssentialMatrix, Unit3>(
      boost::bind(EssentialMatrix::FromRotationAndDirection, trueRotation, _1, boost::none,
                  boost::none),
                  trueDirection);
  EXPECT(assert_equal(expectedH2, actualH2, 1e-7));
}

//*************************************************************************
TEST (EssentialMatrix, FromPose3) {
  EssentialMatrix expected(trueRotation, trueDirection);
  Pose3 pose(trueRotation, trueTranslation);
  EssentialMatrix actual = EssentialMatrix::FromPose3(pose);
  EXPECT(assert_equal(expected, actual));
}

//*******************************************************************************
TEST(EssentialMatrix, localCoordinates0) {
  EssentialMatrix E;
  Vector expected = Z_5x1;
  Vector actual = E.localCoordinates(E);
  EXPECT(assert_equal(expected, actual, 1e-8));
}

//*************************************************************************
TEST (EssentialMatrix, localCoordinates) {

  // Pose between two cameras
  Pose3 pose(trueRotation, trueTranslation);
  EssentialMatrix hx = EssentialMatrix::FromPose3(pose);
  Vector actual = hx.localCoordinates(EssentialMatrix::FromPose3(pose));
  EXPECT(assert_equal(Z_5x1, actual, 1e-8));

  Vector6 d;
  d << 0.1, 0.2, 0.3, 0, 0, 0;
  Vector actual2 = hx.localCoordinates(
      EssentialMatrix::FromPose3(pose.retract(d)));
  EXPECT(assert_equal(d.head(5), actual2, 1e-8));
}

//*************************************************************************
TEST (EssentialMatrix, retract0) {
  EssentialMatrix actual = trueE.retract(Z_5x1);
  EXPECT(assert_equal(trueE, actual));
}

//*************************************************************************
TEST (EssentialMatrix, retract1) {
  EssentialMatrix expected(trueRotation.retract(Vector3(0.1, 0, 0)), trueDirection);
  EssentialMatrix actual = trueE.retract((Vector(5) << 0.1, 0, 0, 0, 0).finished());
  EXPECT(assert_equal(expected, actual));
}

//*************************************************************************
TEST (EssentialMatrix, retract2) {
  EssentialMatrix expected(trueRotation,
      trueDirection.retract(Vector2(0.1, 0)));
  EssentialMatrix actual = trueE.retract((Vector(5) << 0, 0, 0, 0.1, 0).finished());
  EXPECT(assert_equal(expected, actual));
}

//*************************************************************************
TEST (EssentialMatrix, RoundTrip) {
  Vector5 d;
  d << 0.1, 0.2, 0.3, 0.4, 0.5;
  EssentialMatrix e, hx = e.retract(d);
  Vector actual = e.localCoordinates(hx);
  EXPECT(assert_equal(d, actual, 1e-8));
}

//*************************************************************************
Point3 transform_to_(const EssentialMatrix& E, const Point3& point) {
  return E.transformTo(point);
}
TEST (EssentialMatrix, transformTo) {
  // test with a more complicated EssentialMatrix
  Rot3 aRb2 = Rot3::Yaw(M_PI / 3.0) * Rot3::Pitch(M_PI_4)
      * Rot3::Roll(M_PI / 6.0);
  Point3 aTb2(19.2, 3.7, 5.9);
  EssentialMatrix E(aRb2, Unit3(aTb2));
  //EssentialMatrix E(aRb, Unit3(aTb).retract(Vector2(0.1, 0)));
  static Point3 P(0.2, 0.7, -2);
  Matrix actH1, actH2;
  E.transformTo(P, actH1, actH2);
  Matrix expH1 = numericalDerivative21(transform_to_, E, P), //
  expH2 = numericalDerivative22(transform_to_, E, P);
  EXPECT(assert_equal(expH1, actH1, 1e-8));
  EXPECT(assert_equal(expH2, actH2, 1e-8));
}

//*************************************************************************
EssentialMatrix rotate_(const EssentialMatrix& E, const Rot3& cRb) {
  return E.rotate(cRb);
}
TEST (EssentialMatrix, rotate) {
  // Suppose the essential matrix is specified in a body coordinate frame B
  // which is rotated with respect to the camera frame C, via rotation bRc.
  // The rotation between body and camera is:
  Point3 bX(1, 0, 0), bY(0, 1, 0), bZ(0, 0, 1);
  Rot3 bRc(bX, bZ, -bY), cRb = bRc.inverse();

  // Let's compute the ground truth E in body frame:
  Rot3 b1Rb2 = bRc * trueRotation * cRb;
  Point3 b1Tb2 = bRc * trueTranslation;
  EssentialMatrix bodyE(b1Rb2, Unit3(b1Tb2));
  EXPECT(assert_equal(bodyE, bRc * trueE, 1e-8));
  EXPECT(assert_equal(bodyE, trueE.rotate(bRc), 1e-8));

  // Let's go back to camera frame:
  EXPECT(assert_equal(trueE, cRb * bodyE, 1e-8));
  EXPECT(assert_equal(trueE, bodyE.rotate(cRb), 1e-8));

  // Derivatives
  Matrix actH1, actH2;
  try {
    bodyE.rotate(cRb, actH1, actH2);
  } catch (exception& e) {
  } // avoid exception
  Matrix expH1 = numericalDerivative21(rotate_, bodyE, cRb), //
  expH2 = numericalDerivative22(rotate_, bodyE, cRb);
  EXPECT(assert_equal(expH1, actH1, 1e-7));
  // Does not work yet EXPECT(assert_equal(expH2, actH2, 1e-8));
}

//*************************************************************************
TEST (EssentialMatrix, FromPose3_a) {
  Matrix actualH;
  Pose3 pose(trueRotation, trueTranslation); // Pose between two cameras
  EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8));
  Matrix expectedH = numericalDerivative11<EssentialMatrix, Pose3>(
      boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose);
  EXPECT(assert_equal(expectedH, actualH, 1e-7));
}

//*************************************************************************
TEST (EssentialMatrix, FromPose3_b) {
  Matrix actualH;
  Rot3 c1Rc2 = Rot3::Ypr(0.1, -0.2, 0.3);
  Point3 c1Tc2(0.4, 0.5, 0.6);
  EssentialMatrix E(c1Rc2, Unit3(c1Tc2));
  Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras
  EXPECT(assert_equal(E, EssentialMatrix::FromPose3(pose, actualH), 1e-8));
  Matrix expectedH = numericalDerivative11<EssentialMatrix, Pose3>(
      boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose);
  EXPECT(assert_equal(expectedH, actualH, 1e-5));
}

//*************************************************************************
TEST (EssentialMatrix, streaming) {
  EssentialMatrix expected(trueRotation, trueDirection), actual;
  stringstream ss;
  ss << expected;
  ss >> actual;
  EXPECT(assert_equal(expected, actual));
}

//*************************************************************************
TEST (EssentialMatrix, epipoles) {
  // Create an E
  Rot3 c1Rc2 = Rot3::Ypr(0.1, -0.2, 0.3);
  Point3 c1Tc2(0.4, 0.5, 0.6);
  EssentialMatrix E(c1Rc2, Unit3(c1Tc2));

  // Calculate expected values through SVD
  Matrix U, V;
  Vector S;
  gtsam::svd(E.matrix(), U, S, V);

  // take care of SVD sign ambiguity
  if (U(0, 2) > 0) {
    U = -U;
    V = -V;
  }

  // check rank 2 constraint
  CHECK(std::abs(S(2))<1e-10);

  // check epipoles not at infinity
  CHECK(std::abs(U(2,2))>1e-10 && std::abs(V(2,2))>1e-10);

  // Check epipoles

  // Epipole in image 1 is just E.direction()
  Unit3 e1(-U(0, 2), -U(1, 2), -U(2, 2));
  Unit3 actual = E.epipole_a();
  EXPECT(assert_equal(e1, actual));

  // take care of SVD sign ambiguity
  if (V(0, 2) < 0) {
    U = -U;
    V = -V;
  }

  // Epipole in image 2 is E.rotation().unrotate(E.direction())
  Unit3 e2(V(0, 2), V(1, 2), V(2, 2));
  EXPECT(assert_equal(e2, E.epipole_b()));
}

/* ************************************************************************* */
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */


#ifndef __INTEGRATE_UTIL_HH__
#define __INTEGRATE_UTIL_HH__
/********************************* TRICK HEADER *******************************
PURPOSE:
      (Integration wapper functions)
LIBRARY DEPENDENCY:
      ((../src/integrate.cpp))
*******************************************************************************/

#include <armadillo>
#include <cassert>
#include <vector>
#include "Rocket_Flight_DM.hh"

#define INTEGRATE(in, diff)                         \
  do {                                              \
    double in##d_new = diff;                        \
    in = integrate(in##d_new, in##d, in, int_step); \
    in##d = in##d_new;                              \
  } while (0)

#define INTEGRATE_MAT(in, diff)                     \
  do {                                              \
    arma::mat in##D_NEW = diff;                     \
    in = integrate(in##D_NEW, in##D, in, int_step); \
    in##D = in##D_NEW;                              \
  } while (0)

/**
 * \brief Integration of scalar state variable.
 * Modified Euler Midpoint method
 * Example first order lag:
 *   phid_new=(phic-phi)/tphi;
 *   phi=integrate(phid_new,phid,phi,int_step);
 *   phid=phid_new;
 */
double integrate(const double &dydx_new, const double &dydx, const double &y,
                 const double &int_step);

arma::mat integrate(arma::mat &DYDX_NEW, arma::mat &DYDX, arma::mat &Y,
                    const double int_step);
template <typename T>
void IntegratorRK4(std::vector<arma::vec> V_in, std::vector<arma::vec> &V_out,
                   void (T::*fp)(std::vector<arma::vec> Var_in,
                                 std::vector<arma::vec> &Var_out),
                   T *ClassPointer, double int_step) {
  {
    std::vector<std::vector<arma::vec>> KMAT;
    for (unsigned int i = 0; i < V_in.size(); i++) {
      std::vector<arma::vec> KROW(4);
      KMAT.push_back(KROW);
    }

    V_out = V_in;

    ((ClassPointer)->*fp)(V_out, KMAT[0]);

    for (unsigned int i = 0; i < V_in.size(); i++) {
      V_out[i] = V_in[i] + KMAT[0][i] * 0.5 * int_step;
    }

    ((ClassPointer)->*fp)(V_out, KMAT[1]);

    for (unsigned int i = 0; i < V_in.size(); i++) {
      V_out[i] = V_in[i] + KMAT[1][i] * 0.5 * int_step;
    }

    ((ClassPointer)->*fp)(V_out, KMAT[2]);

    for (unsigned int i = 0; i < V_in.size(); i++) {
      V_out[i] = V_in[i] + KMAT[2][i] * int_step;
    }

    ((ClassPointer)->*fp)(V_out, KMAT[3]);

    for (unsigned int i = 0; i < V_in.size(); i++) {
      V_out[i] = V_in[i] +
                 (int_step / 6.0) * (KMAT[0][i] + 2.0 * KMAT[1][i] +
                                     2.0 * KMAT[2][i] + KMAT[3][i]);
    }
  }
}

template <typename T>
void IntegratorEuler(std::vector<arma::vec> V_in, std::vector<arma::vec> &V_out,
                     void (T::*fp)(std::vector<arma::vec> Var_in,
                                   std::vector<arma::vec> &Var_out),
                     T *ClassPointer, double int_step) {
  std::vector<arma::vec> K_TEMP;

  for (unsigned int i = 0; i < V_in.size(); i++) {
    arma::vec temp;
    K_TEMP.push_back(temp);
  }

  ((ClassPointer)->*fp)(V_in, K_TEMP);

  for (unsigned int i = 0; i < V_in.size(); i++) {
    V_out[i] = V_in[i] + K_TEMP[i] * int_step;
  }
}
#endif  // __INTEGRATE_UTIL_HH__


#include "tntn/dem2tintiles_workflow.h"
#include "tntn/MercatorProjection.h"
#include "tntn/terra_meshing.h"
#include "tntn/simple_meshing.h"
#include "tntn/zemlya_meshing.h"
#include "tntn/TileMaker.h"
#include "tntn/logging.h"

#include <vector>
#include <boost/filesystem.hpp>

namespace tntn {

namespace fs = boost::filesystem;

std::vector<Partition> create_partitions_for_zoom_level(const RasterDouble& dem, int zoom)
{
    MercatorProjection projection;
    std::vector<Partition> partitions;

    double resolution = dem.get_cell_size();
    const auto points_bbox = dem.get_bounding_box();

    // const auto points_bbox = points.bounding_box();

    auto tile_xy = projection.MetersToTileXY({points_bbox.min.x, points_bbox.min.y}, zoom);
    const int tminx = tile_xy.x;
    const int tminy = tile_xy.y;

    tile_xy = projection.MetersToTileXY({points_bbox.max.x, points_bbox.max.y}, zoom);
    const int tmaxx = tile_xy.x;
    const int tmaxy = tile_xy.y;

    const int tile_size = projection.tileSize();
    const double tile_size_in_meters = projection.tileSizeInMeters(zoom);
    int nt = static_cast<int>(resolution * 800 / tile_size_in_meters);
    if(nt == 0) nt = 1;

    for(int tx = tminx; tx <= tmaxx; tx += nt)
    {
        for(int ty = tminy; ty <= tmaxy; ty += nt)
        {
            auto bbox_min = projection.PixelsToMeters({tx * tile_size, ty * tile_size}, zoom);
            auto bbox_max =
                projection.PixelsToMeters({(tx + nt) * tile_size, (ty + nt) * tile_size}, zoom);

            BoundingBox bbox = {bbox_min,
                                {std::min(bbox_max.x, points_bbox.max.x),
                                 std::min(bbox_max.y, points_bbox.max.y)}};

            double buffer = resolution * 100; // in meters
            BoundingBox bboxWithBuffer = {{bbox.min.x - buffer, bbox.min.y - buffer},
                                          {bbox.max.x + buffer, bbox.max.y + buffer}};

            Partition part = {bboxWithBuffer,
                              {tx, ty},
                              {(bbox_max.x > points_bbox.max.x ? tmaxx : tx + nt - 1),
                               (bbox_max.y > points_bbox.max.y ? tmaxy : ty + nt - 1)}};

            partitions.push_back(part);
        }
    }
    return partitions;
}

bool create_tiles_for_zoom_level(const RasterDouble& dem,
                                 const std::vector<Partition>& partitions,
                                 int zoom,
                                 const std::string& output_basedir,
                                 const double method_parameter,
                                 const std::string& meshing_method,
                                 MeshWriter& mesh_writer)
{
    for(const auto& part : partitions)
    {
        const auto bbox = part.bbox;
        TNTN_LOG_DEBUG("current tile bbox (world coordinates) [({},{}),({},{})]",
                       bbox.min.x,
                       bbox.min.y,
                       bbox.max.x,
                       bbox.max.y);

        int x1 = dem.x2col(bbox.min.x);
        int y1 = dem.y2row(bbox.min.y);

        int x2 = dem.x2col(bbox.max.x);
        int y2 = dem.y2row(bbox.max.y);

        TNTN_LOG_DEBUG("current tile raster crop box: [({},{}),({},{})]", x1, y1, x2, y2);

        if(x2 < x1)
        {
            std::swap(x1, x2);
        }

        if(y2 < y1)
        {
            std::swap(y1, y2);
        }

        auto raster_tile = std::make_unique<RasterDouble>();
        dem.crop(x1, y1, x2 - x1, y2 - y1, *raster_tile);

        std::unique_ptr<Mesh> mesh;

        if(meshing_method == "terra")
        {
            mesh = generate_tin_terra(std::move(raster_tile), method_parameter);
        }
        else if(meshing_method == "zemlya")
        {
            mesh = generate_tin_zemlya(std::move(raster_tile), method_parameter);
        }
#if defined(TNTN_USE_ADDONS) && TNTN_USE_ADDONS
        else if(meshing_method == "curvature")
        {
            mesh = generate_tin_curvature(*raster_tile, method_parameter);
        }
#endif
        else if(meshing_method == "dense")
        {
            mesh = generate_tin_dense_quadwalk(*raster_tile, (int)method_parameter);
        }
        else
        {
            TNTN_LOG_ERROR("Unknown meshing method {}, aborting", meshing_method);
            return false;
        }

        // Cut the TIN into tiles
        TileMaker tm;
        tm.loadMesh(std::move(mesh));

        fs::create_directory(fs::path(output_basedir));
        fs::create_directory(fs::path(output_basedir) / std::to_string(zoom));

        for(int tx = part.tmin.x; tx <= part.tmax.x; tx++)
        {
            for(int ty = part.tmin.y; ty <= part.tmax.y; ty++)
            {
                TNTN_LOG_INFO("Creating tile: {},{}", tx, ty);

                auto tile_dir =
                    fs::path(output_basedir) / std::to_string(zoom) / std::to_string(tx);
                fs::create_directory(tile_dir);

                auto file_path =
                    tile_dir / (std::to_string(ty) + "." + mesh_writer.file_extension());

                if(!tm.dumpTile(tx, ty, zoom, file_path.c_str(), mesh_writer))
                {
                    TNTN_LOG_ERROR("error dumping tile z:{} x:{} y:{}", zoom, tx, ty);
                    return false;
                }
            }
        }
    }
    return true;
}

} //namespace tntn


#pragma once

#include <iostream>
#include <vector>
#include <fstream>
#include <string>
#include <chrono>
#include <stdlib.h>
#include <boost/thread/thread.hpp>
#include <boost/range/combine.hpp>
#include <boost/foreach.hpp>

#include "bvsma/include/functions.hpp"

using namespace std;
typedef unsigned char byte;

void ProverUsage();

void VerifierUsage();

/******************************\
|**** Interactive ZKProofs ****|
\******************************/

/* Prover */

/* Simple */
int zkPoKBasicProver(shared_ptr<CommParty> prover, shared_ptr<OpenSSLDlogECFp> dlog, shared_ptr<GroupElement> h, biginteger w);
int zkPoKSinglePlainProver(shared_ptr<CommParty> prover, shared_ptr<OpenSSLDlogECFp> dlog, shared_ptr<ElGamalOnGroupElementEnc> elgamal, biginteger m, biginteger r);

/* Multiple AND */
int zkPoKMultiANDPlainProver(shared_ptr<CommParty> prover, shared_ptr<OpenSSLDlogECFp> dlog, shared_ptr<ElGamalOnGroupElementEnc> elgamal, vector<biginteger> vectM, vector<biginteger> vectR);

/* Verifier */

/* Simple */
bool zkPoKBasicVerifier(shared_ptr<CommParty> verifier, shared_ptr<OpenSSLDlogECFp> dlog, shared_ptr<GroupElement> h);
bool zkPoKSinglePlainVerifier(shared_ptr<CommParty> verifier, shared_ptr<OpenSSLDlogECFp> dlog, shared_ptr<ElGamalOnGroupElementEnc> elgamal, shared_ptr<AsymmetricCiphertext> cipher);

/* Multiple AND */
bool zkPoKMultiANDPlainVerifier(shared_ptr<CommParty> verifier, shared_ptr<OpenSSLDlogECFp> dlog, shared_ptr<ElGamalOnGroupElementEnc> elgamal, vector<shared_ptr<AsymmetricCiphertext>> vectCipher);

/**********************************\
|**** Non-Interactive ZKProofs ****|
\**********************************/

/* Prover */

/* Simple */
int niZKDecZeroProver(shared_ptr<CommParty> prover, shared_ptr<OpenSSLDlogECFp> dlog, shared_ptr<ElGamalOnGroupElementEnc> elgamal, shared_ptr<AsymmetricCiphertext> cipher, biginteger x);
int niZKBlindedProver(shared_ptr<CommParty> prover, shared_ptr<OpenSSLDlogECFp> dlog, shared_ptr<ElGamalOnGroupElementEnc> elgamal, shared_ptr<AsymmetricCiphertext> cipher, biginteger rBlind);
int niZKPartialDecProver(shared_ptr<CommParty> prover, shared_ptr<OpenSSLDlogECFp> dlog, shared_ptr<PublicKey> provThreshPubKey, shared_ptr<AsymmetricCiphertext> cipher, biginteger provPrivKey);

/* Multiple AND */
int niZKMultiANDDecZeroProver(shared_ptr<CommParty> prover, shared_ptr<OpenSSLDlogECFp> dlog, shared_ptr<ElGamalOnGroupElementEnc> elgamal, vector<shared_ptr<AsymmetricCiphertext>> vectCipher, biginteger x);
int niZKMultiANDBlindedProver(shared_ptr<CommParty> prover, shared_ptr<OpenSSLDlogECFp> dlog, shared_ptr<ElGamalOnGroupElementEnc> elgamal, vector<shared_ptr<AsymmetricCiphertext>> vectCipher, vector<biginteger> rBlindVect);
int niZKMultiANDPartialDecProver(shared_ptr<CommParty> prover, shared_ptr<OpenSSLDlogECFp> dlog, shared_ptr<PublicKey> provThreshPubKey, vector<shared_ptr<AsymmetricCiphertext>> vectCipher, biginteger provPrivKey);

/* Verifier */

/* Simple */
bool niZKDecZeroVerifier(shared_ptr<CommParty> verifier, shared_ptr<OpenSSLDlogECFp> dlog, shared_ptr<ElGamalOnGroupElementEnc> elgamal, shared_ptr<AsymmetricCiphertext> cipher);
bool niZKBlindedVerifier(shared_ptr<CommParty> verifier, shared_ptr<OpenSSLDlogECFp> dlog, shared_ptr<ElGamalOnGroupElementEnc> elgamal, shared_ptr<AsymmetricCiphertext> cipher, shared_ptr<AsymmetricCiphertext> blindedCipher);
bool niZKPartialDecVerifier(shared_ptr<CommParty> verifier, shared_ptr<OpenSSLDlogECFp> dlog, shared_ptr<PublicKey> provThreshPubKey, shared_ptr<AsymmetricCiphertext> cipher, shared_ptr<GroupElement> partialCipher);

/* Multiple AND */
bool niZKMultiANDDecZeroVerifier(shared_ptr<CommParty> verifier, shared_ptr<OpenSSLDlogECFp> dlog, shared_ptr<ElGamalOnGroupElementEnc> elgamal, vector<shared_ptr<AsymmetricCiphertext>> vectCipher);
bool niZKMultiANDBlindedVerifier(shared_ptr<CommParty> verifier, shared_ptr<OpenSSLDlogECFp> dlog, shared_ptr<ElGamalOnGroupElementEnc> elgamal, vector<shared_ptr<AsymmetricCiphertext>> vectCipher, vector<shared_ptr<AsymmetricCiphertext>> vectBlindedCipher);
bool niZKMultiANDPartialDecVerifier(shared_ptr<CommParty> verifier, shared_ptr<OpenSSLDlogECFp> dlog, shared_ptr<PublicKey> provThreshPubKey, vector<shared_ptr<AsymmetricCiphertext>> vectCipher, vector<shared_ptr<GroupElement>> partialCipherVect);


#ifndef _SOT_CORE_CAUSAL_FILTER_H_
#define _SOT_CORE_CAUSAL_FILTER_H_
/*
 * Copyright 2017-, Rohan Budhirja, LAAS-CNRS
 *
 * This file is part of sot-torque-control.
 * sot-torque-control is free software: you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public License
 * as published by the Free Software Foundation, either version 3 of
 * the License, or (at your option) any later version.
 * sot-torque-control 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 Lesser General Public License for more details.  You should
 * have received a copy of the GNU Lesser General Public License along
 * with sot-torque-control.  If not, see <http://www.gnu.org/licenses/>.
 */

/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <Eigen/Core>

/** \addtogroup Filters
    \section subsec_causalfilter CausalFilter
    Filter data with an IIR or FIR filter.

    Filter a data sequence, \f$x\f$, using a digital filter.
    The filter is a direct form II transposed implementation
    of the standard difference equation.
    This means that the filter implements:

    \f$ a[0]*y[N] = b[0]*x[N] + b[1]*x[N-1] + ... + b[m-1]*x[N-(m-1)]
    - a[1]*y[N-1] - ... - a[n-1]*y[N-(n-1)] \f$

    where \f$m\f$ is the degree of the numerator,
    \f$n\f$ is the degree of the denominator,
    and \f$N\f$ is the sample number

 */
namespace dynamicgraph {
namespace sot {

class CausalFilter {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  /** --- CONSTRUCTOR ----
      \param[in] timestep
      \param[in] xSize
      \param[in] filter_numerator
      \param[in] filter_denominator

      xSize is
  */
  CausalFilter(const double &timestep, const int &xSize,
               const Eigen::VectorXd &filter_numerator,
               const Eigen::VectorXd &filter_denominator);

  void get_x_dx_ddx(const Eigen::VectorXd &base_x,
                    Eigen::VectorXd &x_output_dx_ddx);

  void switch_filter(const Eigen::VectorXd &filter_numerator,
                     const Eigen::VectorXd &filter_denominator);

private:
  /// sampling timestep of the input signal
  double m_dt;
  /// Size
  int m_x_size;
  /// Size of the numerator \f$m\f$
  Eigen::VectorXd::Index m_filter_order_m;
  /// Size of the denominator \f$n\f$
  Eigen::VectorXd::Index m_filter_order_n;

  /// Coefficients of the numerator \f$b\f$
  Eigen::VectorXd m_filter_numerator;
  /// Coefficients of the denominator \f$a\f$
  Eigen::VectorXd m_filter_denominator;
  bool m_first_sample;
  ///
  int m_pt_numerator;
  int m_pt_denominator;
  Eigen::MatrixXd m_input_buffer;
  Eigen::MatrixXd m_output_buffer;
}; // class CausalFilter
} // namespace sot
} // namespace dynamicgraph
#endif /* _SOT_CORE_CAUSAL_FILTER_H_ */


#include <boost/math/special_functions/next.hpp>
#include <boost/random.hpp>

#include <limits>

#include "caffe/common.hpp"
#include "caffe/util/math_functions.hpp"
#include "caffe/util/rng.hpp"

namespace caffe {

template<>
void caffe_cpu_gemm<float>(const CBLAS_TRANSPOSE TransA,
    const CBLAS_TRANSPOSE TransB, const int M, const int N, const int K,
    const float alpha, const float* A, const float* B, const float beta,
    float* C) {
  int lda = (TransA == CblasNoTrans) ? K : M;
  int ldb = (TransB == CblasNoTrans) ? N : K;
  cblas_sgemm(CblasRowMajor, TransA, TransB, M, N, K, alpha, A, lda, B,
      ldb, beta, C, N);
}

template<>
void caffe_cpu_gemm<double>(const CBLAS_TRANSPOSE TransA,
    const CBLAS_TRANSPOSE TransB, const int M, const int N, const int K,
    const double alpha, const double* A, const double* B, const double beta,
    double* C) {
  int lda = (TransA == CblasNoTrans) ? K : M;
  int ldb = (TransB == CblasNoTrans) ? N : K;
  cblas_dgemm(CblasRowMajor, TransA, TransB, M, N, K, alpha, A, lda, B,
      ldb, beta, C, N);
}

template <>
void caffe_cpu_gemv<float>(const CBLAS_TRANSPOSE TransA, const int M,
    const int N, const float alpha, const float* A, const float* x,
    const float beta, float* y) {
  cblas_sgemv(CblasRowMajor, TransA, M, N, alpha, A, N, x, 1, beta, y, 1);
}

template <>
void caffe_cpu_gemv<double>(const CBLAS_TRANSPOSE TransA, const int M,
    const int N, const double alpha, const double* A, const double* x,
    const double beta, double* y) {
  cblas_dgemv(CblasRowMajor, TransA, M, N, alpha, A, N, x, 1, beta, y, 1);
}

template <>
void caffe_axpy<float>(const int N, const float alpha, const float* X,
    float* Y) { cblas_saxpy(N, alpha, X, 1, Y, 1); }

template <>
void caffe_axpy<double>(const int N, const double alpha, const double* X,
    double* Y) { cblas_daxpy(N, alpha, X, 1, Y, 1); }

template <typename Dtype>
void caffe_set(const int N, const Dtype alpha, Dtype* Y) {
  if (alpha == 0) {
    memset(Y, 0, sizeof(Dtype) * N);  // NOLINT(caffe/alt_fn)
    return;
  }
  for (int i = 0; i < N; ++i) {
    Y[i] = alpha;
  }
}

template void caffe_set<int>(const int N, const int alpha, int* Y);
template void caffe_set<float>(const int N, const float alpha, float* Y);
template void caffe_set<double>(const int N, const double alpha, double* Y);

template <>
void caffe_add_scalar(const int N, const float alpha, float* Y) {
  for (int i = 0; i < N; ++i) {
    Y[i] += alpha;
  }
}

template <>
void caffe_add_scalar(const int N, const double alpha, double* Y) {
  for (int i = 0; i < N; ++i) {
    Y[i] += alpha;
  }
}

template <typename Dtype>
void caffe_copy(const int N, const Dtype* X, Dtype* Y) {
  if (X != Y) {
    if (Caffe::mode() == Caffe::GPU) {
#ifndef CPU_ONLY
      // NOLINT_NEXT_LINE(caffe/alt_fn)
      CUDA_CHECK(cudaMemcpy(Y, X, sizeof(Dtype) * N, cudaMemcpyDefault));
#else
      NO_GPU;
#endif
    } else {
      memcpy(Y, X, sizeof(Dtype) * N);  // NOLINT(caffe/alt_fn)
    }
  }
}

template void caffe_copy<int>(const int N, const int* X, int* Y);
template void caffe_copy<unsigned int>(const int N, const unsigned int* X,
    unsigned int* Y);
template void caffe_copy<float>(const int N, const float* X, float* Y);
template void caffe_copy<double>(const int N, const double* X, double* Y);

template <>
void caffe_scal<float>(const int N, const float alpha, float *X) {
  cblas_sscal(N, alpha, X, 1);
}

template <>
void caffe_scal<double>(const int N, const double alpha, double *X) {
  cblas_dscal(N, alpha, X, 1);
}

template <>
void caffe_cpu_axpby<float>(const int N, const float alpha, const float* X,
                            const float beta, float* Y) {
  cblas_saxpby(N, alpha, X, 1, beta, Y, 1);
}

template <>
void caffe_cpu_axpby<double>(const int N, const double alpha, const double* X,
                             const double beta, double* Y) {
  cblas_daxpby(N, alpha, X, 1, beta, Y, 1);
}

template <>
void caffe_add<float>(const int n, const float* a, const float* b,
    float* y) {
  vsAdd(n, a, b, y);
}

template <>
void caffe_add<double>(const int n, const double* a, const double* b,
    double* y) {
  vdAdd(n, a, b, y);
}

template <>
void caffe_sub<float>(const int n, const float* a, const float* b,
    float* y) {
  vsSub(n, a, b, y);
}

template <>
void caffe_sub<double>(const int n, const double* a, const double* b,
    double* y) {
  vdSub(n, a, b, y);
}

template <>
void caffe_mul<float>(const int n, const float* a, const float* b,
    float* y) {
  vsMul(n, a, b, y);
}

template <>
void caffe_mul<double>(const int n, const double* a, const double* b,
    double* y) {
  vdMul(n, a, b, y);
}

template <>
void caffe_div<float>(const int n, const float* a, const float* b,
    float* y) {
  vsDiv(n, a, b, y);
}

template <>
void caffe_div<double>(const int n, const double* a, const double* b,
    double* y) {
  vdDiv(n, a, b, y);
}

template <>
void caffe_powx<float>(const int n, const float* a, const float b,
    float* y) {
  vsPowx(n, a, b, y);
}

template <>
void caffe_powx<double>(const int n, const double* a, const double b,
    double* y) {
  vdPowx(n, a, b, y);
}

template <>
void caffe_sqr<float>(const int n, const float* a, float* y) {
  vsSqr(n, a, y);
}

template <>
void caffe_sqr<double>(const int n, const double* a, double* y) {
  vdSqr(n, a, y);
}

template <>
void caffe_exp<float>(const int n, const float* a, float* y) {
  vsExp(n, a, y);
}

template <>
void caffe_exp<double>(const int n, const double* a, double* y) {
  vdExp(n, a, y);
}

template <>
void caffe_abs<float>(const int n, const float* a, float* y) {
    vsAbs(n, a, y);
}

template <>
void caffe_abs<double>(const int n, const double* a, double* y) {
    vdAbs(n, a, y);
}

unsigned int caffe_rng_rand() {
  return (*caffe_rng())();
}

template <typename Dtype>
Dtype caffe_nextafter(const Dtype b) {
  return boost::math::nextafter<Dtype>(
      b, std::numeric_limits<Dtype>::max());
}

template
float caffe_nextafter(const float b);

template
double caffe_nextafter(const double b);

template <typename Dtype>
void caffe_rng_uniform(const int n, const Dtype a, const Dtype b, Dtype* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_LE(a, b);
  boost::uniform_real<Dtype> random_distribution(a, caffe_nextafter<Dtype>(b));
  boost::variate_generator<caffe::rng_t*, boost::uniform_real<Dtype> >
      variate_generator(caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

template
void caffe_rng_uniform<float>(const int n, const float a, const float b,
                              float* r);

template
void caffe_rng_uniform<double>(const int n, const double a, const double b,
                               double* r);

template <typename Dtype>
void caffe_rng_gaussian(const int n, const Dtype a,
                        const Dtype sigma, Dtype* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GT(sigma, 0);
  boost::normal_distribution<Dtype> random_distribution(a, sigma);
  boost::variate_generator<caffe::rng_t*, boost::normal_distribution<Dtype> >
      variate_generator(caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

template
void caffe_rng_gaussian<float>(const int n, const float mu,
                               const float sigma, float* r);

template
void caffe_rng_gaussian<double>(const int n, const double mu,
                                const double sigma, double* r);

template <typename Dtype>
void caffe_rng_bernoulli(const int n, const Dtype p, int* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GE(p, 0);
  CHECK_LE(p, 1);
  boost::bernoulli_distribution<Dtype> random_distribution(p);
  boost::variate_generator<caffe::rng_t*, boost::bernoulli_distribution<Dtype> >
      variate_generator(caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

template
void caffe_rng_bernoulli<double>(const int n, const double p, int* r);

template
void caffe_rng_bernoulli<float>(const int n, const float p, int* r);

template <typename Dtype>
void caffe_rng_bernoulli(const int n, const Dtype p, unsigned int* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GE(p, 0);
  CHECK_LE(p, 1);
  boost::bernoulli_distribution<Dtype> random_distribution(p);
  boost::variate_generator<caffe::rng_t*, boost::bernoulli_distribution<Dtype> >
      variate_generator(caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = static_cast<unsigned int>(variate_generator());
  }
}

template
void caffe_rng_bernoulli<double>(const int n, const double p, unsigned int* r);

template
void caffe_rng_bernoulli<float>(const int n, const float p, unsigned int* r);

template <>
float caffe_cpu_strided_dot<float>(const int n, const float* x, const int incx,
    const float* y, const int incy) {
  return cblas_sdot(n, x, incx, y, incy);
}

template <>
double caffe_cpu_strided_dot<double>(const int n, const double* x,
    const int incx, const double* y, const int incy) {
  return cblas_ddot(n, x, incx, y, incy);
}

template <typename Dtype>
Dtype caffe_cpu_dot(const int n, const Dtype* x, const Dtype* y) {
  return caffe_cpu_strided_dot(n, x, 1, y, 1);
}

template
float caffe_cpu_dot<float>(const int n, const float* x, const float* y);

template
double caffe_cpu_dot<double>(const int n, const double* x, const double* y);

template <>
int caffe_cpu_hamming_distance<float>(const int n, const float* x,
                                  const float* y) {
  int dist = 0;
  for (int i = 0; i < n; ++i) {
    dist += __builtin_popcount(static_cast<uint32_t>(x[i]) ^
                               static_cast<uint32_t>(y[i]));
  }
  return dist;
}

template <>
int caffe_cpu_hamming_distance<double>(const int n, const double* x,
                                   const double* y) {
  int dist = 0;
  for (int i = 0; i < n; ++i) {
    dist += __builtin_popcountl(static_cast<uint64_t>(x[i]) ^
                                static_cast<uint64_t>(y[i]));
  }
  return dist;
}

template <>
float caffe_cpu_asum<float>(const int n, const float* x) {
  return cblas_sasum(n, x, 1);
}

template <>
double caffe_cpu_asum<double>(const int n, const double* x) {
  return cblas_dasum(n, x, 1);
}

template <>
void caffe_cpu_scale<float>(const int n, const float alpha, const float *x,
                            float* y) {
  cblas_scopy(n, x, 1, y, 1);
  cblas_sscal(n, alpha, y, 1);
}

template <>
void caffe_cpu_scale<double>(const int n, const double alpha, const double *x,
                             double* y) {
  cblas_dcopy(n, x, 1, y, 1);
  cblas_dscal(n, alpha, y, 1);
}
/* changes */

//calculate gamma in kernel function
//K: dimension of data
//S: size of all data
//N: num of output
//W: weights, N*K
//X: X (input of this layer)
template <>
float cal_gamma_cpu<float>(const int K, const int S, const int N, const float* W, const float* X, float* tempX1, float* tempX2){
    srand((unsigned int)time(0));
    float gamma = 0;
    float temp = 0;

    //random sample S pair to calculate gamma
    for(int i = 0;i < S;++i){
        memset(tempX1, 0, sizeof(float) * K);
        memset(tempX2, 0, sizeof(float) * K);
        int s1 = rand() % S;
        int s2 = rand() % S;
        s2 = (s1 != s2) ? s2 : (s2 + 1) % S;

        const float* x1 = X + s1 * K;
        const float* x2 = X + s2 * K;

        caffe_cpu_gemv<float>(CblasNoTrans, N, K, 1.0, W, x1, 0.0, tempX1);
        caffe_cpu_gemv<float>(CblasNoTrans, N, K, 1.0, W, x2, 0.0, tempX2);

        //caffe_cpu_sub<float>(K, tempX1, tempX2, tempX2);
        caffe_cpu_axpby(K, 1.0f, tempX1, -1.0f, tempX2);
        temp = caffe_cpu_dot<float>(K, tempX2, tempX2);
        gamma += temp;
    }
    return S / gamma;
}

//output:
//  tempX1: W*x1-W*x2
//  tempX2: x1-x2
//  KK: co * (x1-x2)^T * W^T  should be 1*N
template<>
void cal_add_item_cpu<float>(const float co, const int N, const int K, const float* W, const float* x1, float* tempX1,
    const float* x2, float* tempX2, const float gamma, float* KK){

    memset(tempX1, 0, sizeof(float) * K);
    memset(tempX2, 0, sizeof(float) * K);

    caffe_cpu_gemv<float>(CblasNoTrans, N, K, 1.0, W, x1, 0.0, tempX1);
    caffe_cpu_gemv<float>(CblasNoTrans, N, K, 1.0, W, x2, 0.0, tempX2);

    float square_sum = 0;

    caffe_cpu_axpby(K, -1.0f, tempX2, 1.0f, tempX1);
    caffe_cpu_axpby(K, 1.0f, x1, 0.0f, tempX2);
    caffe_cpu_axpby(K, -1.0f, x2, 1.0f, tempX2);
    square_sum = caffe_cpu_dot<float>(K, tempX1, tempX1);

    //calculate 2 * \gamma * kernel
    float kernel = 0.0f;
    float tempGamma = gamma / 4.0f;
    for(int i = 0;i < 5;++i){
        float temp = (0.0 - tempGamma) * square_sum;
        temp = exp(temp);
        kernel += 2 * tempGamma* temp;
        tempGamma = tempGamma * 2;
    }
    /*float kernel = (0.0 - gamma) * square_sum;
    kernel = exp(kernel);
    kernel = 2 * gamma * kernel;
*/
    //calculate KK <- co * kernel * X^T * W + 1 * KK
    caffe_cpu_gemm<float>(CblasNoTrans, CblasTrans, 1, N, K, co*kernel, tempX2, W, 0.0, KK);
}

template<>
void cal_add_item_cpu<double>(const double co, const int N, const int K, const double* W, const double* x1, double* tempX1,
    const double* x2, double* tempX2, const double gamma, double* KK){
    //TODO: complete double version of this function
}

// Gradient with respect to weight for MMD
      //N: number of output neuron
      //K: dimension of the feature
      //M: size of all data
      //S: size of source data in a batch
      //W: weight of this layer
      //X: input of this layer
      //gamma: gamma / learning rate
      //delta_W: gredient of weight

template<>
void caffe_cpu_mmd<float>(const int N, const int K, const int M, const int S, const int labeledTargetSize,
    const float* W, const float* X, const float gamma, float* delta_W){
    srand((unsigned int)time(0));

    //output the value of delta_W before MMD gradient
    float sum = 0;
    for(int i = 0;i < N;++i){
        for(int j = 0;j < K;++j){
            sum += (delta_W[i * N + j] > 0) ? delta_W[i*N+j] : (-1 * delta_W[i*N+j]);
        }
    }
    LOG(INFO) << "delta_W before MMD, sum = " << sum << ", average = " << sum / (N*K);

    float *KK = new float[N];
    float *tempX1 = new float[K];
    float *tempX2 = new float[K];

    float kernel_gamma = cal_gamma_cpu(K, M, N, W, X, tempX1, tempX2);
    int SS = (S>(M-S)) ? S : M-S;

    for(int i = 0;i < SS;++i){
        //random
        int s1 = rand() % S;
        int s2 = rand() % S;
        if(s1 == s2){
            s2 = (s2 + 100) % S;
        }
        int t1 = rand() % (M - S - labeledTargetSize);
        int t2 = rand() % (M - S - labeledTargetSize);
        if(t1 == t2){
            t2 = (t2 + 100) % (M - S - labeledTargetSize);
        }
        t1 = t1 + S + labeledTargetSize;
        t2 = t2 + S + labeledTargetSize;

        const float *x_s1 = X + s1 * K;
        const float *x_s2 = X + s2 * K;
        const float *x_t1 = X + t1 * K;
        const float *x_t2 = X + t2 * K;
        const float tempS = 1.0;

        //calculate four items of MMD gradient
        memset(KK, 0, sizeof(float) * N);
        cal_add_item_cpu<float>(-1, N, K, W, x_s1, tempX1, x_s2, tempX2, kernel_gamma, KK);
        caffe_cpu_gemm<float>(CblasNoTrans, CblasTrans, N, K, 1, tempS * gamma, KK,tempX2, 1.0, delta_W);

        memset(KK, 0, sizeof(float) * N);
        cal_add_item_cpu<float>(1, N, K, W, x_s1, tempX1, x_t2, tempX2, kernel_gamma, KK);
        caffe_cpu_gemm<float>(CblasNoTrans, CblasTrans, N, K, 1, tempS * gamma, KK,tempX2, 1.0, delta_W);

        memset(KK, 0, sizeof(float) * N);
        cal_add_item_cpu<float>(1, N, K, W, x_s2, tempX1, x_t1, tempX2, kernel_gamma, KK);
        caffe_cpu_gemm<float>(CblasNoTrans, CblasTrans, N, K, 1, tempS * gamma, KK,tempX2, 1.0, delta_W);

        memset(KK, 0, sizeof(float) * N);
        cal_add_item_cpu<float>(-1, N, K, W, x_t1, tempX1, x_t2, tempX2, kernel_gamma, KK);
        caffe_cpu_gemm<float>(CblasNoTrans, CblasTrans, N, K, 1, tempS * gamma, KK,tempX2, 1.0, delta_W);
    }
    //output the value of delta_W after MMD gradient
    sum = 0;
    for(int i = 0;i < N;++i){
        for(int j = 0;j < K;++j){
            sum += (delta_W[i * N + j] > 0) ? delta_W[i*N+j] : (-1 * delta_W[i*N+j]);
        }
    }
    LOG(INFO) << "delta_W after MMD, sum = " << sum << ", average = " << sum / (N*K);

    delete [] KK;
    delete [] tempX1;
    delete [] tempX2;
}

template<>
void caffe_cpu_mmd<double>(const int N, const int K, const int M, const int S, const int labeledTargetSize,
    const double* W, const double* X, const double gamma, double* delta_W){
    //TODO: complete the double version of this function
}

}  // namespace caffe


#include <CGAL/Simple_cartesian.h>
#include <CGAL/Arr_segment_traits_2.h>
#include <CGAL/Arrangement_2.h>
#include <CGAL/Polyhedron_3.h>
#include <CGAL/Surface_mesh.h>
#include <CGAL/Triangulation_2.h>
#include <CGAL/Delaunay_triangulation_3.h>
#include <CGAL/Delaunay_triangulation_cell_base_3.h>
#include <CGAL/Linear_cell_complex_for_combinatorial_map.h>
#include <CGAL/Linear_cell_complex_for_generalized_map.h>
#include <CGAL/boost/graph/graph_traits_Arrangement_2.h>
#include <CGAL/boost/graph/graph_traits_Triangulation_2.h>
#include <map>
#include <boost/unordered_map.hpp>
#include <CGAL/boost/graph/helpers.h>

typedef CGAL::Simple_cartesian<double> Kernel;
typedef CGAL::Arr_segment_traits_2<Kernel> Arrangement_traits_2;
typedef CGAL::Arrangement_2<Arrangement_traits_2> Arrangement_2;

typedef CGAL::Polyhedron_3<Kernel> Polyhedron;
typedef CGAL::Surface_mesh<Kernel::Point_3> Surface_mesh;
typedef CGAL::Triangulation_2<Kernel> Triangulation_2;
#ifdef CGAL_LINKED_WITH_TBB
typedef CGAL::Triangulation_data_structure_3<
    CGAL::Triangulation_vertex_base_3<Kernel>,
    CGAL::Delaunay_triangulation_cell_base_3<Kernel>,
    CGAL::Parallel_tag>                          Tds;
typedef CGAL::Delaunay_triangulation_3<Kernel, Tds> Triangulation_3;
#endif

typedef CGAL::Linear_cell_complex_for_combinatorial_map<3, 3> Linear_cell_complex_3_cmap;
typedef CGAL::Linear_cell_complex_for_generalized_map<3, 3> Linear_cell_complex_3_gmap;

template <typename P,typename Descriptor>
void
fct(const P& )
{
  std::map<Descriptor,int> M;
  Descriptor d;
  typename std::map<Descriptor,int>::const_iterator it = M.find(d);
  CGAL_USE(it);

  boost::unordered_map<Descriptor, int> U;
  U[d] = 12;
}

template<typename LCC>
void fct2()
{
  typedef typename LCC::Dart_handle dh;
  typedef typename LCC::Vertex_attribute_handle vh;

  { // For dart handle
  std::map<dh, int> M;
  dh e;
  typename std::map<dh, int>::const_iterator it = M.find(e);
  CGAL_USE(it);

  boost::unordered_map<dh, int> U;
  U[e] = 12;
  }

  { // For vertex attribute handle
  std::map<vh, int> M;
  vh e;
  typename std::map<vh, int>::const_iterator it = M.find(e);
  CGAL_USE(it);

  boost::unordered_map<vh, int> U;
  U[e] = 12;
  }
}

template <typename P>
void
fct3(const P& )
{
  typedef typename P::Vertex_handle vertex_descriptor;

  std::map<vertex_descriptor,int> M;
  vertex_descriptor vd;
  M.find(vd);

  boost::unordered_map<vertex_descriptor, int> U;
  U[vd] = 12;
}

template <class P>
void test_edge_hash_and_null(const P& p)
{
  typedef boost::graph_traits<P> GT;
  typedef typename GT::halfedge_descriptor halfedge_descriptor;
  typedef typename GT::vertex_descriptor vertex_descriptor;
  typedef typename GT::face_descriptor face_descriptor;

  for(halfedge_descriptor h : halfedges(p))
  {
    assert(
      hash_value( edge(h,p) ) ==
      hash_value( edge(opposite(h,p),p) ) );
  }
  face_descriptor f1=GT::null_face(), f2=GT::null_face();
  vertex_descriptor v1=GT::null_vertex(), v2=GT::null_vertex();
  halfedge_descriptor h1=GT::null_halfedge(), h2=GT::null_halfedge();

  assert(hash_value(f1)==hash_value(f2));
  assert(hash_value(v1)==hash_value(v2));
  assert(hash_value(h1)==hash_value(h2));
}

template <typename P>
void
fct4(const P& p)
{
  fct<P, typename boost::graph_traits<P>::vertex_descriptor>(p);
  fct<P, typename boost::graph_traits<P>::halfedge_descriptor>(p);
  fct<P, typename boost::graph_traits<P>::edge_descriptor>(p);
  fct<P, typename boost::graph_traits<P>::face_descriptor>(p);
  test_edge_hash_and_null(p);
}

int main()
{
  Arrangement_2 A;
  fct<Arrangement_2, boost::graph_traits<Arrangement_2>::vertex_descriptor>(A);
  fct<Arrangement_2, boost::graph_traits<Arrangement_2>::edge_descriptor>(A);

  Kernel::Point_3 p3;
  Polyhedron P;
  CGAL::make_triangle(p3,p3,p3,P);
  fct4(P);

  Surface_mesh S;
  CGAL::make_triangle(p3,p3,p3,S);
  fct4(S);

  Triangulation_2 T;
  T.insert(Kernel::Point_2(0,0));
  T.insert(Kernel::Point_2(0,1));
  T.insert(Kernel::Point_2(1,0));
  T.insert(Kernel::Point_2(1,1));
  fct4(T);

  fct2<Linear_cell_complex_3_cmap>();
  fct2<Linear_cell_complex_3_gmap>();

#ifdef CGAL_LINKED_WITH_TBB
  Triangulation_3 T3;
  fct3(T3);
#endif

  return 0;
}


/*
 * Copyright (C) 2020 Open Source Robotics Foundation
 *
 * 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 <rmf_planner_viz/draw/Capsule.hpp>

#include <SFML/Graphics/VertexArray.hpp>
#include <SFML/Graphics/RenderTarget.hpp>

#include <Eigen/Geometry>
#include <cmath>

namespace rmf_planner_viz {
namespace draw {

//==============================================================================
class Capsule::Implementation
{
public:

  Implementation(
      const sf::Vertex& v0,
      const sf::Vertex& v1,
      float radius,
      std::size_t resolution)
    : cap_0(sf::TriangleFan, resolution+1),
      cap_1(sf::TriangleFan, resolution+1),
      center(sf::Triangles, 6),
      resolution(resolution)
  {
    const auto& p0 = v0.position;
    this->p0 = Eigen::Vector2f(p0.x, p0.y);
    const auto& p1 = v1.position;
    this->p1 = Eigen::Vector2f(p1.x, p1.y);
    this->radius = radius;

    const sf::Vector2f dp = p1 - p0;
    const float length = std::sqrt(dp.x*dp.x + dp.y*dp.y);
    const sf::Vector2f cross = sf::Vector2f(-dp.y, dp.x)/length*radius;

    const sf::Vector2f c0a = p0 + cross;
    const sf::Vector2f c0b = p0 - cross;
    const sf::Vector2f c1a = p1 + cross;
    const sf::Vector2f c1b = p1 - cross;

    center[0].position = c0a;
    center[1].position = c0b;
    center[2].position = c1a;

    center[3].position = c1a;
    center[4].position = c1b;
    center[5].position = c0b;

    const auto rotate = [](const sf::Vector2f& p, float theta) -> sf::Vector2f
    {
      const float ct = std::cos(theta);
      const float st = std::sin(theta);
      return sf::Vector2f(p.x*ct - p.y*st, p.x*st + p.y*ct);
    };

    cap_0[0].position = p0;

    cap_1[0].position = p1;

    for (std::size_t i=0; i < resolution; ++i)
    {
      const float s = static_cast<float>(i)/static_cast<float>(resolution-1);
      const float theta = s*M_PI;
      cap_0[i+1].position = p0 + rotate(cross, theta);
      cap_1[i+1].position = p1 - rotate(cross, theta);
    }

    set_start_color(v0.color);
    set_end_color(v1.color);
  }

  void set_start_color(const sf::Color& color)
  {
    center[0].color = color;
    center[1].color = color;
    center[5].color = color;

    cap_0[0].color = color;
    for (std::size_t i = 0; i < resolution; ++i)
      cap_0[i+1].color = color;
  }

  void set_end_color(const sf::Color& color)
  {
    center[2].color = color;
    center[3].color = color;
    center[4].color = color;

    cap_1[0].color = color;
    for (std::size_t i = 0; i < resolution; ++i)
      cap_1[i+1].color = color;
  }

  sf::VertexArray cap_0;
  sf::VertexArray cap_1;
  sf::VertexArray center;

  Eigen::Vector2f p0;
  Eigen::Vector2f p1;
  float radius;
  std::size_t resolution;
};

//==============================================================================
Capsule::Capsule(
    const sf::Vertex& v0,
    const sf::Vertex& v1,
    float radius,
    std::size_t resolution)
  : _pimpl(rmf_utils::make_impl<Implementation>(v0, v1, radius, resolution))
{
  // Do nothing
}

//==============================================================================
void Capsule::draw(sf::RenderTarget& target, sf::RenderStates states) const
{
  target.draw(_pimpl->center, states);
  target.draw(_pimpl->cap_0, states);
  target.draw(_pimpl->cap_1, states);
}

//==============================================================================
bool Capsule::pick(float x, float y) const
{
  const Eigen::Vector2f p(x, y);
  const Eigen::Vector2f p0 = _pimpl->p0;
  const Eigen::Vector2f p1 = _pimpl->p1;
  const float radius = _pimpl->radius;

  if ((p0-p).norm() <= radius)
    return true;

  if ((p1-p).norm() <= radius)
    return true;

  const float lane_length = (p1 - p0).norm();
  if (lane_length < 1e-8)
    return false;

  const Eigen::Vector2f pn = (p1 - p0)/lane_length;
  const Eigen::Vector2f p_l = p - p0;
  const float p_l_projection = p_l.dot(pn);

  if (p_l_projection < 0.0)
    return false;

  if (lane_length < p_l_projection)
    return false;

  const double lane_dist = (p_l - p_l_projection*pn).norm();
  if (lane_dist <= radius)
    return true;

  return false;
}

//==============================================================================
Capsule& Capsule::set_start_color(const sf::Color& color)
{
  _pimpl->set_start_color(color);
  return *this;
}

//==============================================================================
Capsule& Capsule::set_end_color(const sf::Color& color)
{
  _pimpl->set_end_color(color);
  return *this;
}

} // namespace draw
} // namespace rmf_planner_viz


/*********************************************************************
 * BSD 3-Clause License
 *
 * Copyright (c) 2020 Northwestern University
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *  * Redistributions of source code must retain the above copyright notice, this
 *    list of conditions and the following disclaimer.
 *
 *  * Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 *  * Neither the name of the copyright holder nor the names of its
 *    contributors may be used to endorse or promote products derived from
 *    this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/
/**
 * @file mapping.hpp
 * @author Boston Cleek
 * @date 8 Nov 2020
 * @brief Occupancy grid mapping
 */
#ifndef MAPPING_HPP
#define MAPPING_HPP

#include <armadillo>

#include <ergodic_exploration/grid.hpp>
#include <sensor_msgs/LaserScan.h>

namespace ergodic_exploration
{
using arma::mat;
using arma::vec;

/** @brief Occupancy grid mapping */
class OccupancyMapper
{
public:
  /**
   * @brief Constructor
   * @param Tbs - transformation from base link to frame of laser scanner
   */
  OccupancyMapper(const mat& Tbs);

  /**
   * @brief Update the occupancy grid map
   * @param grid[out] - current map
   * @param scan - laser scan
   * @param pose - robot pose in map frame
   * @return true if map is updated
   */
  bool updateMap(GridMap& grid, const sensor_msgs::LaserScan::ConstPtr& scan,
                 const vec& pose);

private:
  /**
   * @brief Bresenham's line algorithm on occupancy map
   * @param grid[out] - current map
   * @param x0 - line starting x position
   * @param y0 - line starting y position
   * @param x1 - line ending x position
   * @param y1 - line ending y position
   * @details Ray tracing is used to find the grid cells in free space.
   * Where x is the jth column and u is the ith row in the grid.
   */
  void rayTrace(GridMap& grid, int x0, int y0, int x1, int y1) const;

  /**
   * @brief Bresenham's algorithm for drawing lines downward
   * @param grid[out] - current map
   * @param x0 - line starting x position
   * @param y0 - line starting y position
   * @param x1 - line ending x position
   * @param y1 - line ending y position
   * @details x is the jth column and u is the ith row in the grid
   */
  void lineLow(GridMap& grid, int x0, int y0, int x1, int y1) const;

  /**
   * @brief Bresenham's algorithm for drawing lines upward
   * @param grid[out] - current map
   * @param x0 - line starting x position
   * @param y0 - line starting y position
   * @param x1 - line ending x position
   * @param y1 - line ending y position
   * @details x is the jth column and u is the ith row in the grid
   */
  void lineHigh(GridMap& grid, int x0, int y0, int x1, int y1) const;

  /**
   * @brief Bresenham's algorithm for drawing lines at +/- 45 deg
   * @param grid[out] - current map
   * @param x0 - line starting x position
   * @param y0 - line starting y position
   * @param x1 - line ending x position
   * @param y1 - line ending y position
   * @details x is the jth column and u is the ith row in the grid
   */
  void lineDiag(GridMap& grid, int x0, int y0, int x1, int y1) const;

  /**
   * @brief Update grid cell probability of occupancy
   * @param grid[out] - current map
   * @param x - grid cell x position
   * @param y - grid cell y position
   * @param log - either log_odds_occ_ or log_odds_free_
   * @details x is the jth column and u is the ith row in the grid
   */
  void updateCell(GridMap& grid, int x, int y, double log) const;

private:
  mat Tbs_;                              // transform base link to laser scanner
  double prior_, prob_occ_, prob_free_;  // probabilities for cell states
  double log_odds_prior_;                // log odds prior
  double log_odds_occ_;                  // log odds occupied
  double log_odds_free_;                 // log odds free
};
}  // namespace ergodic_exploration
#endif


/*
 * This is part of the fl library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2015 Max Planck Society,
 * 				 Autonomous Motion Department,
 * 			     Institute for Intelligent Systems
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file gaussian_test.cpp
 * \date October 2014
 * \author Jan Issac (jan.issac@gmail.com)
 */

#include <gtest/gtest.h>
#include "../typecast.hpp"

#include <Eigen/Dense>

#include <cmath>
#include <iostream>

#include <fl/distribution/decorrelated_gaussian.hpp>

template <typename TestType>
class DecorrelatedGaussianTests
    : public testing::Test
{
public:
    typedef typename TestType::Parameter Configuration;

    enum: signed int
    {
        Dim = Configuration::Dim,
        Size = fl::TestSize<Dim, TestType>::Value
    };

    typedef Eigen::Matrix<fl::Real, Size, 1> Vector;

protected:
    template <typename Gaussian, typename Covariance>
    void test_gaussian_attributes(Gaussian& gaussian,
                                  const Covariance& covariance,
                                  const Covariance& precision,
                                  const Covariance& square_root)
    {
        EXPECT_GT(gaussian.dimension(), 0);

        EXPECT_TRUE(
            fl::are_similar(gaussian.covariance().diagonal(),
                            covariance.diagonal()));

        EXPECT_TRUE(
            fl::are_similar(gaussian.precision().diagonal(),
                            precision.diagonal()));

        EXPECT_TRUE(
            fl::are_similar(gaussian.square_root().diagonal(),
                            square_root.diagonal()));

        EXPECT_TRUE(gaussian.has_full_rank());
    }

    template <typename Gaussian>
    void test_gaussian_covariance(Gaussian& gaussian)
    {
        typedef typename Gaussian::SecondMoment SecondMoment;

        SecondMoment covariance;
        SecondMoment square_root;
        SecondMoment precision;

        covariance.setIdentity(Dim);
        precision.setIdentity(Dim);
        square_root.setIdentity(Dim);

        // first verify standard gaussian
        {
            SCOPED_TRACE("Unchanged");

            test_gaussian_attributes(
                        gaussian, covariance, precision, square_root);
        }

        // set covariance and verify representations
        {
            SCOPED_TRACE("Covariance setter");

            covariance.diagonal().setRandom(Dim);
            covariance.diagonal() = covariance.diagonal().cwiseProduct(
                                        covariance.diagonal());

            square_root.diagonal() = covariance.diagonal().cwiseSqrt();
            precision.diagonal() = covariance.diagonal().cwiseInverse();

            gaussian.covariance(covariance);

            test_gaussian_attributes(
                        gaussian, covariance, precision, square_root);
        }

        // set square root and verify representations
        {
            SCOPED_TRACE("SquareRoot setter");

            covariance.diagonal().setRandom(Dim);
            covariance.diagonal() = covariance.diagonal().cwiseProduct(
                                        covariance.diagonal());
            square_root.diagonal() = covariance.diagonal().cwiseSqrt();
            precision.diagonal() = covariance.diagonal().cwiseInverse();

            gaussian.square_root(square_root);

            test_gaussian_attributes(
                        gaussian, covariance, precision, square_root);
        }

        // set covariance and verify representations
        {
            SCOPED_TRACE("Precision setter");

            covariance.diagonal().setRandom(Dim);
            covariance.diagonal() = covariance.diagonal().cwiseProduct(
                                        covariance.diagonal());

            square_root.diagonal() = covariance.diagonal().cwiseSqrt();
            precision.diagonal() = covariance.diagonal().cwiseInverse();

            gaussian.precision(precision);

            test_gaussian_attributes(
                        gaussian, covariance, precision, square_root);
        }
    }
};

TYPED_TEST_CASE_P(DecorrelatedGaussianTests);

TYPED_TEST_P(DecorrelatedGaussianTests, dimension)
{
    typedef TestFixture This;
    typedef fl::DecorrelatedGaussian<typename This::Vector> Gaussian;

    auto gaussian = Gaussian(This::Dim);

    EXPECT_EQ(gaussian.dimension(), This::Dim);
    EXPECT_EQ(gaussian.standard_variate_dimension(), This::Dim);
    EXPECT_EQ(gaussian.mean().size(), This::Dim);
    EXPECT_EQ(gaussian.covariance().rows(), This::Dim);
    EXPECT_EQ(gaussian.precision().rows(), This::Dim);
    EXPECT_EQ(gaussian.square_root().rows(), This::Dim);

    auto noise = Gaussian::StandardVariate::Random(
                        gaussian.standard_variate_dimension(), 1).eval();

    EXPECT_EQ(gaussian.map_standard_normal(noise).size(), This::Dim);
}

TYPED_TEST_P(DecorrelatedGaussianTests, standard_covariance)
{
    typedef TestFixture This;
    typedef fl::DecorrelatedGaussian<typename This::Vector> Gaussian;
    auto gaussian = Gaussian(This::Dim);

    This::test_gaussian_covariance(gaussian);
}

TYPED_TEST_P(DecorrelatedGaussianTests, dynamic_uninitialized_gaussian)
{
    typedef TestFixture This;
    typedef fl::DecorrelatedGaussian<typename This::Vector> Gaussian;

    auto gaussian = Gaussian();

    if (This::Size != Eigen::Dynamic)
    {
        EXPECT_NO_THROW(gaussian.covariance());
        EXPECT_NO_THROW(gaussian.precision());
        EXPECT_NO_THROW(gaussian.square_root());
    }
    else
    {
        EXPECT_THROW(gaussian.covariance(), fl::GaussianUninitializedException);
        EXPECT_THROW(gaussian.precision(), fl::GaussianUninitializedException);
        EXPECT_THROW(gaussian.square_root(), fl::GaussianUninitializedException);

        gaussian.dimension(1);
        EXPECT_NO_THROW(gaussian.covariance());
        EXPECT_NO_THROW(gaussian.precision());
        EXPECT_NO_THROW(gaussian.square_root());

        gaussian.dimension(0);
        EXPECT_THROW(gaussian.covariance(), fl::GaussianUninitializedException);
        EXPECT_THROW(gaussian.precision(), fl::GaussianUninitializedException);
        EXPECT_THROW(gaussian.square_root(), fl::GaussianUninitializedException);
    }
}

TYPED_TEST_P(DecorrelatedGaussianTests, gaussian_covariance_dimension_init)
{
    typedef TestFixture This;
    typedef fl::DecorrelatedGaussian<typename This::Vector> Gaussian;

    auto gaussian = Gaussian();

    gaussian.dimension(This::Dim);
    EXPECT_NO_THROW(This::test_gaussian_covariance(gaussian));
}

TYPED_TEST_P(DecorrelatedGaussianTests, gaussian_covariance_constructor_init)
{
    typedef TestFixture This;
    typedef fl::DecorrelatedGaussian<typename This::Vector> Gaussian;

    auto gaussian = Gaussian(This::Dim);
    EXPECT_NO_THROW(This::test_gaussian_covariance(gaussian));
}

REGISTER_TYPED_TEST_CASE_P(DecorrelatedGaussianTests,
                           dimension,
                           standard_covariance,
                           dynamic_uninitialized_gaussian,
                           gaussian_covariance_dimension_init,
                           gaussian_covariance_constructor_init);

template <int Dimension>
struct TestConfiguration
{
    enum: signed int { Dim = Dimension };
};

typedef ::testing::Types<
            fl::StaticTest<TestConfiguration<2>>,
            fl::StaticTest<TestConfiguration<3>>,
            fl::StaticTest<TestConfiguration<10>>,
            fl::StaticTest<TestConfiguration<10000>>,
            fl::DynamicTest<TestConfiguration<2>>,
            fl::DynamicTest<TestConfiguration<3>>,
            fl::DynamicTest<TestConfiguration<10>>,
            fl::DynamicTest<TestConfiguration<1000000>>
        > TestTypes;

INSTANTIATE_TYPED_TEST_CASE_P(DecorrelatedGaussianTestCases,
                              DecorrelatedGaussianTests,
                              TestTypes);


/**
 * Variable ordering for the independent set problem
 */

#ifndef INDEPSET_ORDERINGS_HPP_
#define INDEPSET_ORDERINGS_HPP_

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/discrete_distribution.hpp>

#include "../../core/order.hpp"
#include "indepset_instance.hpp"
#include "indepset_state.hpp"

using namespace std;

/** Return an ordering for the independent set problem given an id */
Ordering* get_ordering_by_id_indepset(int id, IndepSetInstance* inst, Options& options);

// Vertex that it is in the least number of states - with option to choose randomly with probability (1-prob)
struct MinInState : Ordering {

	IndepSetInstance* inst;
	boost::random::mt19937 gen;
	int* in_state_counter;          /**< number of states containing each variable */
	double prob;			        /**< probability min in state is applied (otherwise random) */

	MinInState(IndepSetInstance* _inst, double _prob = 1) : inst(_inst), prob(_prob)
	{
		sprintf(name, "min_in_state");

		assert(prob >= 0 && prob <= 1);
		if (prob < 1) {
			gen.seed(inst->graph->n_vertices + inst->graph->n_edges);
		}

		in_state_counter = new int[inst->graph->n_vertices];
	}

	~MinInState()
	{
		delete[] in_state_counter;
	}

	int select_next_var(int layer);
	int select_vertex_with_min_in_state(int layer);
	int select_vertex_randomly(int layer);

	void cb_initialize();
	void cb_state_created(State* state);
	void cb_state_removed(State* state);
};

// Maximal Path Decomposition
struct MaximalPathDecomp : Ordering {

	IndepSetInstance* inst;
	vector<int> v_in_layer;   // vertex at each layer

	MaximalPathDecomp(IndepSetInstance* _inst) : inst(_inst)
	{
		sprintf(name, "maxpath");
		construct_ordering();
	}

	int select_next_var(int layer)
	{
		assert(layer >= 0 && layer < inst->graph->n_vertices);
		return v_in_layer[layer];
	}

private:
	void construct_ordering();
};

// Minimum degree ordering
struct MinDegreeOrdering : Ordering {

	IndepSetInstance* inst;
	vector<int> v_in_layer;   // vertex at each layer

	MinDegreeOrdering(IndepSetInstance* _inst) : inst(_inst)
	{
		sprintf(name, "mindegree");
		construct_ordering();
	}

	int select_next_var(int layer)
	{
		assert(layer >= 0 && layer < inst->graph->n_vertices);
		return v_in_layer[layer];
	}

private:
	void construct_ordering();
};

// Cut vertex decomposition
struct CutVertexDecomposition : Ordering {

	IndepSetInstance* inst;
	vector<int> v_in_layer;      // vertex at each layer
	bool** original_adj_matrix;

	CutVertexDecomposition(IndepSetInstance* _inst, bool general = true) : inst(_inst)
	{
		sprintf(name, "cut-vertex");
		v_in_layer.resize(inst->graph->n_vertices);

		// If a general graph (not a tree), take a spanning tree
		if (general) {
			restrict_graph();
		}
		construct_ordering();
		if (general) {
			regenerate_graph();
		}
	}

	int select_next_var(int layer)
	{
		assert(layer >= 0 && layer < inst->graph->n_vertices);
		return v_in_layer[layer];
	}

private:
	void        restrict_graph();
	void        regenerate_graph();
	void        construct_ordering();
	void        identify_components(vector< vector<int> >& comps, vector<bool>& is_in_graph);
	vector<int> find_ordering(vector<bool> is_in_graph);
};

#endif


/// \file
/// Maintainer: Luzian Hug
/// Created: 25.03.2018
///
///

#include "mean_shift.h"
#include "spatial/flann.h"
#include "spatial/uniform_grid.h"
#include <Eigen/Dense>
#include <boost/log/trivial.hpp>
#include <iostream>

namespace MouseTrack {

MeanShift::MeanShift(double window_size) : _window_size(window_size) {
  // empty
}

std::vector<Cluster> MeanShift::operator()(const PointCloud &cloud) const {
  BOOST_LOG_TRIVIAL(trace) << "MeanShift algorithm started";
  // Convert point cloud to Eigen vectors

  if (cloud.size() == 0) {
    return std::vector<Cluster>();
  }

  const int dimensions = cloud.charDim();

  Oracle::PointList points;
  points.resize(dimensions, cloud.size());
  for (PointIndex i = 0; i < cloud.size(); i += 1) {
    auto v = cloud[i].characteristic();
    points.col(i) = v;
  }

  bool normalize = false;
  if (normalize) {
    Eigen::VectorXd min = points.rowwise().minCoeff();
    Eigen::VectorXd max = points.rowwise().maxCoeff();
    Eigen::VectorXd bb_size = max - min;
    points = (points.array().colwise()) / bb_size.array();
  }

  std::vector<Eigen::VectorXd> currCenters = convergePoints(points);

  BOOST_LOG_TRIVIAL(debug) << "Merging points to clusters";
  // Merge step

  std::vector<Cluster> clusters = mergePoints(currCenters);

  BOOST_LOG_TRIVIAL(trace) << "MeanShift converged! #Clusters: "
                           << clusters.size();
  return clusters;
}

std::vector<Eigen::VectorXd>
MeanShift::convergePoints(const Oracle::PointList &points) const {

  const int dimensions = points.rows();

  std::vector<Eigen::VectorXd> currCenters, pointsVec;

  for (int i = 0; i < points.cols(); i += 1) {
    auto v = points.col(i);
    currCenters.push_back(v);
    pointsVec.push_back(v);
  }

  std::unique_ptr<Oracle> oraclePtr;
  {
    OFactory::Query q;
    q.maxR = 2 * _window_size;
    q.dimensions = dimensions;
    oraclePtr = oracleFactory().forQuery(q);
  }
  Oracle &oracle = *oraclePtr;

  // Initialize some stuff used in the MeanShift loop
  Eigen::VectorXd prevCenter;

  oracle.compute(points);

  // For each point...
  for (PointIndex i = 0; i < currCenters.size(); i++) {
    int iterations = 0; // for logging and abort condition

    // ... iterate until convergence
    do {
      iterations++;
      // perform one iteration of mean shift
      prevCenter = currCenters[i];
      std::vector<PointIndex> locals =
          oracle.find_in_range(currCenters[i], 2 * _window_size)[0];
      if (locals.empty()) {
        BOOST_LOG_TRIVIAL(warning)
            << "No points in neighborhood, falling back to brute force.";
        currCenters[i] = iterate_mode(currCenters[i], pointsVec);
        break;
      } else {
        std::vector<Eigen::VectorXd> localPoints;
        for (int li : locals) {
          localPoints.push_back(points.col(li));
        }

        currCenters[i] = iterate_mode(currCenters[i], localPoints);
      }

      if (iterations > _max_iterations) {
        BOOST_LOG_TRIVIAL(warning)
            << "Max number of " << _max_iterations << " iterations for point "
            << i << " reached - continuing without convergence for this point";
        break;
      }
    } while ((prevCenter - currCenters[i]).norm() > _convergence_threshold);
    if (i % 1024 == 0) {
      BOOST_LOG_TRIVIAL(trace)
          << "converged i " << i << " after " << iterations << " iterations";
    }
  }
  return currCenters;
}

std::vector<Cluster>
MeanShift::mergePoints(std::vector<Eigen::VectorXd> &currCenters) const {
  // Initialize Clusters. Initially, every point has its own cluster.
  const size_t nPoints = currCenters.size();
  std::vector<Cluster> clusters(nPoints);
  for (PointIndex i = 0; i < nPoints; i++) {
    clusters[i].points().push_back(i);
  }
  // For every mode..
  int merged = 0;
  for (size_t i = 0; i < currCenters.size(); i++) {
    if (merged % 1024 == 0) {
      BOOST_LOG_TRIVIAL(trace) << merged << " points merged, i: " << i
                               << ", remaining: " << currCenters.size() - i;
    }
    // Check modes we haven't already executed the (i)-loop for
    for (size_t j = i + 1; j < currCenters.size(); j++) {
      Eigen::VectorXd diff = currCenters[i] - currCenters[j];
      if (diff.norm() < _merge_threshold) {
        // Merge clusters. Erase one of the modes corresponding to the clusters
        // and append points belonging to j to cluser of i
        clusters[i].points().push_back(clusters[j].points()[0]);
        clusters.erase(clusters.begin() + j);
        currCenters.erase(currCenters.begin() + j);
        // We need to decrement i here because there might be another cluster
        // that wants to merge with i.
        i--;
        ++merged;
        break;
      }
    }
  }
  return clusters;
}

double MeanShift::gaussian_weight(const Eigen::VectorXd point,
                                  const Eigen::VectorXd mean) const {

  // assert if dimensions match
  assert(mean.size() == point.size());

  // Euclidean distance squared btw. mean of kernel and point of interest
  const double d = (mean - point).squaredNorm();

  // Apply gaussian distribution
  return exp(-d / (2 * _window_size));
}

Eigen::VectorXd
MeanShift::iterate_mode(const Eigen::VectorXd mode,
                        const std::vector<Eigen::VectorXd> &fixedPoints) const {
  // COG Normalization Factor
  double normfact = 0;
  // Rest of COG
  Eigen::VectorXd cog = Eigen::VectorXd::Zero(mode.size());
  for (size_t i = 0; i < fixedPoints.size(); i++) {
    double temp = gaussian_weight(fixedPoints[i], mode);
    normfact += temp;
    cog += temp * fixedPoints[i];
  }
  cog = cog * (1.0 / normfact);
  return cog;
}

void MeanShift::setMaxIterations(int max_iterations) {
  _max_iterations = max_iterations;
}

int MeanShift::getMaxIterations() const { return _max_iterations; }

void MeanShift::setMergeThreshold(double merge_threshold) {
  _merge_threshold = merge_threshold;
}
double MeanShift::getMergeThreshold() const { return _merge_threshold; }

void MeanShift::setConvergenceThreshold(double convergence_threshold) {
  _convergence_threshold = convergence_threshold;
}
double MeanShift::getConvergenceThreshold() const {
  return _convergence_threshold;
}

void MeanShift::setWindowSize(double window_size) {
  _window_size = window_size;
}
double MeanShift::getWindowSize() const { return _window_size; }

MeanShift::OFactory &MeanShift::oracleFactory() { return _oracleFactory; }

const MeanShift::OFactory &MeanShift::oracleFactory() const {
  return _oracleFactory;
}

} // namespace MouseTrack


#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <limits.h>
#include <string>
#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include <set>
#include <boost/tuple/tuple.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/lexical_cast.hpp>

#include "routing.hpp"

using namespace std;
using namespace boost;

/* build forwarding tables does what it sounds like it does.  It takes in
 * the BGL Graph + Routes and a Routing Algorithm, and returns a next-hop
 * list for every node to every other node based on the algorithm given.
*/
next_hop build_forwarding_tables(Graph G, int route, Routes *R){
  DEBUGE("Building Tables\n");
  switch(route){
    case 1:
      return ospf(G,R);
      break;
    case 2: //bellman-ford?
    case 3: //multipath?
      DEBUGE("Non Implemented Routing Algorithms");
      exit(1);
      break;
  }
  next_hop null(1);
  return null;
}

next_hop ospf(Graph G, Routes *R){
  print_graph(G);  //verify graph is correct

  graph_traits<Graph>::vertex_iterator vi, vend;
  int verts = num_vertices(G);
  next_hop H(verts, std::vector<vertex_descriptor>(verts));
  node_hop parent(verts);

  for (boost::tie(vi,vend) = vertices(G); vi != vend; ++vi){
    vertex_descriptor s = *vi;
    int node = boost::lexical_cast<int>(G[*vi].name);

  /*
  *  Run Dijkstra's, need to pass in the graph, the node to run dijkstra's
  *  from, and pass in the weight map that will get converted into a distance
  *  map.
  */
    Graph G_copy(num_vertices(G));

    //weight map and predecessor maps are define in BGL, use a unique version which
    //uses my structures and builds the next-hop data structure.
    boost::dijkstra_shortest_paths (G,s,
    weight_map(get(&edge_info::weight,
               G)).distance_map(make_iterator_property_map((*R)[node].begin(),
               get(vertex_index, G))).
    predecessor_map(make_iterator_property_map((H)[node].begin(),
               get(vertex_index, G), (((H)[node])[0]) )).
    visitor(make_dijkstra_visitor(copy_graph(G_copy, on_examine_edge()))));

  }

/* TURN ON TO VERIFY GRAPH

  for (int src=0; src < (int)num_vertices(G); src++){
    for (int dest=0; dest < (int)num_vertices(G); dest++){
      DEBUGE("Next hop from: %d to get to: %d is ::  %d\n", src, dest, (H)[src][dest]);
    }
  }
  DEBUGE("After Dij\n");
  // Verify that dijkstra is working, print distance from each node.
  //SHRT_MAX defn in limits.h = 32767
#ifdef PRINT_DEBUG
  for (int i = 0; i < (int)num_vertices(G); i++){ // if verts in graph > 32767, overflow negative
    DEBUGE("Node %d | Distance\n", i);
    vector<double> distances = (*R)[i];
    for (boost::tie(vi,vend) = vertices(G); vi != vend; ++vi){
      DEBUGE("distance(%s) = %f\n",G[*vi].name.c_str(),distances[*vi]);
    }
  }
#endif
*/

  return H;

}


//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/math_basic_types.h>
#include <OpenTissue/collision/gjk/gjk_reduce_edge.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

#include <cmath>

using namespace OpenTissue;

BOOST_AUTO_TEST_SUITE(opentissue_collision_gjk_reduce_edge);

BOOST_AUTO_TEST_CASE(case_by_case_test)
{
  typedef OpenTissue::math::BasicMathTypes<double, size_t> math_types;
  typedef math_types::vector3_type                         vector3_type;
  typedef math_types::real_type                            real_type;

  typedef OpenTissue::gjk::Simplex<vector3_type>           simplex_type;

  // First we create a simplex that represents an edge
  vector3_type const a = vector3_type(1.0, 0.0, 0.0);
  vector3_type const b = vector3_type(0.0, 0.0, 0.0);

  // First we use a test point that does not lie on the line

  // Front side of A voronoi plane
  {
    simplex_type S;

    OpenTissue::gjk::add_point_to_simplex( a, a, a, S);
    OpenTissue::gjk::add_point_to_simplex( b, b, b, S);

    vector3_type const p = vector3_type( 1.5, 1.0,  1.0);

    OpenTissue::gjk::detail::reduce_edge( p, S );

    BOOST_CHECK( OpenTissue::gjk::dimension( S ) == 1u );

    int bit_A    = 0;
    size_t idx_A = 0;
    OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A );

    BOOST_CHECK(S.m_bitmask == bit_A );
    BOOST_CHECK(S.m_v[idx_A] == a);
    BOOST_CHECK(S.m_a[idx_A] == a);
    BOOST_CHECK(S.m_b[idx_A] == a);
    BOOST_CHECK_CLOSE(S.m_w[idx_A], 1.0, 0.01);

  }
  // Back side of A voronoi plane
  {
    simplex_type S;

    OpenTissue::gjk::add_point_to_simplex( a, a, a, S);
    OpenTissue::gjk::add_point_to_simplex( b, b, b, S);

    vector3_type const p = vector3_type( 0.5, 1.0,  1.0);

    OpenTissue::gjk::detail::reduce_edge( p, S );

    BOOST_CHECK( OpenTissue::gjk::dimension( S ) == 2u );

    int bit_A    = 0;
    size_t idx_A = 0;
    int bit_B    = 0;
    size_t idx_B = 0;
    OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A, idx_B, bit_B );

    BOOST_CHECK( S.m_bitmask == (bit_A | bit_B));

    BOOST_CHECK(S.m_v[idx_A] == a);
    BOOST_CHECK(S.m_a[idx_A] == a);
    BOOST_CHECK(S.m_b[idx_A] == a);
    BOOST_CHECK_CLOSE(S.m_w[idx_A], 0.5, 0.01);

    BOOST_CHECK(S.m_v[idx_B] == b);
    BOOST_CHECK(S.m_a[idx_B] == b);
    BOOST_CHECK(S.m_b[idx_B] == b);
    BOOST_CHECK_CLOSE(S.m_w[idx_B], 0.5, 0.01);
  }
  // In A voronoi plane
  {
    simplex_type S;

    OpenTissue::gjk::add_point_to_simplex( a, a, a, S);
    OpenTissue::gjk::add_point_to_simplex( b, b, b, S);

    vector3_type const p = vector3_type( 1.0, 1.0,  1.0);

    OpenTissue::gjk::detail::reduce_edge( p, S );

    BOOST_CHECK( OpenTissue::gjk::dimension( S ) == 1u );

    int bit_A    = 0;
    size_t idx_A = 0;
    OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A );

    BOOST_CHECK(S.m_bitmask == bit_A );
    BOOST_CHECK(S.m_v[idx_A] == a);
    BOOST_CHECK(S.m_a[idx_A] == a);
    BOOST_CHECK(S.m_b[idx_A] == a);
    BOOST_CHECK_CLOSE(S.m_w[idx_A], 1.0, 0.01);
  }

  // Front side of B voronoi plane
  {
    simplex_type S;

    OpenTissue::gjk::add_point_to_simplex( a, a, a, S);
    OpenTissue::gjk::add_point_to_simplex( b, b, b, S);

    vector3_type const p = vector3_type( -0.5, 1.0,  1.0);

    OpenTissue::gjk::detail::reduce_edge( p, S );

    BOOST_CHECK( OpenTissue::gjk::dimension( S ) == 1u );

    int bit_A    = 0;
    size_t idx_A = 0;
    OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A );

    BOOST_CHECK(S.m_bitmask == bit_A );
    BOOST_CHECK(S.m_v[idx_A] == b);
    BOOST_CHECK(S.m_a[idx_A] == b);
    BOOST_CHECK(S.m_b[idx_A] == b);
    BOOST_CHECK_CLOSE(S.m_w[idx_A], 1.0, 0.01);
  }
  // Back side of B voronoi plane
  {
    simplex_type S;

    OpenTissue::gjk::add_point_to_simplex( a, a, a, S);
    OpenTissue::gjk::add_point_to_simplex( b, b, b, S);

    vector3_type const p = vector3_type( 0.5, 1.0,  1.0);

    OpenTissue::gjk::detail::reduce_edge( p, S );

    BOOST_CHECK( OpenTissue::gjk::dimension( S ) == 2u );

    int bit_A    = 0;
    size_t idx_A = 0;
    int bit_B    = 0;
    size_t idx_B = 0;
    OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A, idx_B, bit_B );

    BOOST_CHECK( S.m_bitmask == (bit_A | bit_B));

    BOOST_CHECK(S.m_v[idx_A] == a);
    BOOST_CHECK(S.m_a[idx_A] == a);
    BOOST_CHECK(S.m_b[idx_A] == a);
    BOOST_CHECK_CLOSE(S.m_w[idx_A], 0.5, 0.01);

    BOOST_CHECK(S.m_v[idx_B] == b);
    BOOST_CHECK(S.m_a[idx_B] == b);
    BOOST_CHECK(S.m_b[idx_B] == b);
    BOOST_CHECK_CLOSE(S.m_w[idx_B], 0.5, 0.01);
  }
  // In B voronoi plane
  {
    simplex_type S;

    OpenTissue::gjk::add_point_to_simplex( a, a, a, S);
    OpenTissue::gjk::add_point_to_simplex( b, b, b, S);

    vector3_type const p = vector3_type( 0.0, 1.0,  1.0);

    OpenTissue::gjk::detail::reduce_edge( p, S );

    BOOST_CHECK( OpenTissue::gjk::dimension( S ) == 1u );

    int bit_A    = 0;
    size_t idx_A = 0;
    OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A );

    BOOST_CHECK(S.m_bitmask == bit_A );
    BOOST_CHECK(S.m_v[idx_A] == b);
    BOOST_CHECK(S.m_a[idx_A] == b);
    BOOST_CHECK(S.m_b[idx_A] == b);
    BOOST_CHECK_CLOSE(S.m_w[idx_A], 1.0, 0.01);
  }

  // Second we use a test point that lies on the line

  // Front side of A voronoi plane
  {
    simplex_type S;

    OpenTissue::gjk::add_point_to_simplex( a, a, a, S);
    OpenTissue::gjk::add_point_to_simplex( b, b, b, S);

    vector3_type const p = vector3_type( 1.5, 0.0,  0.0);

    OpenTissue::gjk::detail::reduce_edge( p, S );

    BOOST_CHECK( OpenTissue::gjk::dimension( S ) == 1u );

    int bit_A    = 0;
    size_t idx_A = 0;
    OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A );

    BOOST_CHECK(S.m_bitmask == bit_A );
    BOOST_CHECK(S.m_v[idx_A] == a);
    BOOST_CHECK(S.m_a[idx_A] == a);
    BOOST_CHECK(S.m_b[idx_A] == a);
    BOOST_CHECK_CLOSE(S.m_w[idx_A], 1.0, 0.01);
  }
  // Back side of A voronoi plane
  {
    simplex_type S;

    OpenTissue::gjk::add_point_to_simplex( a, a, a, S);
    OpenTissue::gjk::add_point_to_simplex( b, b, b, S);

    vector3_type const p = vector3_type( 0.5, 0.0,  0.0);

    OpenTissue::gjk::detail::reduce_edge( p, S );

    BOOST_CHECK( OpenTissue::gjk::dimension( S ) == 2u );

    int bit_A    = 0;
    size_t idx_A = 0;
    int bit_B    = 0;
    size_t idx_B = 0;
    OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A, idx_B, bit_B );

    BOOST_CHECK( S.m_bitmask == (bit_A | bit_B));

    BOOST_CHECK(S.m_v[idx_A] == a);
    BOOST_CHECK(S.m_a[idx_A] == a);
    BOOST_CHECK(S.m_b[idx_A] == a);
    BOOST_CHECK_CLOSE(S.m_w[idx_A], 0.5, 0.01);

    BOOST_CHECK(S.m_v[idx_B] == b);
    BOOST_CHECK(S.m_a[idx_B] == b);
    BOOST_CHECK(S.m_b[idx_B] == b);
    BOOST_CHECK_CLOSE(S.m_w[idx_B], 0.5, 0.01);
  }
  // In A voronoi plane
  {
    simplex_type S;

    OpenTissue::gjk::add_point_to_simplex( a, a, a, S);
    OpenTissue::gjk::add_point_to_simplex( b, b, b, S);

    vector3_type const p = vector3_type( 1.0, 0.0,  0.0);

    OpenTissue::gjk::detail::reduce_edge( p, S );

    BOOST_CHECK( OpenTissue::gjk::dimension( S ) == 1u );

    int bit_A    = 0;
    size_t idx_A = 0;
    OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A );

    BOOST_CHECK(S.m_bitmask == bit_A );
    BOOST_CHECK(S.m_v[idx_A] == a);
    BOOST_CHECK(S.m_a[idx_A] == a);
    BOOST_CHECK(S.m_b[idx_A] == a);
    BOOST_CHECK_CLOSE(S.m_w[idx_A], 1.0, 0.01);
  }

  // Front side of B voronoi plane
  {
    simplex_type S;

    OpenTissue::gjk::add_point_to_simplex( a, a, a, S);
    OpenTissue::gjk::add_point_to_simplex( b, b, b, S);

    vector3_type const p = vector3_type( -0.5, 0.0,  0.0);

    OpenTissue::gjk::detail::reduce_edge( p, S );

    BOOST_CHECK( OpenTissue::gjk::dimension( S ) == 1u );

    int bit_A    = 0;
    size_t idx_A = 0;
    OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A );

    BOOST_CHECK(S.m_bitmask == bit_A );
    BOOST_CHECK(S.m_v[idx_A] == b);
    BOOST_CHECK(S.m_a[idx_A] == b);
    BOOST_CHECK(S.m_b[idx_A] == b);
    BOOST_CHECK_CLOSE(S.m_w[idx_A], 1.0, 0.01);
  }
  // Back side of B voronoi plane
  {
    simplex_type S;

    OpenTissue::gjk::add_point_to_simplex( a, a, a, S);
    OpenTissue::gjk::add_point_to_simplex( b, b, b, S);

    vector3_type const p = vector3_type( 0.5, 0.0,  0.0);

    OpenTissue::gjk::detail::reduce_edge( p, S );

    BOOST_CHECK( OpenTissue::gjk::dimension( S ) == 2u );

    int bit_A    = 0;
    size_t idx_A = 0;
    int bit_B    = 0;
    size_t idx_B = 0;
    OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A, idx_B, bit_B );

    BOOST_CHECK( S.m_bitmask == (bit_A | bit_B));

    BOOST_CHECK(S.m_v[idx_A] == a);
    BOOST_CHECK(S.m_a[idx_A] == a);
    BOOST_CHECK(S.m_b[idx_A] == a);
    BOOST_CHECK_CLOSE(S.m_w[idx_A], 0.5, 0.01);

    BOOST_CHECK(S.m_v[idx_B] == b);
    BOOST_CHECK(S.m_a[idx_B] == b);
    BOOST_CHECK(S.m_b[idx_B] == b);
    BOOST_CHECK_CLOSE(S.m_w[idx_B], 0.5, 0.01);
  }
  // In B voronoi plane
  {
    simplex_type S;

    OpenTissue::gjk::add_point_to_simplex( a, a, a, S);
    OpenTissue::gjk::add_point_to_simplex( b, b, b, S);

    vector3_type const p = vector3_type( 0.0, 0.0,  0.0);

    OpenTissue::gjk::detail::reduce_edge( p, S );

    BOOST_CHECK( OpenTissue::gjk::dimension( S ) == 1u );

    int bit_A    = 0;
    size_t idx_A = 0;
    OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A );

    BOOST_CHECK(S.m_bitmask == bit_A );
    BOOST_CHECK(S.m_v[idx_A] == b);
    BOOST_CHECK(S.m_a[idx_A] == b);
    BOOST_CHECK(S.m_b[idx_A] == b);
    BOOST_CHECK_CLOSE(S.m_w[idx_A], 1.0, 0.01);
  }

  // Assymetric test case when simplex is AB
  {
    simplex_type S;

    OpenTissue::gjk::add_point_to_simplex( a, a, a, S);
    OpenTissue::gjk::add_point_to_simplex( b, b, b, S);

    vector3_type const p = (2.0*a + 3.0*b)/5.0;

    OpenTissue::gjk::detail::reduce_edge( p, S );

    BOOST_CHECK( OpenTissue::gjk::dimension( S ) == 2u );

    int bit_A    = 0;
    size_t idx_A = 0;
    int bit_B    = 0;
    size_t idx_B = 0;
    OpenTissue::gjk::get_used_indices( S.m_bitmask, idx_A, bit_A, idx_B, bit_B );

    BOOST_CHECK( S.m_bitmask == (bit_A | bit_B));

    BOOST_CHECK(S.m_v[idx_A] == a);
    BOOST_CHECK(S.m_a[idx_A] == a);
    BOOST_CHECK(S.m_b[idx_A] == a);
    BOOST_CHECK_CLOSE(S.m_w[idx_A], 0.4, 0.01);

    BOOST_CHECK(S.m_v[idx_B] == b);
    BOOST_CHECK(S.m_a[idx_B] == b);
    BOOST_CHECK(S.m_b[idx_B] == b);
    BOOST_CHECK_CLOSE(S.m_w[idx_B], 0.6, 0.01);
  }

}

BOOST_AUTO_TEST_SUITE_END();