/* Copyright (C) 2012-2017 IBM Corp.
 * This program is 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. See accompanying LICENSE file.
 */

/* Test_Replicate.cpp - Testing the functionality of replicating one
 * slot from a vector acress the whole vector (or replicating each slot
 * to a full cipehrtext)
 */

#include <cassert>
#include <NTL/lzz_pXFactoring.h>
NTL_CLIENT

#include "FHE.h"
#include "replicate.h"
#include "timing.h"
#include "ArgMap.h"

static bool noPrint = false;

static bool check_replicate(const Ctxt& c1, const Ctxt& c0, long i,
			    const FHESecKey& sKey, const EncryptedArray& ea)
{
  PlaintextArray pa0(ea), pa1(ea);
  ea.decrypt(c0, sKey, pa0);
  ea.decrypt(c1, sKey, pa1);
  replicate(ea, pa0, i);

  return equals(ea, pa1, pa0); // returns true if replication succeeded
}

class StopReplicate { };

// A class that handles the replicated ciphertexts one at a time
class ReplicateTester : public ReplicateHandler {
public:
  const FHESecKey& sKey;
  const EncryptedArray& ea;
  const PlaintextArray& pa;
  long B;

  double t_last, t_total;
  long pos;
  bool error;

  ReplicateTester(const FHESecKey& _sKey, const EncryptedArray& _ea,
                  const PlaintextArray& _pa, long _B)
  : sKey(_sKey), ea(_ea), pa(_pa), B(_B)
  {
    t_last = GetTime();
    t_total = 0.0;
    pos = 0;
    error = false;
  }

  // This method is called for every replicated ciphertext: in the i'th time
  // that it is called, the cipehrtext will have in all the slots the content
  // of the i'th input slot. In this test program we only decrypt and check
  // the result, in a real program it will do something with the cipehrtext.
  virtual void handle(const Ctxt& ctxt) {

    double t_new = GetTime();
    double t_elapsed = t_new - t_last;
    t_total += t_elapsed;

    // Decrypt and check
    PlaintextArray pa1 = pa;
    replicate(ea, pa1, pos);
    PlaintextArray pa2(ea);

    if (pos==0 && !noPrint) CheckCtxt(ctxt, "replicateAll");

    ea.decrypt(ctxt, sKey, pa2);
    if (!equals(ea, pa1, pa2)) error = true; // record the error, if any
    t_last = GetTime();

    pos++;
    if (B > 0 && pos >= B) throw StopReplicate();
  }
};

void  TestIt(long m, long p, long r, long d, long L, long bnd, long B)
{
  if (!noPrint)
    std::cout << "*** TestIt" << (isDryRun()? "(dry run):" : ":")
	 << " m=" << m
	 << ", p=" << p
	 << ", r=" << r
	 << ", d=" << d
	 << ", L=" << L
	 << ", bnd=" << bnd
	 << ", B=" << B
	 << endl;

  setTimersOn();
  FHEcontext context(m, p, r);
  buildModChain(context, L, /*c=*/2);

  ZZX G;
  if (d == 0)
    G = context.alMod.getFactorsOverZZ()[0];
  else
    G = makeIrredPoly(p, d);

  if (!noPrint) {
    context.zMStar.printout();
    cout << endl;
    cout << "G = " << G << "\n";
  }

  FHESecKey secretKey(context);
  const FHEPubKey& publicKey = secretKey;
  secretKey.GenSecKey(); // A +-1/0 secret key
  addSome1DMatrices(secretKey); // compute key-switching matrices that we need

  EncryptedArray ea(context, G);
  PlaintextArray xp0(ea), xp1(ea);
  random(ea, xp0);
  random(ea, xp1);

  Ctxt xc0(publicKey);
  ea.encrypt(xc0, publicKey, xp0);

  ZZX poly_xp1;
  ea.encode(poly_xp1, xp1);

  if (!noPrint)  cout << "** Testing replicate():\n";
  bool error = false;
  Ctxt xc1 = xc0;
  if (!noPrint) CheckCtxt(xc1, "before replicate");
  replicate(ea, xc1, ea.size()/2);
  if (!check_replicate(xc1, xc0, ea.size()/2, secretKey, ea)) error = true;
  if (!noPrint) CheckCtxt(xc1, "after replicate");

  // Get some timing results
  for (long i=0; i<20 && i<ea.size(); i++) {
    xc1 = xc0;
    FHE_NTIMER_START(replicate);
    replicate(ea, xc1, i);
    if (!check_replicate(xc1, xc0, i, secretKey, ea)) error = true;
    FHE_NTIMER_STOP(replicate);
  }
  cout << (error? "BAD" : "GOOD") << endl;

  if (!noPrint) {
    printAllTimers();
    cout << "\n** Testing replicateAll()... " << std::flush;
  }
#ifdef DEBUG_PRINTOUT
  replicateVerboseFlag = true;
#else
  replicateVerboseFlag = false;
#endif

  error = false;
  ReplicateTester *handler = new ReplicateTester(secretKey, ea, xp0, B);
  try {
    FHE_NTIMER_START(replicateAll);
    replicateAll(ea, xc0, handler, bnd);
  }
  catch (StopReplicate) {
  }
  std::cout << (handler->error? "BAD" : "GOOD") << endl;
  if (!noPrint)
    cout << "  total time=" << handler->t_total << " ("
         << ((B>0)? B : ea.size()) << " vectors)\n";
  delete handler;
}

int main(int argc, char *argv[])
{
  ArgMap amap;

  bool dry=false;
  amap.arg("dry", dry, "dry=1 for a dry-run");

  long m=2047;
  amap.arg("m", m, "cyclotomic ring");

  long p=2;
  amap.arg("p", p, "plaintext base");

  long r=1;
  amap.arg("r", r,  "lifting");

  long d=1;
  amap.arg("d", d, "degree of the field extension");
  amap.note("d == 0 => factors[0] defines extension");

  long L=250;
  amap.arg("L", L, "# of bits in the modulus chain");

  long bnd = 64;
  amap.arg("bnd", bnd, "recursion bound for replication");

  long B = 0;
  amap.arg("B", B, "bound for # of replications", "all");

  amap.arg("noPrint", noPrint, "suppress printouts");

  amap.parse(argc, argv);
  setDryRun(dry);

  TestIt(m, p, r, d, L, bnd, B);
  cout << endl;
}


/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2013, Rice 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 Rice University 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 OWNER 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.
*********************************************************************/

/* Author: Javier V. Gomez */

// TODO: encapsulate heuristics in the MotionCompare function.
// TODO: udpdate documentation for name changes (set_w, set_h)
// TODO: ask if I should do this on the FMT as well.
// TODO: think about the possibility of inherit from FMT.
// TODO: remove saveTree function.

#include <ompl/base/spaces/RealVectorStateSpace.h>
#include <ompl/geometric/SimpleSetup.h>
#include <ompl/geometric/planners/fmt/FMT.h>
#include <ompl/geometric/planners/fmt/FMTHeuristic.h>
#include <ompl/util/PPM.h>
#include <ompl/base/PredefinedStateSampler.h>

#include <ompl/config.h>
#include <../tests/resources/config.h>

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

namespace ob = ompl::base;
namespace og = ompl::geometric;

void preSampleFree(const ob::SpaceInformationPtr si, const size_t n, std::vector<const ob::State*> &states)
{
    size_t statesSampled = 0;
    ob::StateSamplerPtr sampler = si->allocStateSampler();
    states.reserve(n);
    ob::State* s = si->allocState();

    while (statesSampled < n)
    {
        sampler->sampleUniform(s);
        bool collision_free = si->isValid(s);
        if (collision_free)
        {
            ++statesSampled;
            states.push_back(s);
            s = si->allocState();
        } // If collision free
    } // While statesSampled < n
}

struct PredefinedStateSamplerAllocator
{
    PredefinedStateSamplerAllocator(const std::vector<const ob::State*> &states) : states_(states) {}

    ob::StateSamplerPtr operator()(const ob::StateSpace* space)
    {
        return ob::StateSamplerPtr(new ob::PredefinedStateSampler(space, states_));
    }

    const std::vector<const ob::State*>& states_;
};

template<typename planner_t>
class Plane2DEnvironment
{
public:

    Plane2DEnvironment(const char *ppm_file)
    {
        bool ok = false;
        try
        {
            ppm_.loadFile(ppm_file);
            ok = true;
        }
        catch(ompl::Exception &ex)
        {
            OMPL_ERROR("Unable to load %s.\n%s", ppm_file, ex.what());
        }
        if (ok)
        {
            ob::RealVectorStateSpace *space = new ob::RealVectorStateSpace();
            space->addDimension(0.0, ppm_.getWidth());
            space->addDimension(0.0, ppm_.getHeight());
            maxWidth_ = ppm_.getWidth() - 1;
            maxHeight_ = ppm_.getHeight() - 1;
            ss_.reset(new og::SimpleSetup(ob::StateSpacePtr(space)));

            // set state validity checking for this space
            ss_->setStateValidityChecker(boost::bind(&Plane2DEnvironment::isStateValid, this, _1));
            space->setup();
            ss_->getSpaceInformation()->setStateValidityCheckingResolution(1.0 / space->getMaximumExtent());

            // configuring underlying planner
            //og::FMTHeuristic* fmt = new og::FMTHeuristic(ss_->getSpaceInformation());
            ss_->setPlanner(ob::PlannerPtr( new planner_t(ss_->getSpaceInformation()) ));
        }
    }

    bool plan(unsigned int start_row, unsigned int start_col, unsigned int goal_row, unsigned int goal_col)
    {
        if (!ss_)
            return false;
        ob::ScopedState<> start(ss_->getStateSpace());
        start[0] = start_row;
        start[1] = start_col;
        ob::ScopedState<> goal(ss_->getStateSpace());
        goal[0] = goal_row;
        goal[1] = goal_col;
        ss_->setStartAndGoalStates(start, goal);
        ss_->solve();

        const std::size_t ns = ss_->getProblemDefinition()->getSolutionCount();
        OMPL_INFORM("Found %d solutions", (int)ns);
        if (ss_->haveSolutionPath())
        {
            /*ss_->simplifySolution();
            og::PathGeometric &p = ss_->getSolutionPath();
            ss_->getPathSimplifier()->simplifyMax(p);
            ss_->getPathSimplifier()->smoothBSpline(p);*/
            return true;
        }
        else
            return false;
    }

    void recordSolution()
    {
        if (!ss_ || !ss_->haveSolutionPath())
            return;
        og::PathGeometric &p = ss_->getSolutionPath();
        p.interpolate();
        for (std::size_t i = 0 ; i < p.getStateCount() ; ++i)
        {
            const int w = std::min(maxWidth_, (int)p.getState(i)->as<ob::RealVectorStateSpace::StateType>()->values[0]);
            const int h = std::min(maxHeight_, (int)p.getState(i)->as<ob::RealVectorStateSpace::StateType>()->values[1]);
            ompl::PPM::Color &c = ppm_.getPixel(h, w);
            c.red = 255;
            c.green = 0;
            c.blue = 0;
        }
    }

    void save(const char *filename)
    {
        if (!ss_)
            return;
        ppm_.saveFile(filename);
    }

    const ob::SpaceInformationPtr getSpaceInformation() const
    {
        return ss_->getSpaceInformation();
    }

    void saveTree(const std::string &filename)
    {
        ss_->getPlanner()->as<og::FMTHeuristic>()->saveTree(filename);
    }

    void setStateSamplerAllocator(const ob::StateSamplerAllocator ssa)
    {
        ss_->getStateSpace()->setStateSamplerAllocator(ssa);
    }

private:

    bool isStateValid(const ob::State *state) const
    {
        const int w = std::min((int)state->as<ob::RealVectorStateSpace::StateType>()->values[0], maxWidth_);
        const int h = std::min((int)state->as<ob::RealVectorStateSpace::StateType>()->values[1], maxHeight_);

        const ompl::PPM::Color &c = ppm_.getPixel(h, w);
        return c.red > 127 && c.green > 127 && c.blue > 127;
    }

    og::SimpleSetupPtr ss_;
    int maxWidth_;
    int maxHeight_;
    ompl::PPM ppm_;
};

int main(int argc, char ** argv)
{
    boost::filesystem::path path(TEST_RESOURCES_DIR);

    //Plane2DEnvironment<og::FMT> env((path / "ppm/empty.ppm").string().c_str());
    //Plane2DEnvironment<og::FMTHeuristic> env2((path / "ppm/empty.ppm").string().c_str());
    Plane2DEnvironment<og::FMT> env((path / "ppm/floor.ppm").string().c_str());
    Plane2DEnvironment<og::FMTHeuristic> env2((path / "ppm/floor.ppm").string().c_str());

    // Filling the predefined sampler.
    const size_t n = 1000;
    std::vector<const ob::State*> states;
    preSampleFree(env.getSpaceInformation(), n, states);
    PredefinedStateSamplerAllocator pssa(states);
    env.setStateSamplerAllocator(pssa);
    env2.setStateSamplerAllocator(pssa);

    if (env.plan(atoi(argv[1]), atoi(argv[2]),atoi(argv[3]),atoi(argv[4])))
    {
        env.recordSolution();
        env.saveTree("tree.txt");
        env.save("result_demo.ppm");
    }

    if (env2.plan(atoi(argv[1]), atoi(argv[2]),atoi(argv[3]),atoi(argv[4])))
    {
        env2.recordSolution();
        env2.saveTree("tree2.txt");
        env2.save("result_demo2.ppm");
    }

    return 0;
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_SEC_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SEC_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-trigonometric
    Function object implementing sec capabilities

    secant of the angle in radian: \f$1/\cos(x)\f$.

    @par Semantic:

    For every parameter of floating type

    @code
    auto r = sec(x);
    @endcode

    @see cos, secd, secpi, rec

  **/
  Value sec(Value const & v0);
} }
#endif

#include <boost/simd/function/scalar/sec.hpp>
#include <boost/simd/function/simd/sec.hpp>

#endif


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_ABS_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ABS_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-arithmetic
    The function object @c abs computes the absolute value of its parameter.
    If the result cannot be represented by the return type the return is equal to the input.

    @par Header <boost/simd/function/abs.hpp>

    @par Notes

    - Be aware that for signed integers the absolute value of @ref Valmin is
      not representable  in the input type and the result is undefined.

      To avoid this, you may use the `saturated_` decorator (See @ref group-decorator.)

    - @c abs is a also a standard library function name and there possibly exists
    a C macro version which may be called instead of the boost simd version.
    To avoid this you may prefix abs using boost::simd::abs notation.

    @par Decorators

     - `std_`  (See @ref group-decorator) for floating entries results in a call to std::abs,
       in this case the behavior is also undefined if the result cannot be represented by the
       return type.
     - `saturated_` (See @ref group-decorator) garanties that saturated_(abs)(x) will never
       be strictly less than 0. In fact the only change if that for any signed type T
       saturated_(abs)(Valmin<T>()) will be Valmax<T>()) which is already true for
       boost::simd::abs with floating types but not for integral signed types.

    @see sqr_abs, sqr

    @par Example:

      @snippet abs.cpp abs

    @par Possible output:

      @snippet abs.txt abs

  **/
  Value abs(Value const& x);
} }
#endif

#include <boost/simd/function/scalar/abs.hpp>
#include <boost/simd/function/simd/abs.hpp>

#endif


//          Copyright Maksym V. Bilinets 2015 - 2019.
// Distributed under the Boost Software License, Version 1.0.
//      (See accompanying file LICENSE.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt )

#include "tools/marking_tree_invariant.h"
#include "tools/trees_generator.h"

#include <dst/allocator/global_counter_allocator.h>
#include <dst/binary_tree/mixin/marking.h>
#include <dst/binary_tree/tree.h>

#include <boost/test/unit_test.hpp>

#include <cstddef> // std::size_t
#include <iostream>
#include <iterator> // std::advance
#include <numeric>  // std::iota

namespace dst_test
{

enum red_t
{
  red
};

enum green_t
{
  green
};

enum blue_t
{
  blue
};

using rgb_tree = dst::binary_tree::tree<int,
                                        std::allocator<int>,
                                        dst::binary_tree::Marking<red_t>,
                                        dst::binary_tree::Marking<green_t>,
                                        dst::binary_tree::Marking<blue_t>>;

BOOST_AUTO_TEST_SUITE(test_binary_tree_marking)

BOOST_AUTO_TEST_CASE(test_rgb_tree_rotations)
{
  rgb_tree t = generate_fibonacci_tree(10);

  BOOST_TEST((std::distance(t.cbegin().base(), t.croot()) + 1) == 10);

  std::iota(t.begin(), t.end(), 0);

  const auto it_end = t.cend();
  for (auto it = t.cbegin(); it != it_end; ++it)
  {
    t.mark(it, red);

    if (*it % 2)
      t.mark(it, green);

    if (*it % 3)
      t.mark(it, blue);
  }

  std::vector<int> red_nodes(t.begin_marked(red), t.end_marked(red));
  std::vector<int> green_nodes(t.begin_marked(green), t.end_marked(green));
  std::vector<int> blue_nodes(t.begin_marked(blue), t.end_marked(blue));

  BOOST_TEST(rgb_tree::marked_nodes(t.croot(), red) == t.size());
  BOOST_TEST(rgb_tree::marked_nodes(t.croot(), green) == t.size() / 2);
  BOOST_TEST(rgb_tree::marked_nodes(t.croot(), blue) == 2 * t.size() / 3);

  BOOST_TEST(marking_invariant_holds(t, red));
  BOOST_TEST(marking_invariant_holds(t, green));
  BOOST_TEST(marking_invariant_holds(t, blue));

  auto it = t.croot();
  while (!!it)
  {
    it = t.rotate_right(it);

    while (!!it && !left(it))
    {
      it = right(it);
    }

    BOOST_TEST(std::vector<int>(t.begin_marked(red), t.end_marked(red)) ==
                 red_nodes,
               boost::test_tools::per_element());

    BOOST_TEST(std::vector<int>(t.begin_marked(green), t.end_marked(green)) ==
                 green_nodes,
               boost::test_tools::per_element());

    BOOST_TEST(std::vector<int>(t.begin_marked(blue), t.end_marked(blue)) ==
                 blue_nodes,
               boost::test_tools::per_element());

    BOOST_TEST(marking_invariant_holds(t, red));
    BOOST_TEST(marking_invariant_holds(t, green));
    BOOST_TEST(marking_invariant_holds(t, blue));
  }

  BOOST_TEST((std::distance(t.cbegin().base(), t.croot()) + 1) == 1);

  for (auto it = t.croot(); !!it && !!right(it); it = t.rotate_left(it))
  {

    BOOST_TEST(std::vector<int>(t.begin_marked(red), t.end_marked(red)) ==
                 red_nodes,
               boost::test_tools::per_element());

    BOOST_TEST(std::vector<int>(t.begin_marked(green), t.end_marked(green)) ==
                 green_nodes,
               boost::test_tools::per_element());

    BOOST_TEST(std::vector<int>(t.begin_marked(blue), t.end_marked(blue)) ==
                 blue_nodes,
               boost::test_tools::per_element());

    BOOST_TEST(marking_invariant_holds(t, red));
    BOOST_TEST(marking_invariant_holds(t, green));
    BOOST_TEST(marking_invariant_holds(t, blue));
  }

  BOOST_TEST((std::distance(t.cbegin().base(), t.croot()) + 1) == t.size());
}

BOOST_AUTO_TEST_SUITE_END()
}


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

 * 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 testOrientedPlane3Factor.cpp
 * @date Dec 19, 2012
 * @author Alex Trevor
 * @brief Tests the OrientedPlane3Factor class
 */

#include <gtsam/slam/OrientedPlane3Factor.h>

#include <gtsam/base/numericalDerivative.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/ISAM2.h>

#include <CppUnitLite/TestHarness.h>

#include <boost/assign/std/vector.hpp>
#include <boost/assign/std.hpp>
#include <boost/bind.hpp>

using namespace boost::assign;
using namespace gtsam;
using namespace std;

GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3)
GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3)

using symbol_shorthand::P;  //< Planes
using symbol_shorthand::X;  //< Pose3

// *************************************************************************
TEST(OrientedPlane3Factor, lm_translation_error) {
  // Tests one pose, two measurements of the landmark that differ in range only.
  // Normal along -x, 3m away
  OrientedPlane3 test_lm0(-1.0, 0.0, 0.0, 3.0);

  NonlinearFactorGraph graph;

  // Init pose and prior.  Pose Prior is needed since a single plane measurement
  // does not fully constrain the pose
  Pose3 init_pose(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
  Vector6 sigmas;
  sigmas << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001;
  graph.addPrior(X(0), init_pose, noiseModel::Diagonal::Sigmas(sigmas));

  // Add two landmark measurements, differing in range
  Vector3 sigmas3 {0.1, 0.1, 0.1};
  Vector4 measurement0 {-1.0, 0.0, 0.0, 3.0};
  OrientedPlane3Factor factor0(
      measurement0, noiseModel::Diagonal::Sigmas(sigmas3), X(0), P(0));
  graph.add(factor0);
  Vector4 measurement1 {-1.0, 0.0, 0.0, 1.0};
  OrientedPlane3Factor factor1(
      measurement1, noiseModel::Diagonal::Sigmas(sigmas3), X(0), P(0));
  graph.add(factor1);

  // Initial Estimate
  Values values;
  values.insert(X(0), init_pose);
  values.insert(P(0), test_lm0);

  // Optimize
  ISAM2 isam2;
  ISAM2Result result = isam2.update(graph, values);
  Values result_values = isam2.calculateEstimate();
  OrientedPlane3 optimized_plane_landmark =
      result_values.at<OrientedPlane3>(P(0));

  // Given two noisy measurements of equal weight, expect result between the two
  OrientedPlane3 expected_plane_landmark(-1.0, 0.0, 0.0, 2.0);
  EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark));
}

// *************************************************************************
// TODO As described in PR #564 after correcting the derivatives in
// OrientedPlane3Factor this test fails. It should be debugged and re-enabled.
/*
TEST (OrientedPlane3Factor, lm_rotation_error) {
  // Tests one pose, two measurements of the landmark that differ in angle only.
  // Normal along -x, 3m away
  OrientedPlane3 test_lm0(-1.0/sqrt(1.01), 0.1/sqrt(1.01), 0.0, 3.0);

  NonlinearFactorGraph graph;

  // Init pose and prior.  Pose Prior is needed since a single plane measurement does not fully constrain the pose
  Pose3 init_pose(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
  graph.addPrior(X(0), init_pose,
      noiseModel::Diagonal::Sigmas(
          (Vector(6) << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001).finished()));

  // Add two landmark measurements, differing in angle
  Vector4 measurement0 {-1.0, 0.0, 0.0, 3.0};
  OrientedPlane3Factor factor0(measurement0,
      noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)), X(0), P(0));
  graph.add(factor0);
  Vector4 measurement1 {0.0, -1.0, 0.0, 3.0};
  OrientedPlane3Factor factor1(measurement1,
      noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)), X(0), P(0));
  graph.add(factor1);

  // Initial Estimate
  Values values;
  values.insert(X(0), init_pose);
  values.insert(P(0), test_lm0);

  // Optimize
  ISAM2 isam2;
  ISAM2Result result = isam2.update(graph, values);
  Values result_values = isam2.calculateEstimate();
  auto optimized_plane_landmark = result_values.at<OrientedPlane3>(P(0));

  // Given two noisy measurements of equal weight, expect result between the two
  OrientedPlane3 expected_plane_landmark(-sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0,
      0.0, 3.0);
  EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark));
}
*/

// *************************************************************************
TEST( OrientedPlane3Factor, Derivatives ) {
  // Measurement
  OrientedPlane3 p(sqrt(2)/2, -sqrt(2)/2, 0, 5);

  // Linearisation point
  OrientedPlane3 pLin(sqrt(3)/3, -sqrt(3)/3, sqrt(3)/3, 7);
  gtsam::Point3 pointLin  = gtsam::Point3(1, 2, -4);
  gtsam::Rot3 rotationLin = gtsam::Rot3::RzRyRx(0.5*M_PI, -0.3*M_PI, 1.4*M_PI);
  Pose3 poseLin(rotationLin, pointLin);

  // Factor
  Key planeKey(1), poseKey(2);
  SharedGaussian noise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1));
  OrientedPlane3Factor factor(p.planeCoefficients(), noise, poseKey, planeKey);

  // Calculate numerical derivatives
  boost::function<Vector(const Pose3&, const OrientedPlane3&)> f = boost::bind(
      &OrientedPlane3Factor::evaluateError, factor, _1, _2, boost::none, boost::none);
  Matrix numericalH1 = numericalDerivative21<Vector, Pose3, OrientedPlane3>(f, poseLin, pLin);
  Matrix numericalH2 = numericalDerivative22<Vector, Pose3, OrientedPlane3>(f, poseLin, pLin);

  // Use the factor to calculate the derivative
  Matrix actualH1, actualH2;
  factor.evaluateError(poseLin, pLin, actualH1, actualH2);

  // Verify we get the expected error
  EXPECT(assert_equal(numericalH1, actualH1, 1e-8));
  EXPECT(assert_equal(numericalH2, actualH2, 1e-8));
}

// *************************************************************************
TEST( OrientedPlane3DirectionPrior, Constructor ) {

  // Example: pitch and roll of aircraft in an ENU Cartesian frame.
  // If pitch and roll are zero for an aerospace frame,
  // that means Z is pointing down, i.e., direction of Z = (0,0,-1)

  Vector4 planeOrientation = (Vector(4) << 0.0, 0.0, -1.0, 10.0).finished(); // all vertical planes directly facing the origin

  // Factor
  Key key(1);
  SharedGaussian model = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 10.0));
  OrientedPlane3DirectionPrior factor(key, planeOrientation, model);

  // Create a linearization point at the zero-error point
  Vector4 theta1 {0.0, 0.02, -1.2, 10.0};
  Vector4 theta2 {0.0, 0.1, -0.8, 10.0};
  Vector4 theta3 {0.0, 0.2, -0.9, 10.0};

  OrientedPlane3 T1(theta1);
  OrientedPlane3 T2(theta2);
  OrientedPlane3 T3(theta3);

  // Calculate numerical derivatives
  Matrix expectedH1 = numericalDerivative11<Vector, OrientedPlane3>(
      boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1,
          boost::none), T1);

  Matrix expectedH2 = numericalDerivative11<Vector, OrientedPlane3>(
      boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1,
          boost::none), T2);

  Matrix expectedH3 = numericalDerivative11<Vector, OrientedPlane3>(
      boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1,
          boost::none), T3);

  // Use the factor to calculate the derivative
  Matrix actualH1, actualH2, actualH3;
  factor.evaluateError(T1, actualH1);
  factor.evaluateError(T2, actualH2);
  factor.evaluateError(T3, actualH3);

  // Verify we get the expected error
  EXPECT(assert_equal(expectedH1, actualH1, 1e-8));
  EXPECT(assert_equal(expectedH2, actualH2, 1e-8));
  EXPECT(assert_equal(expectedH3, actualH3, 1e-8));
}

/* ************************************************************************* */
// Simplified version of the test by Marco Camurri to debug issue #561
TEST(OrientedPlane3Factor, Issue561Simplified) {
  // Typedefs
  using Plane = OrientedPlane3;

  NonlinearFactorGraph graph;

  // Setup prior factors
  // Note: If x0 is too far away from the origin (e.g. x=100) this test can fail.
  Pose3 x0(Rot3::identity(), Vector3(10, -1, 1));
  auto x0_noise = noiseModel::Isotropic::Sigma(6, 0.01);
  graph.addPrior<Pose3>(X(0), x0, x0_noise);

  // Two horizontal planes with different heights, in the world frame.
  const Plane p1(0,0,1,1), p2(0,0,1,2);
  auto p1_noise = noiseModel::Diagonal::Sigmas(Vector3{1, 1, 5});
  auto p2_noise = noiseModel::Diagonal::Sigmas(Vector3{1, 1, 5});
  graph.addPrior<Plane>(P(1), p1, p1_noise);
  graph.addPrior<Plane>(P(2), p2, p2_noise);

  // Plane factors
  auto p1_measured_from_x0 = p1.transform(x0); // transform p1 to pose x0 as a measurement
  auto p2_measured_from_x0 = p2.transform(x0); // transform p2 to pose x0 as a measurement
  const auto x0_p1_noise = noiseModel::Isotropic::Sigma(3, 0.05);
  const auto x0_p2_noise = noiseModel::Isotropic::Sigma(3, 0.05);
  graph.emplace_shared<OrientedPlane3Factor>(
      p1_measured_from_x0.planeCoefficients(), x0_p1_noise, X(0), P(1));
  graph.emplace_shared<OrientedPlane3Factor>(
      p2_measured_from_x0.planeCoefficients(), x0_p2_noise, X(0), P(2));

  // Initial values
  // Just offset the initial pose by 1m. This is what we are trying to optimize.
  Values initialEstimate;
  Pose3 x0_initial = x0.compose(Pose3(Rot3::identity(), Vector3(1,0,0)));
  initialEstimate.insert(P(1), p1);
  initialEstimate.insert(P(2), p2);
  initialEstimate.insert(X(0), x0_initial);

  // Optimize
  try {
    GaussNewtonOptimizer optimizer(graph, initialEstimate);
    Values result = optimizer.optimize();
    EXPECT_DOUBLES_EQUAL(0, graph.error(result), 0.1);
    EXPECT(x0.equals(result.at<Pose3>(X(0))));
    EXPECT(p1.equals(result.at<Plane>(P(1))));
    EXPECT(p2.equals(result.at<Plane>(P(2))));
  } catch (const IndeterminantLinearSystemException &e) {
    std::cerr << "CAPTURED THE EXCEPTION: " << DefaultKeyFormatter(e.nearbyVariable()) << std::endl;
    EXPECT(false); // fail if this happens
  }
}

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


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef LA_ALGEBRAIC_CONCEPTS_DOC_INCLUDE
#define LA_ALGEBRAIC_CONCEPTS_DOC_INCLUDE

#ifdef __GXX_CONCEPTS__
#  include <concepts>
#else
#  include <boost/numeric/linear_algebra/pseudo_concept.hpp>
#endif

#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/linear_algebra/inverse.hpp>

/// Namespace for purely algebraic concepts
namespace algebra {

/** @addtogroup Concepts
 *  @{
 */

#ifndef __GXX_CONCEPTS__
    //! Concept Commutative
    /*!
        \param Operation A functor implementing a binary operation
        \param Element The type upon the binary operation is defined

        \par Notation:

            op
    	Object of type Operation

            x, y
    	Objects of type Element

        \invariant

            Commutativity
    	op(x, y) == op(y, x)

     */
    template <typename Operation, typename Element>
    struct Commutative
    {};
#else
    concept Commutative<typename Operation, typename Element>
    {
	axiom Commutativity(Operation op, Element x, Element y)
	{
	    op(x, y) == op(y, x);
	}
    };
#endif

#ifndef __GXX_CONCEPTS__
    //! Concept Associative
    /*!
        \param Operation A functor implementing a binary operation
        \param Element The type upon the binary operation is defined

        \par Notation:

            op
    	Object of type Operation

            x, y, z
    	Objects of type Element

        \invariant

            Associativity
    	op(x, op(y, z)) == op(op(x, y), z)

     */
    template <typename Operation, typename Element>
    struct Associative
    {};
#else
    concept Associative<typename Operation, typename Element>
    {
        axiom Associativity(Operation op, Element x, Element y, Element z)
        {
	    op(x, op(y, z)) == op(op(x, y), z);
        }
    };
#endif

#ifndef __GXX_CONCEPTS__
    //! Concept SemiGroup
    /*!
        \param Operation A functor implementing a binary operation
        \param Element The type upon the binary operation is defined

        \note
        -# The algebraic concept SemiGroup only requires associativity and is identical with the concept Associative.
     */
    template <typename Operation, typename Element>
    struct SemiGroup
        : Associative<Operation, Element>
    {};
#else
    auto concept SemiGroup<typename Operation, typename Element>
      : Associative<Operation, Element>
    {};
#endif

#ifndef __GXX_CONCEPTS__
    //! Concept Monoid
    /*!
        \param Operation A functor implementing a binary operation
        \param Element The type upon the binary operation is defined

        \par Refinement of:
	- SemiGroup
        \par Notation:

            op
    	Object of type Operation

            x
    	Object of type Element

        \invariant

            Neutrality from right
	    op( x, identity(op, x) ) == x

            Neutrality from left
	    op( identity(op, x), x ) == x

     */
    template <typename Operation, typename Element>
    struct Monoid
      : SemiGroup<Operation, Element>
    {
	/// Associated type; if not defined in concept_map automatically detected as result of identity
        typedef associated_type identity_result_type;
        identity_result_type identity(Operation, Element); ///< Identity element of Operation
    };
#else
    concept Monoid<typename Operation, typename Element>
      : SemiGroup<Operation, Element>
    {
        typename identity_result_type;
        identity_result_type identity(Operation, Element);

        axiom Neutrality(Operation op, Element x)
        {
	    op( x, identity(op, x) ) == x;
	    op( identity(op, x), x ) == x;
        }
    };
#endif

#ifdef __GXX_CONCEPTS__
    auto concept Inversion<typename Operation, typename Element>
    {
        typename inverse_result_type;
        inverse_result_type inverse(Operation, Element);

    };
#else
    //! Concept Inversion
    /*!
        \param Operation A functor implementing a binary operation
        \param Element The type upon the binary operation is defined

	\par Associated Types:
	- inverse_result_type
	\par Valid Expressions:
	- inverse(op, x);
     */
    template <typename Operation, typename Element>
    struct Inversion
    {
	/// Associated type; if not defined in concept_map automatically detected as result of inverse
        typedef associated_type inverse_result_type;

	/// Returns inverse of \p x regarding operation \p op
        inverse_result_type inverse(Operation op, Element x);
    };
#endif

#ifdef __GXX_CONCEPTS__
    concept Group<typename Operation, typename Element>
      : Monoid<Operation, Element>, Inversion<Operation, Element>
    {
        axiom Inversion(Operation op, Element x)
        {
	    op( x, inverse(op, x) ) == identity(op, x);
	    op( inverse(op, x), x ) == identity(op, x);
        }
    };
#else
    //! Concept Group
    /*!
        \param Operation A functor implementing a binary operation
        \param Element The type upon the binary operation is defined

        \par Refinement of:
	- Monoid
	- Inversion
        \par Notation:

            op
    	Object of type Operation

            x
    	Object of type Element

        \invariant

            Inverse from right
    	op( x, inverse(op, x) ) == identity(op, x)

            Inverse from left
    	op( inverse(op, x), x ) == identity(op, x)

     */
    template <typename Operation, typename Element>
    struct Group
      : Monoid<Operation, Element>,
	Inversion<Operation, Element>
    {};
#endif

#ifdef __GXX_CONCEPTS__
    auto concept AbelianGroup<typename Operation, typename Element>
      : Group<Operation, Element>, Commutative<Operation, Element>
    {};
#else
    //! Concept AbelianGroup
    /*!
        \param Operation A functor implementing a binary operation
        \param Element The type upon the binary operation is defined

        \par Refinement of:
	- Group
	- Commutative
     */
    template <typename Operation, typename Element>
    struct AbelianGroup
      : Group<Operation, Element>,
	Commutative<Operation, Element>
    {};
#endif

#ifdef __GXX_CONCEPTS__
    concept Distributive<typename AddOp, typename MultOp, typename Element>
    {
        axiom Distributivity(AddOp add, MultOp mult, Element x, Element y, Element z)
        {
	    // From left
	    mult(x, add(y, z)) == add(mult(x, y), mult(x, z));
	    // z right
	    mult(add(x, y), z) == add(mult(x, z), mult(y, z));
        }
    };
#else
    //! Concept Distributive
    /*!
        \param AddOp A functor implementing a binary operation representing addition
        \param MultOp A functor implementing a binary operation representing multiplication
        \param Element The type upon the binary operation is defined

        \par Notation:

            add
	    Object of type AddOp

            mult
	    Object of type Multop

            x, y, z
	    Objects of type Element

        \invariant

            Distributivity from left
	    mult(x, add(y, z)) == add(mult(x, y), mult(x, z))

            Distributivity from right
	    mult(add(x, y), z) == add(mult(x, z), mult(y, z))

     */
    template <typename AddOp, typename MultOp, typename Element>
    struct Distributive
    {};
#endif

#ifdef __GXX_CONCEPTS__
    auto concept Ring<typename AddOp, typename MultOp, typename Element>
      : AbelianGroup<AddOp, Element>,
        SemiGroup<MultOp, Element>,
        Distributive<AddOp, MultOp, Element>
    {};
#else
    //! Concept Ring
    /*!
        \param AddOp A functor implementing a binary operation representing addition
        \param MultOp A functor implementing a binary operation representing multiplication
        \param Element The type upon the binary operation is defined

        \par Refinement of:
	- AbelianGroup <MultOp, Element>
	- SemiGroup <MultOp, Element>
        - Distributive <AddOp, MultOp, Element>
     */
    template <typename AddOp, typename MultOp, typename Element>
    struct Ring
      : AbelianGroup<AddOp, Element>,
        SemiGroup<MultOp, Element>,
        Distributive<AddOp, MultOp, Element>
    {};
#endif

#ifdef __GXX_CONCEPTS__
    auto concept RingWithIdentity<typename AddOp, typename MultOp, typename Element>
      : Ring<AddOp, MultOp, Element>,
        Monoid<MultOp, Element>
    {};
#else
    //! Concept RingWithIdentity
    /*!
        \param AddOp A functor implementing a binary operation representing addition
        \param MultOp A functor implementing a binary operation representing multiplication
        \param Element The type upon the binary operation is defined

        \par Refinement of:
	- Ring <AddOp, MultOp, Element>
        - Monoid <MultOp, Element>
     */
    template <typename AddOp, typename MultOp, typename Element>
    struct RingWithIdentity
      : Ring<AddOp, MultOp, Element>,
        Monoid<MultOp, Element>
    {};
#endif

#ifdef __GXX_CONCEPTS__
    concept DivisionRing<typename AddOp, typename MultOp, typename Element>
      : RingWithIdentity<AddOp, MultOp, Element>,
        Inversion<MultOp, Element>
    {
        // 0 != 1, otherwise trivial
        axiom ZeroIsDifferentFromOne(AddOp add, MultOp mult, Element x)
        {
	    identity(add, x) != identity(mult, x);
        }

        // Non-zero divisibility from left and from right
        axiom NonZeroDivisibility(AddOp add, MultOp mult, Element x)
        {
	    if (x != identity(add, x))
		mult(inverse(mult, x), x) == identity(mult, x);
	    if (x != identity(add, x))
		mult(x, inverse(mult, x)) == identity(mult, x);
        }
    };
#else
    //! Concept DivisionRing
    /*!
        \param AddOp A functor implementing a binary operation representing addition
        \param MultOp A functor implementing a binary operation representing multiplication
        \param Element The type upon the binary operation is defined

        \par Refinement of:
	- RingWithIdentity <AddOp, MultOp, Element>
        - Inversion <MultOp, Element>

        \par Notation:

            add
	    Object of type AddOp

            mult
	    Object of type Multop

            x, y, z
	    Objects of type Element

        \invariant

            Non-zero divisibility from left
	    mult(inverse(mult, x), x) == identity(mult, x)
	    if x != identity(add, x)

            Non-zero divisibility from right
	    mult(x, inverse(mult, x)) == identity(mult, x)
	    if x != identity(add, x)

            Zero is different from one
	    identity(add, x) != identity(mult, x)

	\note
	-# Zero and one can be theoretically identical in a DivisionRing.  However,
	   this implies that there is only one element x in the Ring with x + x = x and
	   x * x = x (which is actually even a Field).
	   Because this structure has no practical value we exclude it from
	   consideration.
     */
    template <typename AddOp, typename MultOp, typename Element>
    struct DivisionRing
      : RingWithIdentity<AddOp, MultOp, Element>,
        Inversion<MultOp, Element>
    {};
#endif

#ifdef __GXX_CONCEPTS__
    // SkewField is defined as synonym for DivisionRing
    auto concept SkewField<typename AddOp, typename MultOp, typename Element>
      : DivisionRing<AddOp, MultOp, Element>
    {};
#else
    //! Concept SkewField
    /*!
        \param AddOp A functor implementing a binary operation representing addition
        \param MultOp A functor implementing a binary operation representing multiplication
        \param Element The type upon the binary operation is defined

        \par Refinement of:
	- DivisionRing <AddOp, MultOp, Element>
	\note
	- Because the refinement of DivisionRing to SkewField is automatic the two concepts
	  are identical.
     */
    template <typename AddOp, typename MultOp, typename Element>
    struct SkewField
      : DivisionRing<AddOp, MultOp, Element>
    {};
#endif

#ifdef __GXX_CONCEPTS__
    auto concept Field<typename AddOp, typename MultOp, typename Element>
      : DivisionRing<AddOp, MultOp, Element>,
        Commutative<MultOp, Element>
    {};
#else
    //! Concept Field
    /*!
        \param AddOp A functor implementing a binary operation representing addition
        \param MultOp A functor implementing a binary operation representing multiplication
        \param Element The type upon the binary operation is defined

        \par Refinement of:
	- DivisionRing <AddOp, MultOp, Element>
	- Commutative <MultOp, Element>
     */
    template <typename AddOp, typename MultOp, typename Element>
    struct Field
      : DivisionRing<AddOp, MultOp, Element>,
        Commutative<MultOp, Element>
    {};
#endif

/*@}*/ // end of group Concepts

} // algebra

#endif // LA_ALGEBRAIC_CONCEPTS_DOC_INCLUDE


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

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

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

//[point
//` Declaration and use of the Boost.Geometry model::point, modelling the Point Concept

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

namespace bg = boost::geometry;

int main()
{
    bg::model::point<double, 2, bg::cs::cartesian> point1;
    bg::model::point<double, 3, bg::cs::cartesian> point2(1.0, 2.0, 3.0); /*< Construct, assigning three coordinates >*/
    point1.set<0>(1.0); /*< Set a coordinate. [*Note]: prefer using `bg::set<0>(point1, 1.0);` >*/
    point1.set<1>(2.0);

    double x = point1.get<0>(); /*< Get a coordinate. [*Note]: prefer using `x = bg::get<0>(point1);` >*/
    double y = point1.get<1>();

    std::cout << x << ", " << y << std::endl;
    return 0;
}

//]

//[point_output
/*`
Output:
[pre
1, 2
]
*/
//]


//
// Created by Alex Beccaro on 18/01/18.
//

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include "../../src/problems/51-100/64/problem64.hpp"

BOOST_AUTO_TEST_SUITE( Problem64 )

    BOOST_AUTO_TEST_CASE( Example ) {
        auto res = problems::problem64::solve(13);
        BOOST_CHECK_EQUAL(res, 4);
    }

    BOOST_AUTO_TEST_CASE( Solution ) {
        auto res = problems::problem64::solve();
        BOOST_CHECK_EQUAL(res, 1322);
    }

BOOST_AUTO_TEST_SUITE_END()


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

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

 * See LICENSE for the license information

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

/*
 * @file Unit3.h
 * @date Feb 02, 2011
 * @author Can Erdogan
 * @author Frank Dellaert
 * @author Alex Trevor
 * @author Zhaoyang Lv
 * @brief The Unit3 class - basically a point on a unit sphere
 */

#include <gtsam/geometry/Unit3.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/config.h>  // for GTSAM_USE_TBB

#ifdef __clang__
#  pragma clang diagnostic push
#  pragma clang diagnostic ignored "-Wunused-variable"
#endif
#include <boost/random/uniform_on_sphere.hpp>
#ifdef __clang__
#  pragma clang diagnostic pop
#endif

#include <boost/random/variate_generator.hpp>
#include <iostream>
#include <limits>
#include <cmath>
#include <vector>

using namespace std;

namespace gtsam {

/* ************************************************************************* */
Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2, 3> H) {
  // 3*3 Derivative of representation with respect to point is 3*3:
  Matrix3 D_p_point;
  Unit3 direction;
  direction.p_ = normalize(point, H ? &D_p_point : 0);
  if (H)
    *H << direction.basis().transpose() * D_p_point;
  return direction;
}

/* ************************************************************************* */
Unit3 Unit3::Random(boost::mt19937 & rng) {
  // TODO(dellaert): allow any engine without including all of boost :-(
  boost::uniform_on_sphere<double> randomDirection(3);
  // This variate_generator object is required for versions of boost somewhere
  // around 1.46, instead of drawing directly using boost::uniform_on_sphere(rng).
  boost::variate_generator<boost::mt19937&, boost::uniform_on_sphere<double> > generator(
      rng, randomDirection);
  const vector<double> d = generator();
  return Unit3(d[0], d[1], d[2]);
}

/* ************************************************************************* */
// Get the axis of rotation with the minimum projected length of the point
static Point3 CalculateBestAxis(const Point3& n) {
  double mx = fabs(n.x()), my = fabs(n.y()), mz = fabs(n.z());
  if ((mx <= my) && (mx <= mz)) {
    return Point3(1.0, 0.0, 0.0);
  } else if ((my <= mx) && (my <= mz)) {
    return Point3(0.0, 1.0, 0.0);
  } else {
    return Point3(0, 0, 1);
  }
}

/* ************************************************************************* */
const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const {
#ifdef GTSAM_USE_TBB
  // NOTE(hayk): At some point it seemed like this reproducably resulted in
  // deadlock. However, I don't know why and I can no longer reproduce it.
  // It either was a red herring or there is still a latent bug left to debug.
  tbb::mutex::scoped_lock lock(B_mutex_);
#endif

  const bool cachedBasis = static_cast<bool>(B_);
  const bool cachedJacobian = static_cast<bool>(H_B_);

  if (H) {
    if (!cachedJacobian) {
      // Compute Jacobian. Recomputes B_
      Matrix32 B;
      Matrix62 jacobian;
      Matrix33 H_B1_n, H_b1_B1, H_b2_n, H_b2_b1;

      // Choose the direction of the first basis vector b1 in the tangent plane
      // by crossing n with the chosen axis.
      const Point3 n(p_), axis = CalculateBestAxis(n);
      const Point3 B1 = gtsam::cross(n, axis, &H_B1_n);

      // Normalize result to get a unit vector: b1 = B1 / |B1|.
      B.col(0) = normalize(B1, &H_b1_B1);

      // Get the second basis vector b2, which is orthogonal to n and b1.
      B.col(1) = gtsam::cross(n, B.col(0), &H_b2_n, &H_b2_b1);

      // Chain rule tomfoolery to compute the jacobian.
      const Matrix32& H_n_p = B;
      jacobian.block<3, 2>(0, 0) = H_b1_B1 * H_B1_n * H_n_p;
      auto H_b1_p = jacobian.block<3, 2>(0, 0);
      jacobian.block<3, 2>(3, 0) = H_b2_n * H_n_p + H_b2_b1 * H_b1_p;

      // Cache the result and jacobian
      H_B_.reset(jacobian);
      B_.reset(B);
    }

    // Return cached jacobian, possibly computed just above
    *H = *H_B_;
  }

  if (!cachedBasis) {
    // Same calculation as above, without derivatives.
    // Done after H block, as that possibly computes B_ for the first time
    Matrix32 B;

    const Point3 n(p_), axis = CalculateBestAxis(n);
    const Point3 B1 = gtsam::cross(n, axis);
    B.col(0) = normalize(B1);
    B.col(1) = gtsam::cross(n, B.col(0));
    B_.reset(B);
  }

  return *B_;
}

/* ************************************************************************* */
Point3 Unit3::point3(OptionalJacobian<3, 2> H) const {
  if (H)
    *H = basis();
  return Point3(p_);
}

/* ************************************************************************* */
Vector3 Unit3::unitVector(OptionalJacobian<3, 2> H) const {
  if (H)
    *H = basis();
  return p_;
}

/* ************************************************************************* */
std::ostream& operator<<(std::ostream& os, const Unit3& pair) {
  os << pair.p_ << endl;
  return os;
}

/* ************************************************************************* */
void Unit3::print(const std::string& s) const {
  cout << s << ":" << p_ << endl;
}

/* ************************************************************************* */
Matrix3 Unit3::skew() const {
  return skewSymmetric(p_.x(), p_.y(), p_.z());
}

/* ************************************************************************* */
double Unit3::dot(const Unit3& q, OptionalJacobian<1, 2> H_p,
                  OptionalJacobian<1, 2> H_q) const {
  // Get the unit vectors of each, and the derivative.
  Matrix32 H_pn_p;
  Point3 pn = point3(H_p ? &H_pn_p : nullptr);

  Matrix32 H_qn_q;
  const Point3 qn = q.point3(H_q ? &H_qn_q : nullptr);

  // Compute the dot product of the Point3s.
  Matrix13 H_dot_pn, H_dot_qn;
  double d = gtsam::dot(pn, qn, H_p ? &H_dot_pn : nullptr, H_q ? &H_dot_qn : nullptr);

  if (H_p) {
    (*H_p) << H_dot_pn * H_pn_p;
  }

  if (H_q) {
    (*H_q) = H_dot_qn * H_qn_q;
  }

  return d;
}

/* ************************************************************************* */
Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2, 2> H_q) const {
  // 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1
  const Vector2 xi = basis().transpose() * q.p_;
  if (H_q) {
    *H_q = basis().transpose() * q.basis();
  }
  return xi;
}

/* ************************************************************************* */
Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p,
                           OptionalJacobian<2, 2> H_q) const {
  // Get the point3 of this, and the derivative.
  Matrix32 H_qn_q;
  const Point3 qn = q.point3(H_q ? &H_qn_q : nullptr);

  // 2D error here is projecting q into the tangent plane of this (p).
  Matrix62 H_B_p;
  Matrix23 Bt = basis(H_p ? &H_B_p : nullptr).transpose();
  Vector2 xi = Bt * qn;

  if (H_p) {
    // Derivatives of each basis vector.
    const Matrix32& H_b1_p = H_B_p.block<3, 2>(0, 0);
    const Matrix32& H_b2_p = H_B_p.block<3, 2>(3, 0);

    // Derivatives of the two entries of xi wrt the basis vectors.
    const Matrix13 H_xi1_b1 = qn.transpose();
    const Matrix13 H_xi2_b2 = qn.transpose();

    // Assemble dxi/dp = dxi/dB * dB/dp.
    const Matrix12 H_xi1_p = H_xi1_b1 * H_b1_p;
    const Matrix12 H_xi2_p = H_xi2_b2 * H_b2_p;
    *H_p << H_xi1_p, H_xi2_p;
  }

  if (H_q) {
    // dxi/dq is given by dxi/dqu * dqu/dq, where qu is the unit vector of q.
    const Matrix23 H_xi_qu = Bt;
    *H_q = H_xi_qu * H_qn_q;
  }

  return xi;
}

/* ************************************************************************* */
double Unit3::distance(const Unit3& q, OptionalJacobian<1, 2> H) const {
  Matrix2 H_xi_q;
  const Vector2 xi = error(q, H ? &H_xi_q : nullptr);
  const double theta = xi.norm();
  if (H)
    *H = (xi.transpose() / theta) * H_xi_q;
  return theta;
}

/* ************************************************************************* */
Unit3 Unit3::retract(const Vector2& v, OptionalJacobian<2,2> H) const {
  // Compute the 3D xi_hat vector
  const Vector3 xi_hat = basis() * v;
  const double theta = xi_hat.norm();
  const double c = std::cos(theta);

  // Treat case of very small v differently.
  Matrix23 H_from_point;
  if (theta < std::numeric_limits<double>::epsilon()) {
    const Unit3 exp_p_xi_hat = Unit3::FromPoint3(c * p_ + xi_hat,
                                                 H? &H_from_point : nullptr);
    if (H) { // Jacobian
      *H = H_from_point *
          (-p_ * xi_hat.transpose() + Matrix33::Identity()) * basis();
    }
    return exp_p_xi_hat;
  }

  const double st = std::sin(theta) / theta;
  const Unit3 exp_p_xi_hat = Unit3::FromPoint3(c * p_ + xi_hat * st,
                                               H? &H_from_point : nullptr);
  if (H) { // Jacobian
    *H = H_from_point *
        (p_ * -st * xi_hat.transpose() + st * Matrix33::Identity() +
        xi_hat * ((c - st) / std::pow(theta, 2)) * xi_hat.transpose()) * basis();
  }
  return exp_p_xi_hat;
}

/* ************************************************************************* */
Vector2 Unit3::localCoordinates(const Unit3& other) const {
  const double x = p_.dot(other.p_);
  // Crucial quantity here is y = theta/sin(theta) with theta=acos(x)
  // Now, y = acos(x) / sin(acos(x)) = acos(x)/sqrt(1-x^2)
  // We treat the special case 1 and -1 below
  const double x2 = x * x;
  const double z = 1 - x2;
  double y;
  if (z < std::numeric_limits<double>::epsilon()) {
    if (x > 0)  // first order expansion at x=1
      y = 1.0 - (x - 1.0) / 3.0;
    else  // cop out
      return Vector2(M_PI, 0.0);
  } else {
    // no special case
    y = acos(x) / sqrt(z);
  }
  return basis().transpose() * y * (other.p_ - x * p_);
}
/* ************************************************************************* */

}  // namespace gtsam


//=======================================================================
// Copyright (c)
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
/**
 * @file bounded_degree_mst_example.cpp
 * @brief
 * @author Piotr Godlewski
 * @version 1.0
 * @date 2013-11-21
 */

//! [Bounded-Degree Minimum Spanning Tree Example]
#include "paal/iterative_rounding/bounded_degree_min_spanning_tree/bounded_degree_mst.hpp"
#include "paal/utils/functors.hpp"

#include <boost/graph/adjacency_list.hpp>

#include <iostream>
#include <vector>

int main() {
    using Graph = boost::adjacency_list<boost::vecS, boost::vecS,
        boost::undirectedS, boost::no_property,
        boost::property<boost::edge_weight_t, int>>;
    using Edge = boost::graph_traits<Graph>::edge_descriptor;

    // sample problem
    std::vector<std::pair<int, int>> edges {{0,1},{0,2},{1,2},{1,3},{1,4},
        {1,5},{5,0},{3,4}};
    std::vector<int> costs {1,2,1,2,1,1,1,5};
    std::vector<int> bounds {3,2,2,2,2,2};

    Graph g(edges.begin(), edges.end(), costs.begin(), 6);
    auto degree_bounds = paal::utils::make_array_to_functor(bounds);

    std::vector<Edge> result_tree;

    // optional input validity checking
    auto bdmst = paal::ir::make_bounded_degree_mst(
        g, degree_bounds, std::back_inserter(result_tree));
    auto error = bdmst.check_input_validity();
    if (error) {
        std::cerr << "The input is not valid!" << std::endl;
        std::cerr << *error << std::endl;
        return -1;
    }

    // solve it
    auto result = paal::ir::bounded_degree_mst_iterative_rounding(
        g, degree_bounds, std::back_inserter(result_tree));

    // print result
    if (result.first == paal::lp::OPTIMAL) {
        std::cout << "Edges in the spanning tree" << std::endl;
        for (auto e : result_tree) {
            std::cout << "Edge " << e << std::endl;
        }
        std::cout << "Cost of the solution: " << *(result.second) << std::endl;
    } else {
        std::cout << "The instance is infeasible" << std::endl;
    }
    paal::lp::glp::free_env();
    return 0;
}
    //! [Bounded-Degree Minimum Spanning Tree Example]


/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2009, Willow Garage, Inc.
 *  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 Willow Garage, Inc. 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 OWNER 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 "precomp.hpp"

// Eigen
#include <Eigen/Core>

// OpenCV
#include <opencv2/core/eigen.hpp>
#include <opencv2/sfm/numeric.hpp>

// libmv headers
#include "libmv/numeric/numeric.h"

#include <iostream>

namespace cv
{
namespace sfm
{

template<typename T>
void
meanAndVarianceAlongRows( const Mat_<T> &A,
                          Mat_<T> mean,
                          Mat_<T> variance )
{
  const int n = A.rows, m = A.cols;

  for( int i = 0; i < n; ++i )
  {
    mean(i) = 0;
    variance(i) = 0;

    for( int j = 0; j < m; ++j )
    {
      T x = A(i,j);
      mean(i) += x;
      variance(i) += x*x;
    }
  }

  mean /= m;
  for (int i = 0; i < n; ++i) {
    variance(i) = variance(i) / m - (mean(i)*mean(i));
  }
}

void
meanAndVarianceAlongRows( InputArray _A,
                          OutputArray _mean,
                          OutputArray _variance )
{
  const Mat A = _A.getMat();
  const int depth = A.depth();
  CV_Assert( depth == CV_32F || depth == CV_64F );

  _mean.create(A.rows, 1, depth);
  _variance.create(A.rows, 1, depth);

  Mat mean = _mean.getMat(), variance = _variance.getMat();

  if( depth == CV_32F )
  {
    meanAndVarianceAlongRows<float>( A, mean, variance );
  }
  else
  {
    meanAndVarianceAlongRows<double>( A, mean, variance );
  }
}

//template<typename T>
//inline Mat
//skewMatMinimal( const Mat_<T> &x )
//{
//  Mat_<T> skew(2,3);
//  skew << 0, -1,  x(1),
//          1,  0, -x(0);
//  return skew;
//}
//
//Mat
//skewMatMinimal( InputArray _x )
//{
//  Mat x = _x.getMat();
//  CV_Assert( x.rows == 3 && x.cols == 1 );
//
//  int depth = x.depth();
//  if( depth == CV_32F )
//  {
//    return skewMatMinimal<float>(x);
//  }
//  else
//  {
//    return skewMatMinimal<double>(x);
//  }
//}

template<typename T>
Mat
skewMat( const Mat_<T> &x )
{
  Mat_<T> skew(3,3);
  skew <<   0 , -x(2),  x(1),
          x(2),    0 , -x(0),
         -x(1),  x(0),    0;

  return skew;
}

Mat
skew( InputArray _x )
{
  const Mat x = _x.getMat();
  const int depth = x.depth();
  CV_Assert( x.size() == Size(3,1) || x.size() == Size(1,3) );
  CV_Assert( depth == CV_32F || depth == CV_64F );

  Mat skewMatrix;
  if( depth == CV_32F )
  {
    skewMatrix = skewMat<float>(x);
  }
  else if( depth == CV_64F )
  {
    skewMatrix = skewMat<double>(x);
  }
  else
  {
    //CV_Error(CV_StsBadArg, "The DataType must be CV_32F or CV_64F");
  }

  return skewMatrix;
}

} /* namespace sfm */
} /* namespace cv */


// This file is a part of the OpenSurgSim project.
// Copyright 2013-2016, SimQuest Solutions Inc.
//
// 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.

/// \file
/// Tests for the Gauss-Seidel implementation of the MLCP solver.

#include <math.h>
#include <memory>
#include <string>

#include "gtest/gtest.h"
#include <boost/chrono.hpp>

#include "SurgSim/Math/Valid.h"
#include "SurgSim/Math/MlcpGaussSeidelSolver.h"
#include "SurgSim/Math/MlcpSolution.h"
#include "SurgSim/Testing/MlcpIO/MlcpTestData.h"
#include "SurgSim/Testing/MlcpIO/ReadText.h"

using SurgSim::Math::isValid;
using SurgSim::Math::MlcpGaussSeidelSolver;

TEST(MlcpGaussSeidelSolverTests, CanConstruct)
{
	//ASSERT_NO_THROW({
	MlcpGaussSeidelSolver mlcpSolver(1.0, 1.0, 100);
}

static void solveAndCompareResult(const std::string& fileName,
								  double gsSolverPrecision = 1e-9, double gsContactTolerance = 1e-9,
								  int gsMaxIterations = 100)
{
	SCOPED_TRACE("while running test " + fileName);

	const std::shared_ptr<MlcpTestData> data = loadTestData(fileName);
	ASSERT_TRUE(data != nullptr) << "Failed to load " << fileName;

	// NB: need to make the solver calls const-correct.
	Eigen::MatrixXd A = data->problem.A;
	Eigen::VectorXd b = data->problem.b;
	Eigen::VectorXd mu = data->problem.mu;
	std::vector<SurgSim::Math::MlcpConstraintType> constraintTypes = data->problem.constraintTypes;

	const size_t size = data->getSize();
	SurgSim::Math::MlcpSolution solution;
	solution.x.resize(size);

	//################################
	// Gauss-Seidel solver
	MlcpGaussSeidelSolver mlcpSolver(gsSolverPrecision, gsContactTolerance,
									 gsMaxIterations);

	solution.x.setZero();

	// XXX set ratio to 1
	mlcpSolver.solve(data->problem, &solution);

	ASSERT_EQ(static_cast<Eigen::Index>(size), solution.x.rows());
	ASSERT_EQ(static_cast<Eigen::Index>(size), data->expectedLambda.rows());
	if (size > 0)
	{
		ASSERT_TRUE(isValid(solution.x)) << solution.x;
	}

	// TODO(advornik): Because this is a mixed LCP problem *with friction*, we can't just easily check that
	//   all x are positive (the frictional entries may not be), or that all Ax+b are positive(ditto).
	//   We need to either (a) make the test aware of the meaning of the constraint types, or (b) get rid
	//   of the constraint types and flag the frictional DOFs more directly.
	//
	//   For now, we check if the MLCP is really a pure LCP (only unilaterals without friction), and we
	//   perform some simple checks that apply to that case and that case only.
	bool isSimpleLcp = true;
	for (auto it = constraintTypes.cbegin();  it != constraintTypes.cend();  ++it)
	{
		if ((*it) != SurgSim::Math::MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT)
		{
			isSimpleLcp = false;
		}
	}

	if (isSimpleLcp && (size > 0))
	{
		EXPECT_GE(solution.x.minCoeff(), 0.0) << "x contains negative coefficients:" << std::endl << solution.x;

		Eigen::VectorXd c = A * solution.x + b;
		EXPECT_GE(c.minCoeff(), -gsSolverPrecision) << "Ax+b contains negative coefficients:" << std::endl << c;

		// Orthogonality test should be taking into account the scaling factor
		double maxAbsForce = abs(solution.x.maxCoeff());
		if (abs(solution.x.minCoeff()) > maxAbsForce)
		{
			maxAbsForce = abs(solution.x.minCoeff());
		}
		const double epsilon = 1e-9 * maxAbsForce;
		EXPECT_NEAR(0.0, c.dot(solution.x), epsilon) << "Ax+b is not orthogonal to x!" << std::endl <<
			"x:" << std::endl << solution.x << std::endl << "Ax+b:" << std::endl << c;
	}

	EXPECT_TRUE(solution.x.isApprox(data->expectedLambda)) << "lambda:" << std::endl << solution.x << std::endl <<
		"expected:" << std::endl << data->expectedLambda;

//	double convergenceCriteria=0.0;
//	bool validSignorini=false;
//	int nbContactToSkip=0;
//	int currentAtomicIndex = calculateConvergenceCriteria(lambda, nbContactToSkip, convergenceCriteria,
//		validSignorini, 1.0);
}

TEST(MlcpGaussSeidelSolverTests, SolveOriginal)
{
	const double gsSolverPrecision = 1e-4;
	const double gsContactTolerance = 2e-4;
	int gsMaxIterations = 30;
	solveAndCompareResult("mlcpOriginalTest.txt", gsSolverPrecision, gsContactTolerance, gsMaxIterations);
}

TEST(MlcpGaussSeidelSolverTests, SolveSequence)
{
	for (int i = 0;  i <= 9;  ++i)
	{
		solveAndCompareResult(getTestFileName("mlcpTest", i, ".txt"));
	}
}

static void solveRepeatedly(const MlcpTestData& data,
							/*XXX const */ MlcpGaussSeidelSolver* mlcpSolver,
							const int repetitions)
{
	// NB: need to make the solver calls const-correct.
	SurgSim::Math::MlcpProblem problem;
	SurgSim::Math::MlcpSolution solution;
	std::vector<SurgSim::Math::MlcpConstraintType> constraintTypes;

	const size_t size = data.getSize();
	solution.x.resize(size);

	for (int i = repetitions;  i > 0;  --i)
	{
		problem = data.problem;

		if (mlcpSolver)
		{
			solution.x.setZero();
			mlcpSolver->solve(problem, &solution);
		}
	}
}

static double measureExecutionTimeUsec(const std::string& fileName,
									   double gsSolverPrecision = 1e-8, double gsContactTolerance = 1e-8,
									   int gsMaxIterations = 20)
{
	SCOPED_TRACE("while running test " + fileName);

	const std::shared_ptr<MlcpTestData> data = loadTestData(fileName);
	EXPECT_TRUE(data != nullptr) << "Failed to load " << fileName;

	MlcpGaussSeidelSolver mlcpSolver(gsSolverPrecision, gsContactTolerance, gsMaxIterations);

	typedef boost::chrono::high_resolution_clock clock;
	clock::time_point calibrationStart = clock::now();

	const int calibrationRepetitions = 1000;
	solveRepeatedly(*data, &mlcpSolver, calibrationRepetitions);

	boost::chrono::duration<double> calibrationTime = clock::now() - calibrationStart;
	double desiredTestTimeSec = 1.0;
	double desiredRepetitions = desiredTestTimeSec * calibrationRepetitions / calibrationTime.count();
	const int repetitions = std::max(10, std::min(1000000, static_cast<int>(desiredRepetitions)));

	clock::time_point time0 = clock::now();

	// Do not actually solve the problem; just copy the input data.
	solveRepeatedly(*data, nullptr, repetitions);

	clock::time_point time1 = clock::now();

	// Actually solve the problem.
	solveRepeatedly(*data, &mlcpSolver, repetitions);

	clock::time_point time2 = clock::now();

	boost::chrono::duration<double> elapsedSolveTime = time2 - time1;
	//std::cout << "Elapsed:               " << (elapsedSolveTime.count() * 1000.) << " ms" << std::endl;
	boost::chrono::duration<double> elapsedBaseline = time1 - time0;
	//std::cout << "Baseline:              " << (elapsedBaseline.count() * 1000.) << " ms" << std::endl;
	double solveTimeUsec = (elapsedSolveTime - elapsedBaseline).count() * 1e6 / repetitions;
	double copyTimeUsec = elapsedBaseline.count() * 1e6 / repetitions;
	std::cout << "Average solution time: " << solveTimeUsec << " microseconds (~" << (solveTimeUsec+copyTimeUsec) <<
		"-" << copyTimeUsec << ") over " << repetitions << " loops" << std::endl;
	return solveTimeUsec;
}

TEST(MlcpGaussSeidelSolverTests, MeasureExecutionTimeOriginal)
{
	const double gsSolverPrecision = 1e-4;
	const double gsContactTolerance = 2e-5;
	int gsMaxIterations = 30;
	double solveTimeUsec = measureExecutionTimeUsec("mlcpOriginalTest.txt", gsSolverPrecision, gsContactTolerance,
													gsMaxIterations);

	// When refactoring the MLCP code, it can be very useful to compare the execution time before and after making
	// changes.  But you can't usefully compare execution times on different machines, or with different build
	// settings, so you'll need to manually uncomment the following line and adjust the time accordingly.
//	EXPECT_LE(solveTimeUsec, 14.0);
	EXPECT_LE(solveTimeUsec, 1e6);
}


/**
 * @file imputation_test.cpp
 * @author Keon Kim
 *
 * Tests for data::Imputer class
 */
#include <sstream>

#include <mlpack/core.hpp>
#include <mlpack/core/data/dataset_mapper.hpp>
#include <mlpack/core/data/map_policies/increment_policy.hpp>
#include <mlpack/core/data/map_policies/missing_policy.hpp>
#include <mlpack/core/data/imputer.hpp>
#include <mlpack/core/data/imputation_methods/custom_imputation.hpp>
#include <mlpack/core/data/imputation_methods/listwise_deletion.hpp>
#include <mlpack/core/data/imputation_methods/mean_imputation.hpp>
#include <mlpack/core/data/imputation_methods/median_imputation.hpp>

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

using namespace mlpack;
using namespace mlpack::data;
using namespace std;

BOOST_AUTO_TEST_SUITE(ImputationTest);
/**
 * 1. Make sure a CSV is loaded correctly with mappings using MissingPolicy.
 * 2. Try Imputer object with CustomImputation method to impute data "a".
 * (It is ok to test on one method since the other ones will be covered in the
 * next cases).
 */
BOOST_AUTO_TEST_CASE(DatasetMapperImputerTest)
{
  fstream f;
  f.open("test_file.csv", fstream::out);
  f << "a, 2, 3"  << endl;
  f << "5, 6, a"  << endl;
  f << "8, 9, 10" << endl;
  f.close();

  arma::mat input;

  std::set<string> mset;
  mset.insert("a");
  MissingPolicy policy(mset);
  DatasetMapper<MissingPolicy> info(policy);
  BOOST_REQUIRE(data::Load("test_file.csv", input, info) == true);

  // row and column test
  BOOST_REQUIRE_EQUAL(input.n_rows, 3);
  BOOST_REQUIRE_EQUAL(input.n_cols, 3);

  // Load check
  // MissingPolicy should convert strings to nans
  BOOST_REQUIRE(std::isnan(input(0, 0)) == true);
  BOOST_REQUIRE_CLOSE(input(0, 1), 5.0, 1e-5);
  BOOST_REQUIRE_CLOSE(input(0, 2), 8.0, 1e-5);
  BOOST_REQUIRE_CLOSE(input(1, 0), 2.0, 1e-5);
  BOOST_REQUIRE_CLOSE(input(1, 1), 6.0, 1e-5);
  BOOST_REQUIRE_CLOSE(input(1, 2), 9.0, 1e-5);
  BOOST_REQUIRE_CLOSE(input(2, 0), 3.0, 1e-5);
  BOOST_REQUIRE(std::isnan(input(2, 1)) == true);
  BOOST_REQUIRE_CLOSE(input(2, 2), 10.0, 1e-5);

  // convert missing vals to 99.
  CustomImputation<double> customStrategy(99);
  Imputer<double,
          DatasetMapper<MissingPolicy>,
          CustomImputation<double>> imputer(info, customStrategy);
  // convert a or nan to 99 for dimension 0
  imputer.Impute(input, "a", 0);

  // Custom imputation result check
  BOOST_REQUIRE_CLOSE(input(0, 0), 99.0, 1e-5);
  BOOST_REQUIRE_CLOSE(input(0, 1), 5.0, 1e-5);
  BOOST_REQUIRE_CLOSE(input(0, 2), 8.0, 1e-5);
  BOOST_REQUIRE_CLOSE(input(1, 0), 2.0, 1e-5);
  BOOST_REQUIRE_CLOSE(input(1, 1), 6.0, 1e-5);
  BOOST_REQUIRE_CLOSE(input(1, 2), 9.0, 1e-5);
  BOOST_REQUIRE_CLOSE(input(2, 0), 3.0, 1e-5);
  BOOST_REQUIRE(std::isnan(input(2, 1)) == true); // remains as NaN
  BOOST_REQUIRE_CLOSE(input(2, 2), 10.0, 1e-5);

  // Remove the file.
  remove("test_file.csv");
}

/**
 * Make sure CustomImputation method replaces data 0 to 99.
 */
BOOST_AUTO_TEST_CASE(CustomImputationTest)
{
  arma::mat columnWiseInput("3.0 0.0 2.0 0.0;"
                  "5.0 6.0 0.0 6.0;"
                  "9.0 8.0 4.0 8.0;");
  arma::mat rowWiseInput(columnWiseInput);
  double customValue = 99;
  double mappedValue = 0.0;

  CustomImputation<double> imputer(customValue);

  // column wise
  imputer.Impute(columnWiseInput, mappedValue, 0/*dimension*/, true);

  BOOST_REQUIRE_CLOSE(columnWiseInput(0, 0), 3.0, 1e-5);
  BOOST_REQUIRE_CLOSE(columnWiseInput(0, 1), 99.0, 1e-5);
  BOOST_REQUIRE_CLOSE(columnWiseInput(0, 2), 2.0, 1e-5);
  BOOST_REQUIRE_CLOSE(columnWiseInput(0, 3), 99.0, 1e-5);
  BOOST_REQUIRE_CLOSE(columnWiseInput(1, 0), 5.0, 1e-5);
  BOOST_REQUIRE_CLOSE(columnWiseInput(1, 1), 6.0, 1e-5);
  BOOST_REQUIRE_CLOSE(columnWiseInput(1, 2), 0.0, 1e-5);
  BOOST_REQUIRE_CLOSE(columnWiseInput(1, 3), 6.0, 1e-5);
  BOOST_REQUIRE_CLOSE(columnWiseInput(2, 0), 9.0, 1e-5);
  BOOST_REQUIRE_CLOSE(columnWiseInput(2, 1), 8.0, 1e-5);
  BOOST_REQUIRE_CLOSE(columnWiseInput(2, 2), 4.0, 1e-5);
  BOOST_REQUIRE_CLOSE(columnWiseInput(2, 3), 8.0, 1e-5);

  // row wise
  imputer.Impute(rowWiseInput, mappedValue, 1, false);

  BOOST_REQUIRE_CLOSE(rowWiseInput(0, 0), 3.0, 1e-5);
  BOOST_REQUIRE_CLOSE(rowWiseInput(0, 1), 99.0, 1e-5);
  BOOST_REQUIRE_CLOSE(rowWiseInput(0, 2), 2.0, 1e-5);
  BOOST_REQUIRE_CLOSE(rowWiseInput(0, 3), 0.0, 1e-5);
  BOOST_REQUIRE_CLOSE(rowWiseInput(1, 0), 5.0, 1e-5);
  BOOST_REQUIRE_CLOSE(rowWiseInput(1, 1), 6.0, 1e-5);
  BOOST_REQUIRE_CLOSE(rowWiseInput(1, 2), 0.0, 1e-5);
  BOOST_REQUIRE_CLOSE(rowWiseInput(1, 3), 6.0, 1e-5);
  BOOST_REQUIRE_CLOSE(rowWiseInput(2, 0), 9.0, 1e-5);
  BOOST_REQUIRE_CLOSE(rowWiseInput(2, 1), 8.0, 1e-5);
  BOOST_REQUIRE_CLOSE(rowWiseInput(2, 2), 4.0, 1e-5);
  BOOST_REQUIRE_CLOSE(rowWiseInput(2, 3), 8.0, 1e-5);
}

/**
 * Make sure MeanImputation method replaces data 0 to mean value of each
 * dimensions.
 */
BOOST_AUTO_TEST_CASE(MeanImputationTest)
{
  arma::mat columnWiseInput("3.0 0.0 2.0 0.0;"
                  "5.0 6.0 0.0 6.0;"
                  "9.0 8.0 4.0 8.0;");
  arma::mat rowWiseInput(columnWiseInput);
  double mappedValue = 0.0;

  MeanImputation<double> imputer;

  // column wise
  imputer.Impute(columnWiseInput, mappedValue, 0, true);

  BOOST_REQUIRE_CLOSE(columnWiseInput(0, 0), 3.0, 1e-5);
  BOOST_REQUIRE_CLOSE(columnWiseInput(0, 1), 2.5, 1e-5);
  BOOST_REQUIRE_CLOSE(columnWiseInput(0, 2), 2.0, 1e-5);
  BOOST_REQUIRE_CLOSE(columnWiseInput(0, 3), 2.5, 1e-5);
  BOOST_REQUIRE_CLOSE(columnWiseInput(1, 0), 5.0, 1e-5);
  BOOST_REQUIRE_CLOSE(columnWiseInput(1, 1), 6.0, 1e-5);
  BOOST_REQUIRE_CLOSE(columnWiseInput(1, 2), 0.0, 1e-5);
  BOOST_REQUIRE_CLOSE(columnWiseInput(1, 3), 6.0, 1e-5);
  BOOST_REQUIRE_CLOSE(columnWiseInput(2, 0), 9.0, 1e-5);
  BOOST_REQUIRE_CLOSE(columnWiseInput(2, 1), 8.0, 1e-5);
  BOOST_REQUIRE_CLOSE(columnWiseInput(2, 2), 4.0, 1e-5);
  BOOST_REQUIRE_CLOSE(columnWiseInput(2, 3), 8.0, 1e-5);

  // row wise
  imputer.Impute(rowWiseInput, mappedValue, 1, false);

  BOOST_REQUIRE_CLOSE(rowWiseInput(0, 0), 3.0, 1e-5);
  BOOST_REQUIRE_CLOSE(rowWiseInput(0, 1), 7.0, 1e-5);
  BOOST_REQUIRE_CLOSE(rowWiseInput(0, 2), 2.0, 1e-5);
  BOOST_REQUIRE_CLOSE(rowWiseInput(0, 3), 0.0, 1e-5);
  BOOST_REQUIRE_CLOSE(rowWiseInput(1, 0), 5.0, 1e-5);
  BOOST_REQUIRE_CLOSE(rowWiseInput(1, 1), 6.0, 1e-5);
  BOOST_REQUIRE_CLOSE(rowWiseInput(1, 2), 0.0, 1e-5);
  BOOST_REQUIRE_CLOSE(rowWiseInput(1, 3), 6.0, 1e-5);
  BOOST_REQUIRE_CLOSE(rowWiseInput(2, 0), 9.0, 1e-5);
  BOOST_REQUIRE_CLOSE(rowWiseInput(2, 1), 8.0, 1e-5);
  BOOST_REQUIRE_CLOSE(rowWiseInput(2, 2), 4.0, 1e-5);
  BOOST_REQUIRE_CLOSE(rowWiseInput(2, 3), 8.0, 1e-5);
}

/**
 * Make sure MeanImputation method replaces data 0 to median value of each
 * dimensions.
 */
BOOST_AUTO_TEST_CASE(MedianImputationTest)
{
  arma::mat columnWiseInput("3.0 0.0 2.0 0.0;"
                  "5.0 6.0 0.0 6.0;"
                  "9.0 8.0 4.0 8.0;");
  arma::mat rowWiseInput(columnWiseInput);
  double mappedValue = 0.0;

  MedianImputation<double> imputer;

  // column wise
  imputer.Impute(columnWiseInput, mappedValue, 1, true);

  BOOST_REQUIRE_CLOSE(columnWiseInput(0, 0), 3.0, 1e-5);
  BOOST_REQUIRE_CLOSE(columnWiseInput(0, 1), 0.0, 1e-5);
  BOOST_REQUIRE_CLOSE(columnWiseInput(0, 2), 2.0, 1e-5);
  BOOST_REQUIRE_CLOSE(columnWiseInput(0, 3), 0.0, 1e-5);
  BOOST_REQUIRE_CLOSE(columnWiseInput(1, 0), 5.0, 1e-5);
  BOOST_REQUIRE_CLOSE(columnWiseInput(1, 1), 6.0, 1e-5);
  BOOST_REQUIRE_CLOSE(columnWiseInput(1, 2), 6.0, 1e-5);
  BOOST_REQUIRE_CLOSE(columnWiseInput(1, 3), 6.0, 1e-5);
  BOOST_REQUIRE_CLOSE(columnWiseInput(2, 0), 9.0, 1e-5);
  BOOST_REQUIRE_CLOSE(columnWiseInput(2, 1), 8.0, 1e-5);
  BOOST_REQUIRE_CLOSE(columnWiseInput(2, 2), 4.0, 1e-5);
  BOOST_REQUIRE_CLOSE(columnWiseInput(2, 3), 8.0, 1e-5);

  // row wise
  imputer.Impute(rowWiseInput, mappedValue, 1, false);

  BOOST_REQUIRE_CLOSE(rowWiseInput(0, 0), 3.0, 1e-5);
  BOOST_REQUIRE_CLOSE(rowWiseInput(0, 1), 7.0, 1e-5);
  BOOST_REQUIRE_CLOSE(rowWiseInput(0, 2), 2.0, 1e-5);
  BOOST_REQUIRE_CLOSE(rowWiseInput(0, 3), 0.0, 1e-5);
  BOOST_REQUIRE_CLOSE(rowWiseInput(1, 0), 5.0, 1e-5);
  BOOST_REQUIRE_CLOSE(rowWiseInput(1, 1), 6.0, 1e-5);
  BOOST_REQUIRE_CLOSE(rowWiseInput(1, 2), 0.0, 1e-5);
  BOOST_REQUIRE_CLOSE(rowWiseInput(1, 3), 6.0, 1e-5);
  BOOST_REQUIRE_CLOSE(rowWiseInput(2, 0), 9.0, 1e-5);
  BOOST_REQUIRE_CLOSE(rowWiseInput(2, 1), 8.0, 1e-5);
  BOOST_REQUIRE_CLOSE(rowWiseInput(2, 2), 4.0, 1e-5);
}

/**
 * Make sure ListwiseDeletion method deletes the whole column (if column wise)
 * or the row (if row wise) containing value of 0.
 */
BOOST_AUTO_TEST_CASE(ListwiseDeletionTest)
{
  arma::mat columnWiseInput("3.0 0.0 2.0 0.0;"
                  "5.0 6.0 0.0 6.0;"
                  "9.0 8.0 4.0 8.0;");
  arma::mat rowWiseInput(columnWiseInput);
  double mappedValue = 0.0;

  ListwiseDeletion<double> imputer;

  // column wise
  imputer.Impute(columnWiseInput, mappedValue, 0, true); // column wise

  BOOST_REQUIRE_CLOSE(columnWiseInput(0, 0), 3.0, 1e-5);
  BOOST_REQUIRE_CLOSE(columnWiseInput(0, 1), 2.0, 1e-5);
  BOOST_REQUIRE_CLOSE(columnWiseInput(1, 0), 5.0, 1e-5);
  BOOST_REQUIRE_CLOSE(columnWiseInput(1, 1), 0.0, 1e-5);
  BOOST_REQUIRE_CLOSE(columnWiseInput(2, 0), 9.0, 1e-5);
  BOOST_REQUIRE_CLOSE(columnWiseInput(2, 1), 4.0, 1e-5);

  // row wise
  imputer.Impute(rowWiseInput, mappedValue, 1, false); // row wise

  BOOST_REQUIRE_CLOSE(rowWiseInput(0, 0), 5.0, 1e-5);
  BOOST_REQUIRE_CLOSE(rowWiseInput(0, 1), 6.0, 1e-5);
  BOOST_REQUIRE_CLOSE(rowWiseInput(0, 2), 0.0, 1e-5);
  BOOST_REQUIRE_CLOSE(rowWiseInput(0, 3), 6.0, 1e-5);
  BOOST_REQUIRE_CLOSE(rowWiseInput(1, 0), 9.0, 1e-5);
  BOOST_REQUIRE_CLOSE(rowWiseInput(1, 1), 8.0, 1e-5);
  BOOST_REQUIRE_CLOSE(rowWiseInput(1, 2), 4.0, 1e-5);
  BOOST_REQUIRE_CLOSE(rowWiseInput(1, 3), 8.0, 1e-5);
}

BOOST_AUTO_TEST_SUITE_END();


// property_map.cpp

//----------------------------------------------------------
// Test various types of containers and property maps
// compatible with Point_set_processing_3 package.
//----------------------------------------------------------
// Usage: no parameters

#include <CGAL/Simple_cartesian.h>
#include <CGAL/property_map.h>
#include <CGAL/value_type_traits.h>

#include <algorithm>
#include <vector>
#include <boost/tuple/tuple.hpp>
#include <boost/tuple/tuple_io.hpp>

typedef CGAL::Simple_cartesian<double> K;
typedef K::Point_3 Point_3;
typedef K::Vector_3 Vector_3;

// Point with normal vector stored in a std::pair.
typedef std::pair<Point_3, Vector_3> PointVectorPair;

// Data type = index, followed by the point, followed by a boolean
// that tells us whether the normal is oriented or not, followed by the normal vector.
typedef boost::tuple<int, Point_3, bool, Vector_3> IndexedPointWithOrientableNormalTuple;

// This is an implementation detail of the process_point_set function.
// We need this function because in process_point_set() we use std::sort.
// We sort arbitrary objects of type T, and the property map will allow
// us to access the Point_3 associated to it.

template <typename T, typename PointPMap>
struct MyLess {

  PointPMap pmap;

  MyLess(const PointPMap& p)
    : pmap(p)
  {}

  bool operator()(const T& t0, const T& t1) const
  {
    return get(pmap, t0) < get(pmap, t1);
  }
};

// In this example we have a function that only operates on the point part.
// It sorts them lexicographically.

template <typename Iterator, typename PointPMap >
void process_point_set(Iterator beg, Iterator end, PointPMap pmap)
{
  MyLess<typename std::iterator_traits<Iterator>::value_type,PointPMap> less(pmap);
  std::sort(beg,end,less);
}

// We can call it just with points. Then interally we use a property map
// that maps point iterators on points.

template <typename Iterator>
void process_point_set(Iterator beg, Iterator end)
{
  process_point_set(beg,end,
    CGAL::make_identity_property_map(
      typename CGAL::value_type_traits<Iterator>::type())
    );
}

// Here comes a function that changes the orientation and the normal

template <typename Iterator, typename OrientationPMap, typename NormalPMap >
void orient_normals(Iterator beg, Iterator end, OrientationPMap orient_pmap, NormalPMap normal_pmap)
{
  for(;beg!= end;++beg){
    const Vector_3& v = get(normal_pmap, *beg);
    put(orient_pmap, *beg, (v == CGAL::NULL_VECTOR));

    if(v.x() < 0){
      put(normal_pmap,*beg, -v);
    }
  }
}

int main()
{
  CGAL::set_pretty_mode(std::cout);

  // Here we run it on plain points. No need for a property map
  {
    std::vector<Point_3> points;

    process_point_set(points.begin(), points.end());
  }

  // Here we run it on points with normal vectors stored in a std::pair.
  // We use a property map that accesses pair::first.
  {
    std::vector<PointVectorPair> points;

    for(int i = 0; i < 10; i++){
      points.push_back(std::make_pair(Point_3(9-i,0,0), Vector_3(i,0,0)));
    }

    process_point_set(points.begin(),
                      points.end(),
                      CGAL::First_of_pair_property_map<PointVectorPair>());

    for(int i = 0; i < 10; i++){
      std::cout << points[i].first << "\t" << points[i].second << std::endl;
    }
  }

  // Here we run it on tuples. To see the interest I made up my own
  // data type = index, followed by the point, followed by a boolean
  // that tells us whether the normal is oriented or not, followed by the normal vector.
  // As the point is the second element of the tuple (that is with index 1)
  // we use a property map that accesses the 1st element of the tuple.
  {
    std::vector<IndexedPointWithOrientableNormalTuple> points;

    for(int i = 0; i < 10; i++){
      double x = (i%2)?i:-i;
      points.push_back(boost::make_tuple(i,Point_3(9-i,0,0), false, Vector_3(x,0,0)));
    }

    process_point_set(points.begin(),
                      points.end(),
                      CGAL::Nth_of_tuple_property_map<1,IndexedPointWithOrientableNormalTuple>());

    std::cout << boost::tuples::set_open('[') << boost::tuples::set_close(']') << boost::tuples::set_delimiter(',');

    for(int i = 0; i < 10; i++){
      std::cout << points[i]  << std::endl;
    }

    //We keep the sequence in order, but determine the normal and if it is different from zero set the Boolean to true
    orient_normals(points.begin(),
                   points.end(),
                   CGAL::make_nth_of_tuple_property_map<2>(IndexedPointWithOrientableNormalTuple()),
                   CGAL::make_nth_of_tuple_property_map<3>(IndexedPointWithOrientableNormalTuple()));

    std::cout << "\nAfter orient_normals\n";
    for(int i = 0; i < 10; i++){
      std::cout << points[i]  << std::endl;
    }
  }
  return 0;
}


#ifndef OOQPINTERFACE_HPP
#define OOQPINTERFACE_HPP

#include "stochasticInput.hpp"
#include "QpGenVars.h"
#include "QpGenResiduals.h"
#include "SimpleVector.h"
#include "Status.h"
#include "QpGenData.h"
#include "OoqpVersion.h"
#include <boost/scoped_ptr.hpp>
#include "SparseGenMatrix.h"
#include "SparseSymMatrix.h"

using boost::scoped_ptr;

template<typename SOLVER, typename FORMULATION>
class OOQPInterface {
public:
	OOQPInterface(stochasticInput &, MPI_Comm = MPI_COMM_WORLD);

	void go();
	double getObjective() const;
	std::vector<double> getFirstStagePrimalColSolution() const;
	std::vector<double> getSecondStagePrimalColSolution(int scen) const;
	std::vector<double> getSecondStageDualRowSolution(int scen) const;

	static bool isDistributed() { return false; }

protected:
	scoped_ptr<FORMULATION> qp;
	scoped_ptr<QpGenData> prob;
	scoped_ptr<QpGenVars> vars;
	scoped_ptr<Residuals> resid;
	scoped_ptr<SOLVER> s;
	vector<int> primalOffsets;
	vector<int> rowToEqIneqIdx;
	vector<int> rowOffsets;
	MPI_Comm comm;

};

namespace{

void scanRows(stochasticInput &input, int &my, int &mz, int &nnzA, int &nnzC, vector<int> &rowToEqIneqIdx) {

	my = mz = nnzA = nnzC = 0;
	{
		CoinPackedMatrix Arow;
		Arow.reverseOrderedCopyOf(input.getFirstStageConstraints());
		int nrow1 = input.nFirstStageCons();
		std::vector<double> const &l = input.getFirstStageRowLB(), &u = input.getFirstStageRowUB();
		for (int i = 0; i < nrow1; i++) {
			if (l[i] == u[i]) {
				my++;
				nnzA += Arow.getVectorSize(i);
			} else {
				mz++;
				nnzC += Arow.getVectorSize(i);
			}
		}
	}

	int nscen = input.nScenarios();
	int rowi = 0;
	for (int i = 0; i < nscen; i++) {
		CoinPackedMatrix Wrow, Trow;
		Wrow.reverseOrderedCopyOf(input.getSecondStageConstraints(i));
		Trow.reverseOrderedCopyOf(input.getLinkingConstraints(i));
		std::vector<double> const &l = input.getSecondStageRowLB(i), &u = input.getSecondStageRowUB(i);
		int nrow2 = input.nSecondStageCons(i);
		for (int k = 0; k < nrow2; k++) {
			if (l[k] == u[k]) {
				nnzA += Wrow.getVectorSize(k) + Trow.getVectorSize(k);
				rowToEqIneqIdx[rowi++] = my++;
			} else {
				nnzC += Wrow.getVectorSize(k) + Trow.getVectorSize(k);
				rowToEqIneqIdx[rowi++] = -mz++-1;
			}
		}
	}

}

void separateRows(stochasticInput &input, SparseGenMatrix &A, SparseGenMatrix &C) {

	int nscen = input.nScenarios();
	int *rowptrA = A.krowM(), *rowptrC = C.krowM();
	int *colidxA = A.jcolM(), *colidxC = C.jcolM();
	double *eltA = A.M(), *eltC = C.M();

	int offset1 = 0;
	for (int i = 0; i < nscen; i++) offset1 += input.nSecondStageVars(i);

	int nnzA = 0, nnzC = 0;
	int nrowA = 0, nrowC = 0;

	int *rowptr, *colidx, *nnz, *nr;
	double *elt;

	// [ W T ]
	int offset2 = 0;
	for (int s = 0; s < nscen; s++) {
		CoinPackedMatrix Wrow, Trow;
		Wrow.reverseOrderedCopyOf(input.getSecondStageConstraints(s));
		Trow.reverseOrderedCopyOf(input.getLinkingConstraints(s));

		std::vector<double> const &l = input.getSecondStageRowLB(s), &u = input.getSecondStageRowUB(s);
		int nrow = input.nSecondStageCons(s);
		for (int i = 0; i < nrow; i++) {
			if (l[i] == u[i]) {
				rowptr = rowptrA;
				colidx = colidxA;
				elt = eltA;
				nr = &nrowA;
				nnz = &nnzA;
			} else {
				rowptr = rowptrC;
				colidx = colidxC;
				elt = eltC;
				nr = &nrowC;
				nnz = &nnzC;
			}
			rowptr[(*nr)++] = *nnz;
			for (CoinBigIndex k = Wrow.getVectorFirst(i); k < Wrow.getVectorLast(i); k++) {
				colidx[*nnz] = Wrow.getIndices()[k]+offset2;
				elt[(*nnz)++] = Wrow.getElements()[k];
			}
			for (CoinBigIndex k = Trow.getVectorFirst(i); k < Trow.getVectorLast(i); k++) {
				colidx[*nnz] = Trow.getIndices()[k]+offset1;
				elt[(*nnz)++] = Trow.getElements()[k];
			}
		}
		offset2 += input.nSecondStageVars(s);
	}

	CoinPackedMatrix Arow;
	Arow.reverseOrderedCopyOf(input.getFirstStageConstraints());
	int nrow = input.nFirstStageCons();
	std::vector<double> const &l = input.getFirstStageRowLB(), &u = input.getFirstStageRowUB();
	for (int i = 0; i < nrow; i++) {
		if (l[i] == u[i]) {
			rowptr = rowptrA;
			colidx = colidxA;
			elt = eltA;
			nr = &nrowA;
			nnz = &nnzA;
		} else {
			rowptr = rowptrC;
			colidx = colidxC;
			elt = eltC;
			nr = &nrowC;
			nnz = &nnzC;
		}
		rowptr[(*nr)++] = *nnz;
		for (CoinBigIndex k = Arow.getVectorFirst(i); k < Arow.getVectorLast(i); k++) {
			colidx[*nnz] = Arow.getIndices()[k]+offset1;
			elt[(*nnz)++] = Arow.getElements()[k];
		}
	}
	rowptrA[nrowA] = nnzA;
	rowptrC[nrowC] = nnzC;

}

void formQ(stochasticInput &input, SparseSymMatrix &Q) {
	/*
	[ Q_1
	       Q_2
	            ...
	                Q_N
	 Q'_1 Q'_2     Q'_N Q ]
	 Cross terms given by input are transposed forms of Q'_i
	 */

	int nscen = input.nScenarios();
	int *rowQ = Q.krowM(), *colidx = Q.jcolM();
	double *elt = Q.M();

	int nnzQ = 0;
	int nrowQ = 0;
	int offset = 0;
	for (int s = 0; s < nscen; s++) {
		CoinPackedMatrix Qrow;
		Qrow.reverseOrderedCopyOf(input.getSecondStageHessian(s));
		int nrow = input.nSecondStageVars(s);
		for (int i = 0; i < nrow; i++) {
			rowQ[nrowQ++] = nnzQ;
			for (CoinBigIndex k = Qrow.getVectorFirst(i); k < Qrow.getVectorLast(i); k++) {
				colidx[nnzQ] = Qrow.getIndices()[k]+offset;
				elt[nnzQ++] = Qrow.getElements()[k];
			}
		}
		offset += nrow;
	}
	int nvar1 = input.nFirstStageVars();

	CoinPackedMatrix Qrow;
	Qrow.reverseOrderedCopyOf(input.getFirstStageHessian());
	vector<CoinPackedMatrix*> Qcrosses(nscen);
	for (int s = 0; s < nscen; s++) {
		Qcrosses[s] = new CoinPackedMatrix(input.getSecondStageCrossHessian(s));
	}
	for (int i = 0; i < nvar1; i++) {
		rowQ[nrowQ++] = nnzQ;
		offset = 0;
		for (int s = 0; s < nscen; s++) {
			CoinPackedMatrix const& Qcross = *Qcrosses[s];
			for (CoinBigIndex k = Qcross.getVectorFirst(i); k < Qcross.getVectorLast(i); k++) {
				colidx[nnzQ] = Qcross.getIndices()[k]+offset;
				elt[nnzQ++] = Qcross.getElements()[k];
			}
			offset += input.nSecondStageVars(s);
		}
		for (CoinBigIndex k = Qrow.getVectorFirst(i); k < Qrow.getVectorLast(i); k++) {
			colidx[nnzQ] = Qrow.getIndices()[k] + offset;
			elt[nnzQ++] = Qrow.getElements()[k];
		}
	}
	rowQ[nrowQ] = nnzQ;

	for (int s = 0; s < nscen; s++) {
		delete Qcrosses[s];
	}

}

}

template<typename SOLVER, typename FORMULATION>
OOQPInterface<SOLVER,FORMULATION>::OOQPInterface(stochasticInput &input, MPI_Comm comm) : comm(comm) {

	// we put the first-stage variables at the end to help with the sparse reordering
	int nvar1 = input.nFirstStageVars();
	int nscen = input.nScenarios();
	int totalVars = 0;
	for (int i = 0; i < nscen; i++) {
		primalOffsets.push_back(totalVars);
		totalVars += input.nSecondStageVars(i);
	}
	primalOffsets.push_back(totalVars);
	totalVars += nvar1;
	int totalCons = 0;
	for (int i = 0; i < nscen; i++) {
		rowOffsets.push_back(totalCons);
		totalCons += input.nSecondStageCons(i);
	}
	rowOffsets.push_back(totalCons);
	totalCons += input.nFirstStageCons();
	rowToEqIneqIdx.resize(totalCons);

	int my, mz, nnzA, nnzC;

	scanRows(input, my, mz, nnzA, nnzC, rowToEqIneqIdx);

	int nnzQ = input.getFirstStageHessian().getNumElements();
	for (int i = 0; i < nscen; i++) {
		nnzQ += input.getSecondStageHessian(i).getNumElements();
		nnzQ += input.getSecondStageCrossHessian(i).getNumElements();
	}

	qp.reset(new FORMULATION(totalVars,my,mz,nnzQ,nnzA,nnzC));
	prob.reset(dynamic_cast<QpGenData*>(qp->makeData()));

	separateRows(input,dynamic_cast<SparseGenMatrix&>(*prob->A),dynamic_cast<SparseGenMatrix&>(*prob->C));
	formQ(input,dynamic_cast<SparseSymMatrix&>(*prob->Q));

	// the beauty of OOP...
	double *bA = dynamic_cast<SimpleVector&>(*prob->bA).elements();
	double *bl = dynamic_cast<SimpleVector&>(*prob->bl).elements();
	double *bu = dynamic_cast<SimpleVector&>(*prob->bu).elements();
	double *iclow = dynamic_cast<SimpleVector&>(*prob->iclow).elements();
	double *icupp = dynamic_cast<SimpleVector&>(*prob->icupp).elements();
	double *g = dynamic_cast<SimpleVector&>(*prob->g).elements();
	double *blx = dynamic_cast<SimpleVector&>(*prob->blx).elements();
	double *bux = dynamic_cast<SimpleVector&>(*prob->bux).elements();
	double *ixupp = dynamic_cast<SimpleVector&>(*prob->ixupp).elements();
	double *ixlow = dynamic_cast<SimpleVector&>(*prob->ixlow).elements();

	int eq_idx = 0, ineq_idx = 0;
	for (int i = 0; i < nscen; i++) {
		std::vector<double> const &l = input.getSecondStageRowLB(i), &u = input.getSecondStageRowUB(i);
		int nrow = input.nSecondStageCons(i);
		for (int k = 0; k < nrow; k++) {
			if (l[k] == u[k]) {
				bA[eq_idx++] = l[k];
			} else {
				if (l[k] > -1e20) {
					bl[ineq_idx] = l[k];
					iclow[ineq_idx] = 1.;
				} else {
					bl[ineq_idx] = 0.;
					iclow[ineq_idx] = 0.;
				}
				if (u[k] < 1e20) {
					bu[ineq_idx] = u[k];
					icupp[ineq_idx] = 1.;
				} else {
					bu[ineq_idx] = 0.;
					icupp[ineq_idx] = 0.;
				}
				ineq_idx++;
			}
		}
	}
	{
		std::vector<double> const &l = input.getFirstStageRowLB(), &u = input.getFirstStageRowUB();
		int nrow = input.nFirstStageCons();
		for (int k = 0; k < nrow; k++) {
			if (l[k] == u[k]) {
				bA[eq_idx++] = l[k];
			} else {
				if (l[k] > -1e20) {
					bl[ineq_idx] = l[k];
					iclow[ineq_idx] = 1.;
				} else {
					bl[ineq_idx] = 0.;
					iclow[ineq_idx] = 0.;
				}
				if (u[k] < 1e20) {
					bu[ineq_idx] = u[k];
					icupp[ineq_idx] = 1.;
				} else {
					bu[ineq_idx] = 0.;
					icupp[ineq_idx] = 0.;
				}
				ineq_idx++;
			}
		}
	}
	//double RESCALE=1/(4096*1000.);
	int idx = 0;
	for (int i = 0; i < nscen; i++) {
		std::vector<double> const &l = input.getSecondStageColLB(i), &u = input.getSecondStageColUB(i),
			&c = input.getSecondStageObj(i);
		int ncol = input.nSecondStageVars(i);
		for (int k = 0; k < ncol; k++) {
		  //g[idx] = RESCALE*c[k];
		  g[idx] = c[k];
			if (l[k] > -1e20) {
				blx[idx] = l[k];
				ixlow[idx] = 1.;
			} else {
				blx[idx] = 0.;
				ixlow[idx] = 0.;
			}
			if (u[k] < 1e20) {
				bux[idx] = u[k];
				ixupp[idx] = 1.;
			} else {
				bux[idx] = 0.;
				ixupp[idx] = 0.;
			}
			idx++;
		}
	}
	{
		std::vector<double> const &l = input.getFirstStageColLB(), &u = input.getFirstStageColUB(),
			&c = input.getFirstStageObj();
		int ncol = input.nFirstStageVars();
		for (int k = 0; k < ncol; k++) {
		  //g[idx] = RESCALE*c[k];
		  g[idx]=c[k];
			if (l[k] > -1e20) {
				blx[idx] = l[k];
				ixlow[idx] = 1.;
			} else {
				blx[idx] = 0.;
				ixlow[idx] = 0.;
			}
			if (u[k] < 1e20) {
				bux[idx] = u[k];
				ixupp[idx] = 1.;
			} else {
				bux[idx] = 0.;
				ixupp[idx] = 0.;
			}
			idx++;
		}

	}

	vars.reset(dynamic_cast<QpGenVars*>(qp->makeVariables(prob.get())));
	resid.reset(qp->makeResiduals(prob.get()));
	s.reset(new SOLVER(qp.get(),prob.get()));

}

template<typename S, typename F>
void OOQPInterface<S,F>::go() {
	int mype;
	MPI_Comm_rank(comm,&mype);

	s->monitorSelf();
	int result = s->solve(prob.get(),vars.get(),resid.get());

	if ( 0 == result && mype == 0) {
	 double objective = prob->objectiveValue(vars.get());

      cout << " " << prob->nx << " variables, "
	   << prob->my  << " equality constraints, "
	   << prob->mz  << " inequality constraints.\n";

      cout << " Iterates: " << s->iter
	   <<",    Optimal Solution:  " << objective << endl;
      }

}

template<typename S, typename F>
double OOQPInterface<S,F>::getObjective() const {
	return prob->objectiveValue(vars.get());
}

template<typename S, typename F>
std::vector<double> OOQPInterface<S,F>::getFirstStagePrimalColSolution() const {
	double const *sol = &dynamic_cast<SimpleVector const&>(*vars->x)[0];
	return std::vector<double>(sol+primalOffsets[primalOffsets.size()-1],sol+vars->x->length());

}

template<typename S, typename F>
std::vector<double> OOQPInterface<S,F>::getSecondStagePrimalColSolution(int scen) const {
	double const *sol = &dynamic_cast<SimpleVector const&>(*vars->x)[0];
	return std::vector<double>(sol+primalOffsets[scen],sol+primalOffsets[scen+1]);
}

template<typename S, typename F>
std::vector<double> OOQPInterface<S,F>::getSecondStageDualRowSolution(int scen) const {
	std::vector<double> out; out.reserve(rowOffsets[scen+1]-rowOffsets[scen]);
	double const *eqsol = &dynamic_cast<SimpleVector const&>(*vars->y)[0];
	double const *ineqsol = &dynamic_cast<SimpleVector const&>(*vars->z)[0];
	for (int idx = rowOffsets[scen]; idx < rowOffsets[scen+1]; idx++) {
		int r = rowToEqIneqIdx[idx];
		if (r >= 0) { // eq
			out.push_back(eqsol[r]);
		} else {
			out.push_back(ineqsol[-r-1]);
		}
	}
	return out;
}
#endif


#include <igl/read_triangle_mesh.h>
#include <wmtk/TetMesh.h>
#include <catch2/catch.hpp>

#include <wmtk/utils/ManifoldUtils.hpp>
#include "wmtk/utils/Logger.hpp"

#include <igl/is_edge_manifold.h>
#include <igl/is_vertex_manifold.h>
#include <Eigen/Core>

TEST_CASE("separate-manifold-patch", "[test_util]")
{
    std::vector<Eigen::Vector3d> vertices = {
        {Eigen::Vector3d(0, 0, 0),
         Eigen::Vector3d(1, 0, 0),
         Eigen::Vector3d(0, 1, 0),
         Eigen::Vector3d(0, 0, 1),
         Eigen::Vector3d(0, -1, 0)}};
    std::vector<std::array<size_t, 3>> faces = {{
        {{0, 1, 2}},
        {{0, 1, 3}},
        {{0, 1, 4}},
    }};

    std::vector<Eigen::Vector3d> out_v;
    std::vector<std::array<size_t, 3>> out_f;
    std::vector<size_t> freeze_v;
    wmtk::separate_to_manifold(vertices, faces, out_v, out_f, freeze_v);
    REQUIRE(out_v.size() == 9);
    REQUIRE(out_f.size() == 3);
}

TEST_CASE("manifold-separate-test-37989", "[test_util]")
{
    std::string filename = WMT_DATA_DIR "/37989_sf.obj";
    wmtk::manifold_internal::Vertices V;
    wmtk::manifold_internal::Facets F;
    igl::read_triangle_mesh(filename, V, F);
    REQUIRE_FALSE(igl::is_edge_manifold(F));
    std::vector<size_t> modified_vertices;
    wmtk::manifold_internal::resolve_nonmanifoldness(V, F, modified_vertices);
    REQUIRE(modified_vertices.size() > 0);
    REQUIRE(igl::is_edge_manifold(F));
    Eigen::VectorXi VI;
    REQUIRE(igl::is_vertex_manifold(F, VI));
}