/*

Copyright (c) 2005-2021, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

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 University of Oxford nor the names of its
   contributors may be used to endorse or promote products derived from this
   software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#ifndef _TESTACINARUNITMODELS_HPP_
#define _TESTACINARUNITMODELS_HPP_

#include <cxxtest/TestSuite.h>

#include "SimpleBalloonAcinarUnit.hpp"
#include "SimpleBalloonExplicitAcinarUnit.hpp"
#include "SigmoidalAcinarUnit.hpp"
#include "Swan2012AcinarUnit.hpp"
#include "TimeStepper.hpp"
#include "MathsCustomFunctions.hpp"
#include "OutputFileHandler.hpp"
#include "AbstractOdeSystem.hpp"
#include "OdeSystemInformation.hpp"
#include "BackwardEulerIvpOdeSolver.hpp"

#include <boost/math/tools/roots.hpp>
#include <boost/bind.hpp>
#include <iomanip>

//#include "PetscSetupAndFinalize.hpp"

//Class to benchmark sigmoidal solution against
class MySigmoidalOde : public AbstractOdeSystem
{
public:
    MySigmoidalOde(double raw, double a, double b, double c, double d) : AbstractOdeSystem(1)
    {
        mpSystemInfo = OdeSystemInformation<MySigmoidalOde>::Instance();
        mRaw = raw;
        mA = a;
        mB = b;
        mC = c;
        mD = d;
    }

    void EvaluateYDerivatives(double time, const std::vector<double>& rY,
                              std::vector<double>& rDY)
    {
        assert(rY[0] > mA);

        //double pleural_pressure = -2400*(sin((M_PI)*time));
        double pleural_pressure = -750 - 250*sin(2*M_PI*(time - 0.25));

        rDY[0] = 1/mRaw*(-(mD*log(mB/(rY[0] - mA) - 1)) + mC + pleural_pressure);
    }

    double mRaw, mA, mB, mC, mD;
};

template<>
void OdeSystemInformation<MySigmoidalOde>::Initialise()
{
    this->mVariableNames.push_back("V");
    this->mVariableUnits.push_back("dimensionless");
    this->mInitialConditions.push_back(2.3918/1e3);

    this->mInitialised = true;
}

//Class to benchmark swan solution against
class MySwanOde : public AbstractOdeSystem
{
public:
    MySwanOde(double raw, double a, double b, double xi) : AbstractOdeSystem(1)
    {
        mpSystemInfo = OdeSystemInformation<MySwanOde>::Instance();
        mRaw = raw;
        mA = a;
        mB = b;
        mXi = xi;
    }

    void EvaluateYDerivatives(double time, const std::vector<double>& rY,
                              std::vector<double>& rDY)
    {
        double pleural_pressure = -750 - 250*sin(2*M_PI*(time - 0.25));

        double V0 = 1/1e3;
        double lambda = std::pow(rY[0]/V0, 1.0/3.0);

        double gamma = (3.0/4.0)*(3*mA + mB)*(lambda*lambda - 1)*(lambda*lambda - 1);
        double Pe = mXi*std::exp(gamma)/(2.0*lambda)*(3*mA + mB)*(lambda*lambda -1);

        rDY[0] = 1/mRaw*(-Pe - pleural_pressure);
    }

    double mRaw, mA, mB, mXi;
};

template<>
void OdeSystemInformation<MySwanOde>::Initialise()
{
    this->mVariableNames.push_back("V");
    this->mVariableUnits.push_back("dimensionless");
    this->mInitialConditions.push_back(2.3918/1e3);

    this->mInitialised = true;
}

class TestAcinarUnitModels: public CxxTest::TestSuite
{
public:

    void TestSimpleBalloonAcinarUnitInspiration()
    {
        double viscosity = 1.92e-5;               //Pa s
        double terminal_airway_radius = 0.05;   //m
        double terminal_airway_length  = 0.02;   //m
        double terminal_airway_resistance = 8*viscosity*terminal_airway_length/(M_PI*SmallPow(terminal_airway_radius, 4));

        SimpleBalloonAcinarUnit acinus;

        // Coverage
        TS_ASSERT_DELTA(acinus.GetStretchRatio(), 0.0, 1e-6);
        acinus.SetStretchRatio(15.3);
        TS_ASSERT_DELTA(acinus.GetStretchRatio(), 0.0, 1e-6);

        acinus.SetAirwayPressure(0.0);
        acinus.SetPleuralPressure(0.0);
        acinus.SetFlow(0.0);
        acinus.SetUndeformedVolume(0.0);
        double compliance = 0.1/98.0665/1e3;  //in m^3 / pa. Converted from 0.1 L/cmH2O per lung.

        acinus.SetCompliance(compliance);
        acinus.SetTerminalBronchioleResistance(terminal_airway_resistance);

        acinus.SolveAndUpdateState(0.0, 0.2);
        TS_ASSERT_DELTA(acinus.GetFlow(), 0.0, 1e-6);   //With no pressure change we expect no flow
        TS_ASSERT_DELTA(acinus.GetVolume(), 0.0, 1e-2); //With no pressure change we expect no volume change

        TimeStepper time_stepper(0.0, 1.0, 0.0001);
        acinus.SetAirwayPressure(0.0);
        double pleural_pressure = 0.0;

        double ode_volume = 0.0;
        double flow_integral = 0.0;

        while (!time_stepper.IsTimeAtEnd())
        {
            pleural_pressure = - 2400*(sin((M_PI)*(time_stepper.GetNextTime())));

            //Solve the acinar problem coupled to a single bronchiole
            acinus.SetPleuralPressure(pleural_pressure);

            //acinus.SolveAndUpdateState(time_stepper.GetTime(), time_stepper.GetNextTime());
            acinus.ComputeExceptFlow(time_stepper.GetTime(), time_stepper.GetNextTime());

            double airway_pressure = acinus.GetAirwayPressure();
            double flow = -airway_pressure/terminal_airway_resistance;

            flow_integral += (time_stepper.GetNextTime() - time_stepper.GetTime())*flow;

            acinus.SetFlow(flow);
            acinus.UpdateFlow(time_stepper.GetTime(), time_stepper.GetNextTime());

            //Solve the corresponding ODE problem using backward Euler for testing
            // dv/dt = -1/R*(V/C - (Paw - Ppl))
            // Discretise using backward euler and rearrange to obtain the below
            double dt = time_stepper.GetNextTimeStep();
            ode_volume = (ode_volume - dt*pleural_pressure/terminal_airway_resistance)/(1 + dt/(terminal_airway_resistance*compliance));

            TS_ASSERT_DELTA(acinus.GetVolume(), ode_volume, 1e-8);

            time_stepper.AdvanceOneTimeStep();
        }

        TS_ASSERT_DELTA(ode_volume, -compliance*pleural_pressure, 1e-8);
        TS_ASSERT_DELTA(acinus.GetVolume(), -compliance*pleural_pressure, 1e-8);
        TS_ASSERT_DELTA(flow_integral, -compliance*pleural_pressure, 1e-8);

        // Coverage
        TS_ASSERT_THROWS_NOTHING(acinus.SetTimestep(0.01));
    }

    void TestSimpleBalloonExplicitAcinarUnitInspiration()
    {
       double viscosity = 1.92e-5;               //Pa s
       double terminal_airway_radius = 0.05;   //m
       double terminal_airway_length  = 0.02;   //m
       double terminal_airway_resistance = 8*viscosity*terminal_airway_length/(M_PI*SmallPow(terminal_airway_radius, 4));

       SimpleBalloonExplicitAcinarUnit acinus;

       // Coverage
       TS_ASSERT_DELTA(acinus.GetStretchRatio(), 0.0, 1e-6);
       acinus.SetStretchRatio(15.3);
       TS_ASSERT_DELTA(acinus.GetStretchRatio(), 0.0, 1e-6);

       acinus.SetAirwayPressure(0.0);
       acinus.SetPleuralPressure(0.0);
       acinus.SetFlow(0.0);
       acinus.SetUndeformedVolume(0.0);
       double compliance = 0.1/98.0665/1e3;  //in m^3 / pa. Converted from 0.1 L/cmH2O per lung.

       acinus.SetCompliance(compliance);
       acinus.SetTerminalBronchioleResistance(terminal_airway_resistance);

       acinus.SolveAndUpdateState(0.0, 0.2);
       TS_ASSERT_DELTA(acinus.GetFlow(), 0.0, 1e-6);   //With no pressure change we expect no flow
       TS_ASSERT_DELTA(acinus.GetVolume(), 0.0, 1e-2); //With no pressure change we expect no volume change

       //Uncomment below to find time step bound
       //std::cout << 2*terminal_airway_radius*compliance << std::endl; abort();

       TimeStepper time_stepper(0.0, 0.01, 0.0000001); //Only solve for a very short time due to dt restriction, this test is mostly for coverage
       acinus.SetAirwayPressure(0.0);
       double pleural_pressure = 0.0;

       double ode_volume = 0.0;
       double flow_integral = 0.0;

       while (!time_stepper.IsTimeAtEnd())
       {
           pleural_pressure = - 2400*(sin((M_PI)*(time_stepper.GetNextTime())));

           //Solve the acinar problem coupled to a single bronchiole
           acinus.SetPleuralPressure(pleural_pressure);
           acinus.ComputeExceptFlow(time_stepper.GetTime(), time_stepper.GetNextTime());

           double airway_pressure = acinus.GetAirwayPressure();
           double flow = -airway_pressure/terminal_airway_resistance;

           flow_integral += (time_stepper.GetNextTime() - time_stepper.GetTime())*flow;

           acinus.SetFlow(flow);
           acinus.UpdateFlow(time_stepper.GetTime(), time_stepper.GetNextTime());

           //Solve the corresponding ODE problem using backward Euler for testing
           // dv/dt = -1/R*(V/C - (Paw - Ppl))
           // Discretise using backward euler and rearrange to obtain the below
           double dt = time_stepper.GetNextTimeStep();
           ode_volume = (ode_volume - dt*pleural_pressure/terminal_airway_resistance)/(1 + dt/(terminal_airway_resistance*compliance));

           TS_ASSERT_DELTA(acinus.GetVolume(), ode_volume, 1e-8);

           time_stepper.AdvanceOneTimeStep();
       }

       // Coverage
       TS_ASSERT_THROWS_NOTHING(acinus.SetTimestep(0.01));
   }

    void TestSigmoidalAcinarUnitInspiration()
    {
        double viscosity = 1.92e-5;               //Pa s
        double terminal_airway_radius = 0.05;   //m
        double terminal_airway_length  = 0.02;   //m
        double terminal_airway_resistance = 8*viscosity*terminal_airway_length/(M_PI*SmallPow(terminal_airway_radius, 4));

        double a = 2/1e3; //m^3 (RV 2L)
        double b = (6 - 2)/1e3; //m^3 (TLC 6L, RV 2L)
        double c = 667; //Pa (6.8 cmH2O)
        double d = 300; //Pa (3.8 cmH2O)

        SigmoidalAcinarUnit acinus;

        // Coverage
        TS_ASSERT_DELTA(acinus.GetStretchRatio(), 0.0, 1e-6);
        acinus.SetStretchRatio(15.3);
        TS_ASSERT_DELTA(acinus.GetStretchRatio(), 0.0, 1e-6);

        acinus.SetA(a);
        acinus.SetB(b);
        acinus.SetC(c);
        acinus.SetD(d);

        acinus.SetAirwayPressure(0.0);
        acinus.SetPleuralPressure(-1.0);
        acinus.SetFlow(0.0);
        acinus.SetUndeformedVolume(3.4573/1e3); //Nearly completely deflated acinus. Model is invalid if it becomes fully deflated.
        acinus.SetTerminalBronchioleResistance(terminal_airway_resistance);

        acinus.SolveAndUpdateState(0.0, 0.2);
        TS_ASSERT_DELTA(acinus.GetFlow(), 0.0, 1e-6);   //With no pressure change we expect no flow
        TS_ASSERT_DELTA(acinus.GetVolume(), 0.0, 1e-2); //With no pressure change we expect no volume change

        // Setup corresponding ODE for testing
        MySigmoidalOde my_ode(terminal_airway_resistance,a,b,c,d);
        BackwardEulerIvpOdeSolver euler_solver(1);
        std::vector<double> initial_condition;
        initial_condition.push_back(3.4573/1e3);
        OdeSolution solutions = euler_solver.Solve(&my_ode, initial_condition, 0, 2, 0.001, 0.001);

        TimeStepper time_stepper(0.0, 2.0, 0.001);

        unsigned i = 0;
        while (!time_stepper.IsTimeAtEnd())
        {
            double pleural_pressure = -750 - 250*sin(2*M_PI*(time_stepper.GetNextTime() - 0.25));

            TS_ASSERT_DELTA(acinus.GetVolume(), solutions.rGetSolutions()[i][0], 1e-5);
            ++i;

            acinus.SetPleuralPressure(pleural_pressure);
            acinus.ComputeExceptFlow(time_stepper.GetTime(), time_stepper.GetNextTime());

            double airway_pressure = acinus.GetAirwayPressure();
            double flow = -airway_pressure/terminal_airway_resistance;

            acinus.SetFlow(flow);
            acinus.UpdateFlow(time_stepper.GetTime(), time_stepper.GetNextTime());

            time_stepper.AdvanceOneTimeStep();
        }

        // Coverage
        TS_ASSERT_THROWS_NOTHING(acinus.SetTimestep(0.01));
    }

    void TestSwan2012AcinarUnit()
    {
        Swan2012AcinarUnit acinus;

        // Coverage
        TS_ASSERT_DELTA(acinus.GetFlow(), 0.0, 1e-6);
        TS_ASSERT_DELTA(acinus.GetStretchRatio(), 1.26, 1e-6);

        // Test against corresponding ODE
        double viscosity = 1.92e-5;               //Pa s
        double terminal_airway_radius = 0.005;   //m
        double terminal_airway_length  = 0.02;   //m
        double terminal_airway_resistance = 8*viscosity*terminal_airway_length/(M_PI*SmallPow(terminal_airway_radius, 4));

        acinus.SetFlow(0.0);
        acinus.SetAirwayPressure(0.0);
        acinus.SetPleuralPressure(0.0);
        acinus.SetUndeformedVolume(1/1e3);
        acinus.SetStretchRatio(std::pow(2, 1.0/3.0));
        acinus.SetTerminalBronchioleResistance(terminal_airway_resistance);

        MySwanOde my_ode(terminal_airway_resistance, 0.433, -0.611, 2500);
        BackwardEulerIvpOdeSolver euler_solver(1);
        std::vector<double> initial_condition;
        initial_condition.push_back(2/1e3);
        OdeSolution solutions = euler_solver.Solve(&my_ode, initial_condition, 0, 2, 0.001, 0.001);

        TimeStepper time_stepper(0.0, 2.0, 0.001);

        unsigned i = 0;
        while (!time_stepper.IsTimeAtEnd())
        {
            double pleural_pressure = -750 - 250*sin(2*M_PI*(time_stepper.GetNextTime() - 0.25));

            TS_ASSERT_DELTA(acinus.GetVolume(), solutions.rGetSolutions()[i][0], 1e-5);
            ++i;

            acinus.SetPleuralPressure(pleural_pressure);
            acinus.ComputeExceptFlow(time_stepper.GetTime(), time_stepper.GetNextTime());

            double airway_pressure = acinus.GetAirwayPressure();
            double flow = -airway_pressure/terminal_airway_resistance;

            acinus.SetFlow(flow);
            acinus.UpdateFlow(time_stepper.GetTime(), time_stepper.GetNextTime());

            time_stepper.AdvanceOneTimeStep();
        }

        // Coverage
        TS_ASSERT_THROWS_NOTHING(acinus.SetTimestep(0.01));
        TS_ASSERT_THROWS_NOTHING(acinus.SolveAndUpdateState(0.0, 1.0));
    }
};
#endif /*_TESTACINARUNITMODELS_HPP_*/


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2010 Jitse Niesen <jitse@maths.leeds.ac.uk>
//
// Eigen is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 3 of the License, or (at your option) any later version.
//
// Alternatively, you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of
// the License, or (at your option) any later version.
//
// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.

#include "main.h"
#include <limits>
#include <Eigen/Eigenvalues>

template<typename MatrixType> void verifyIsQuasiTriangular(const MatrixType& T)
{
  typedef typename MatrixType::Index Index;

  const Index size = T.cols();
  typedef typename MatrixType::Scalar Scalar;

  // Check T is lower Hessenberg
  for(int row = 2; row < size; ++row) {
    for(int col = 0; col < row - 1; ++col) {
      VERIFY(T(row,col) == Scalar(0));
    }
  }

  // Check that any non-zero on the subdiagonal is followed by a zero and is
  // part of a 2x2 diagonal block with imaginary eigenvalues.
  for(int row = 1; row < size; ++row) {
    if (T(row,row-1) != Scalar(0)) {
      VERIFY(row == size-1 || T(row+1,row) == 0);
      Scalar tr = T(row-1,row-1) + T(row,row);
      Scalar det = T(row-1,row-1) * T(row,row) - T(row-1,row) * T(row,row-1);
      VERIFY(4 * det > tr * tr);
    }
  }
}

template<typename MatrixType> void schur(int size = MatrixType::ColsAtCompileTime)
{
  // Test basic functionality: T is quasi-triangular and A = U T U*
  for(int counter = 0; counter < g_repeat; ++counter) {
    MatrixType A = MatrixType::Random(size, size);
    RealSchur<MatrixType> schurOfA(A);
    VERIFY_IS_EQUAL(schurOfA.info(), Success);
    MatrixType U = schurOfA.matrixU();
    MatrixType T = schurOfA.matrixT();
    verifyIsQuasiTriangular(T);
    VERIFY_IS_APPROX(A, U * T * U.transpose());
  }

  // Test asserts when not initialized
  RealSchur<MatrixType> rsUninitialized;
  VERIFY_RAISES_ASSERT(rsUninitialized.matrixT());
  VERIFY_RAISES_ASSERT(rsUninitialized.matrixU());
  VERIFY_RAISES_ASSERT(rsUninitialized.info());

  // Test whether compute() and constructor returns same result
  MatrixType A = MatrixType::Random(size, size);
  RealSchur<MatrixType> rs1;
  rs1.compute(A);
  RealSchur<MatrixType> rs2(A);
  VERIFY_IS_EQUAL(rs1.info(), Success);
  VERIFY_IS_EQUAL(rs2.info(), Success);
  VERIFY_IS_EQUAL(rs1.matrixT(), rs2.matrixT());
  VERIFY_IS_EQUAL(rs1.matrixU(), rs2.matrixU());

  // Test computation of only T, not U
  RealSchur<MatrixType> rsOnlyT(A, false);
  VERIFY_IS_EQUAL(rsOnlyT.info(), Success);
  VERIFY_IS_EQUAL(rs1.matrixT(), rsOnlyT.matrixT());
  VERIFY_RAISES_ASSERT(rsOnlyT.matrixU());

  if (size > 2)
  {
    // Test matrix with NaN
    A(0,0) = std::numeric_limits<typename MatrixType::Scalar>::quiet_NaN();
    RealSchur<MatrixType> rsNaN(A);
    VERIFY_IS_EQUAL(rsNaN.info(), NoConvergence);
  }
}

void test_schur_real()
{
  CALL_SUBTEST_1(( schur<Matrix4f>() ));
  CALL_SUBTEST_2(( schur<MatrixXd>(internal::random<int>(1,EIGEN_TEST_MAX_SIZE/4)) ));
  CALL_SUBTEST_3(( schur<Matrix<float, 1, 1> >() ));
  CALL_SUBTEST_4(( schur<Matrix<double, 3, 3, Eigen::RowMajor> >() ));

  // Test problem size constructors
  CALL_SUBTEST_5(RealSchur<MatrixXf>(10));
}


#include <map>
#include <set>
#include <utility>
#include <vector>

#include <Eigen/Core>

namespace from_igl {

  using namespace igl;

// Dijstra's algorithm for shortest paths, with multiple targets.
// Adapted from http://rosettacode.org/wiki/Dijkstra%27s_algorithm .
//
// Inputs:
//   source           index of source vertex
//   targets          target vector set
//   w                weight of edge corresponding to VV[i][j]
//   VV               #V list of lists of incident vertices (adjacency list), e.g.
//                    as returned by igl::adjacency_list
//
// Output:
//   min_distance     #V by 1 list of the minimum distances from source to all vertices
//   previous         #V by 1 list of the previous visited vertices (for each vertex) - used for backtracking
//
int dijkstra_compute_paths(const int &source,
                           const std::set<int> &targets,
                           const std::vector<std::vector<int> >& VV,
                           const std::vector<std::vector<double> > &w,
                           Eigen::VectorXd &min_distance,
                           Eigen::VectorXi &previous)
{
  int numV = VV.size();
  min_distance.setConstant(numV, 1, std::numeric_limits<double>::infinity());
  min_distance[source] = 0;
  previous.setConstant(numV, 1, -1);
  std::set<std::pair<double, int> > vertex_queue;
  vertex_queue.insert(std::make_pair(min_distance[source], source));

  while (!vertex_queue.empty())
  {
    double dist = vertex_queue.begin()->first;
    int u = vertex_queue.begin()->second;
    vertex_queue.erase(vertex_queue.begin());

    if (targets.find(u)!= targets.end())
      return u;

    // Visit each edge exiting u
    const std::vector<int> &neighbors = VV[u];
    for (int i = 0; i < neighbors.size(); ++i)
    {
      int v = neighbors[i];
      double distance_through_u = dist + w[u][i];

      if (distance_through_u < min_distance[v]) {
        vertex_queue.erase(std::make_pair(min_distance[v], v));

        min_distance[v] = distance_through_u;
        previous[v] = u;
        vertex_queue.insert(std::make_pair(min_distance[v], v));
      }

    }
  }
  //we should never get here
  return -1;
}
} //from_igl


//===-- Boost.MPL -----------------------------------------------*- C++ -*-===//
///
/// \file
/// \brief Boost 元编程库
/// \sa <https://boost.org/doc/libs/master/libs/mpl/>
///
/// \version 2020-01-14
/// \since 2019-09-05
/// \authors zhengrr
/// \copyright Unlicense
///
//===----------------------------------------------------------------------===//

#include <type_traits>

#include <boost/mpl/at.hpp>
#include <boost/mpl/back.hpp>
#include <boost/mpl/begin.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/mpl/char.hpp>
#include <boost/mpl/deque.hpp>
#include <boost/mpl/empty.hpp>
#include <boost/mpl/end.hpp>
#include <boost/mpl/for_each.hpp>
#include <boost/mpl/front.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/integral_c.hpp>
#include <boost/mpl/list.hpp>
#include <boost/mpl/list_c.hpp>
#include <boost/mpl/map.hpp>
#include <boost/mpl/pop_back.hpp>
#include <boost/mpl/pop_front.hpp>
#include <boost/mpl/push_back.hpp>
#include <boost/mpl/push_front.hpp>
#include <boost/mpl/range_c.hpp>
#include <boost/mpl/set.hpp>
#include <boost/mpl/set_c.hpp>
#include <boost/mpl/size.hpp>
#include <boost/mpl/size_t.hpp>
#include <boost/mpl/string.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/mpl/vector_c.hpp>
#include <gtest/gtest.h>

using namespace std;
namespace mpl = boost::mpl;

namespace rrboost {

/// \brief 编译期常量的类型包装。
/// \details 每一个编译期常量的类型包装都是零元元函数，提供 `()` 方法，返回它自身。
TEST(mpl, type_wrapper_for_compile_time_constant)
{
    using bool_true_type = mpl::bool_<true>;
    static_assert(bool_true_type::value == true);

    using char_a_type = mpl::char_<'a'>;
    static_assert(char_a_type::value == 'a');

    using int_13_type = mpl::int_<13>;
    static_assert(int_13_type::value == 13);

    using long_1337_type = mpl::long_<1337>;
    static_assert(long_1337_type::value == 1337);

    using size_t_0_type = mpl::size_t<0>;
    static_assert(size_t_0_type::value == 0);

    using long_long_19920525_type = mpl::integral_c<long long, 19920525>;
    static_assert(long_long_19920525_type::value == 19920525);
}

/// \brief 元编程断言
/// \details 相较于 `static_assert` 提供更多信息
/// \sa <https://boost.org/doc/libs/master/libs/mpl/doc/refmanual/asserts.html>
TEST(mpl, assert)
{
    BOOST_MPL_ASSERT((mpl::bool_<true>));
    BOOST_MPL_ASSERT_NOT((mpl::bool_<false>));
    BOOST_MPL_ASSERT_RELATION((mpl::int_<1337>::value), == , 1337);
}

/// \brief 类型容器：可变数组
TEST(mpl, vector)
{}

/// \brief 类型容器：链表
TEST(mpl, list)
{}

/// \brief 类型容器：双端队列
TEST(mpl, deque)
{}

/// \brief 类型容器：集合
TEST(mpl, set)
{}

/// \brief 类型容器：映射
TEST(mpl, map)
{}

struct value_printer {
    template< typename U > void operator()(U x)
    {
        std::cout << x << '\n';
    }
};

/// \brief 编译期常量类型包装的容器：范围
/// \sa <https://www.boost.org/doc/libs/master/libs/mpl/doc/refmanual/range-c.html>
TEST(mpl, range_c)
{
    using range0to9 = mpl::range_c<int, 0, 10>;

    BOOST_MPL_ASSERT_RELATION((mpl::size<range0to9>::value), == , 10);
    BOOST_MPL_ASSERT_NOT((mpl::empty<range0to9>));

    BOOST_MPL_ASSERT_RELATION((mpl::front<range0to9>::type::value), == , 0);
    BOOST_MPL_ASSERT_RELATION((mpl::back<range0to9>::type::value), == , 9);
    BOOST_MPL_ASSERT_RELATION((mpl::at_c<range0to9, 5>::type::value), == , 5);
}

/// \brief 编译期常量类型包装的容器：可变数组
TEST(mpl, vector_c)
{
    using vector1to8 = mpl::vector_c<int, 1, 2, 3, 4, 5, 6, 7, 8>;
    using vector1to9 = mpl::push_back<vector1to8, mpl::int_<9>>::type;
    using vector0to9 = mpl::push_front<vector1to9, mpl::int_<0>>::type;

    BOOST_MPL_ASSERT_RELATION((mpl::size<vector0to9>::value), == , 10);
    BOOST_MPL_ASSERT_NOT((mpl::empty<vector0to9>));

    BOOST_MPL_ASSERT_RELATION((mpl::front<vector0to9>::type::value), == , 0);
    BOOST_MPL_ASSERT_RELATION((mpl::back<vector0to9>::type::value), == , 9);
    BOOST_MPL_ASSERT_RELATION((mpl::at_c<vector0to9, 5>::type::value), == , 5);
}

/// \brief 编译期常量类型包装的容器：链表
TEST(mpl, list_c)
{}

/// \brief 编译期常量类型包装的容器：集合
TEST(mpl, set_c)
{}

/// \brief 编译期常量类型包装的容器：字符串
TEST(mpl, string)
{}

/// \brief
TEST(mpl, for_each)
{
    using range0to9 = mpl::range_c<int, 0, 10>;
    mpl::for_each<range0to9>([](auto n) {
        using N = decltype(n);
        cout << N::value << '\t' << n << '\n';
    });
}

}


// From: https://github.com/niessner/VoxelHashing/blob/master/DepthSensingCUDA/Source/MatrixConversion.h

#pragma once

#include <Eigen/Eigen>
// #include "d3dx9math.h"
#include <ftl/cuda_matrix_util.hpp>

namespace MatrixConversion
{
	//static D3DXMATRIX EigToMat(const Eigen::Matrix4f& mat)
	//{
	//	D3DXMATRIX m(mat.data());
	//	D3DXMATRIX res; D3DXMatrixTranspose(&res, &m);

	//	return res;
	//}
	//static Eigen::Matrix4f MatToEig(const D3DXMATRIX& mat)
	//{
	//	return Eigen::Matrix4f((float*)mat.m).transpose();
	//}

	/*static mat4f EigToMat(const Eigen::Matrix4f& mat)
	{
		return mat4f(mat.data()).getTranspose();
	}

	static Eigen::Matrix4f MatToEig(const mat4f& mat)
	{
		return Eigen::Matrix4f(mat.ptr()).transpose();
	}*/

	/*static Eigen::Vector4f VecH(const Eigen::Vector3f& v)
	{
		return Eigen::Vector4f(v[0], v[1], v[2], 1.0);
	}

	static Eigen::Vector3f VecDH(const Eigen::Vector4f& v)
	{
		return Eigen::Vector3f(v[0]/v[3], v[1]/v[3], v[2]/v[3]);
	}*/

	/*static Eigen::Vector3f VecToEig(const vec3f& v)
	{
		return Eigen::Vector3f(v[0], v[1], v[2]);
	}

	static vec3f EigToVec(const Eigen::Vector3f& v)
	{
		return vec3f(v[0], v[1], v[2]);
	}

	static vec3f EigToVec(const D3DXMATRIX v)
	{
		return vec3f(v[0], v[1], v[2]);
	}*/

	// dx/cuda conversion
	/*static vec3f toMlib(const D3DXVECTOR3& v) {
		return vec3f(v.x, v.y, v.z);
	}
	static vec4f toMlib(const D3DXVECTOR4& v) {
		return vec4f(v.x, v.y, v.z, v.w);
	}
	static mat4f toMlib(const D3DXMATRIX& m) {
		mat4f c((const float*)&m);
		return c.getTranspose();
	}
	static D3DXVECTOR3 toDX(const vec3f& v) {
		return D3DXVECTOR3(v.x, v.y, v.z);
	}
	static D3DXVECTOR4 toDX(const vec4f& v) {
		return D3DXVECTOR4(v.x, v.y, v.z, v.w);
	}
	static D3DXMATRIX toDX(const mat4f& m) {
		D3DXMATRIX c((const float*)m.ptr());
		D3DXMatrixTranspose(&c, &c);
		return c;
	}*/

	/*static mat4f toMlib(const float4x4& m) {
		return mat4f(m.ptr());
	}
	static vec4f toMlib(const float4& v) {
		return vec4f(v.x, v.y, v.z, v.w);
	}
	static vec3f toMlib(const float3& v) {
		return vec3f(v.x, v.y, v.z);
	}
	static vec4i toMlib(const int4& v) {
		return vec4i(v.x, v.y, v.z, v.w);
	}
	static vec3i toMlib(const int3& v) {
		return vec3i(v.x, v.y, v.z);
	}
	static float4x4 toCUDA(const mat4f& m) {
		return float4x4(m.ptr());
	}*/
	inline float4x4 toCUDA(const Eigen::Matrix4f& mat) {
		return float4x4(mat.data()).getTranspose();
	}

	inline Eigen::Matrix4f toEigen(const float4x4& m) {
		return Eigen::Matrix4f(m.ptr()).transpose();
	}

	/*static float4 toCUDA(const vec4f& v) {
		return make_float4(v.x, v.y, v.z, v.w);
	}
	static float3 toCUDA(const vec3f& v) {
		return make_float3(v.x, v.y, v.z);
	}
	static int4 toCUDA(const vec4i& v) {
		return make_int4(v.x, v.y, v.z, v.w);
	}
	static int3 toCUDA(const vec3i& v) {
		return make_int3(v.x, v.y, v.z);
	}*/

	//doesnt really work
	//template<class FloatType>
	//point3d<FloatType> eulerAngles(const Matrix3x3<FloatType>& r) {
	//	point3d<FloatType> res;

	//	//check for gimbal lock
	//	if (r(0,2) == (FloatType)-1.0) {
	//		FloatType x = 0; //gimbal lock, value of x doesn't matter
	//		FloatType y = (FloatType)M_PI / 2;
	//		FloatType z = x + atan2(r(1,0), r(2,0));
	//		res = point3d<FloatType>(x, y, z);
	//	} else if (r(0,2) == (FloatType)1.0) {
	//		FloatType x = 0;
	//		FloatType y = -(FloatType)M_PI / 2;
	//		FloatType z = -x + atan2(-r(1,0), -r(2,0));
	//		res = point3d<FloatType>(x, y, z);
	//	} else { //two solutions exist
	//		FloatType x1 = -asin(r(0,2));
	//		FloatType x2 = (FloatType)M_PI - x1;

	//		FloatType y1 = atan2(r(1,2) / cos(x1), r(2,2) / cos(x1));
	//		FloatType y2 = atan2(r(1,2) / cos(x2), r(2,2) / cos(x2));

	//		FloatType z2 = atan2(r(0,1) / cos(x2), r(0,0) / cos(x2));
	//		FloatType z1 = atan2(r(0,1) / cos(x1), r(0,0) / cos(x1));

	//		//choose one solution to return
	//		//for example the "shortest" rotation
	//		if ((std::abs(x1) + std::abs(y1) + std::abs(z1)) <= (std::abs(x2) + std::abs(y2) + std::abs(z2))) {
	//			res = point3d<FloatType>(x1, y1, z1);
	//		} else {
	//			res = point3d<FloatType>(x2, y2, z2);
	//		}
	//	}

	//	res.x = math::radiansToDegrees(-res.x);
	//	res.y = math::radiansToDegrees(-res.y);
	//	res.z = math::radiansToDegrees(-res.z);

	//	return res;
	//}
}


/*
 * Copyright (c) 2011 Adrian Michel
 * http://www.amichel.com
 *
 * Permission to use, copy, modify, distribute and sell this
 * software and its documentation for any purpose is hereby
 * granted without fee, provided that both the above copyright
 * notice and this permission notice appear in all copies and in
 * the supporting documentation.
 *
 * This library is distributed in the hope that it will be
 * useful. However, Adrian Michel makes no representations about
 * the suitability of this software for any purpose.  It is
 * provided "as is" without any express or implied warranty.
 *
 * Should you find this library useful, please email
 * info@amichel.com with a link or other reference
 * to your work.
*/

#ifndef DE_RANDOM_GENERATOR_HPP_INCLUDED
#define DE_RANDOM_GENERATOR_HPP_INCLUDED

// MS compatible compilers support #pragma once

#if defined(_MSC_VER) && (_MSC_VER >= 1020)
#pragma once
#endif

#include <boost/math/special_functions/round.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/mersenne_twister.hpp>

namespace de
{

inline double genrand( double min = 0, double max = 1 )
{
	static boost::random::mt19937 gen;
	boost::random::uniform_real_distribution<> dist( min, max );
	boost::variate_generator<boost::random::mt19937&, boost::random::uniform_real_distribution< double > > value(gen, dist);

	return value();
}

inline int genintrand( double min, double max, bool upperexclusive = false )
{
	assert( min < max );
	int ret = 0;
	do ret = boost::math::round( genrand( min, max ) ); while( ret < min || ret > max || upperexclusive && ret == max );
	return ret;
}

}

#endif //DE_RANDOM_GENERATOR_HPP_INCLUDED


#pragma once

#include <polyfem/Problem.hpp>
#include <polyfem/ProblemWithSolution.hpp>

#include <vector>
#include <Eigen/Dense>

namespace polyfem
{
	class ElasticProblem : public Problem
	{
	public:
		ElasticProblem(const std::string &name);

		void rhs(const AssemblerUtils &assembler, const std::string &formulation, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		bool is_rhs_zero() const override { return true; }

		void bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;

		bool has_exact_sol() const override { return false; }
		bool is_scalar() const override { return false; }

		int n_incremental_load_steps(const double diag) const override { return 1 / diag; }
	};

	class TorsionElasticProblem : public Problem
	{
	public:
		TorsionElasticProblem(const std::string &name);

		void rhs(const AssemblerUtils &assembler, const std::string &formulation, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		bool is_rhs_zero() const override { return true; }

		void bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;

		bool has_exact_sol() const override { return false; }
		bool is_scalar() const override { return false; }
		bool is_constant_in_time() const override { return false; }

		void set_parameters(const json &params) override;

		int n_incremental_load_steps(const double diag) const override { return 10 * n_turns_; }

	private:
		double n_turns_ = 0.5;
		int coordiante_0_ = 0;
		int coordiante_1_ = 1;
		RowVectorNd trans_;
	};

	class DoubleTorsionElasticProblem : public Problem
	{
	public:
		DoubleTorsionElasticProblem(const std::string &name);

		void rhs(const AssemblerUtils &assembler, const std::string &formulation, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		bool is_rhs_zero() const override { return true; }

		void bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		void velocity_bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		void acceleration_bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;

		void initial_solution(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &pts, Eigen::MatrixXd &val) const override;
		void initial_velocity(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &pts, Eigen::MatrixXd &val) const override;
		void initial_acceleration(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &pts, Eigen::MatrixXd &val) const override;

		bool has_exact_sol() const override { return false; }
		bool is_scalar() const override { return false; }
		bool is_constant_in_time() const override { return false; }
		bool is_time_dependent() const override { return true; }

		void set_parameters(const json &params) override;

	private:
		double angular_v0_ = 0.5;
		double angular_v1_ = -0.5;
		std::array<int, 2> coordiante_0_ = {{0, 1}};
		std::array<int, 2> coordiante_1_ = {{0, 1}};
		RowVectorNd trans_0_;
		RowVectorNd trans_1_;
	};

	class ElasticProblemZeroBC : public Problem
	{
	public:
		ElasticProblemZeroBC(const std::string &name);
		bool is_rhs_zero() const override { return false; }

		void rhs(const AssemblerUtils &assembler, const std::string &formulation, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		void bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;

		bool has_exact_sol() const override { return false; }
		bool is_scalar() const override { return false; }

		int n_incremental_load_steps(const double diag) const override { return 2 / diag; }
	};

	class ElasticProblemExact : public ProblemWithSolution
	{
	public:
		ElasticProblemExact(const std::string &name);

		VectorNd eval_fun(const VectorNd &pt, const double t) const override;
		AutodiffGradPt eval_fun(const AutodiffGradPt &pt, const double t) const override;
		AutodiffHessianPt eval_fun(const AutodiffHessianPt &pt, const double t) const override;

		bool is_scalar() const override { return false; }
	};

	class CompressionElasticProblemExact : public ProblemWithSolution
	{
	public:
		CompressionElasticProblemExact(const std::string &name);

		VectorNd eval_fun(const VectorNd &pt, const double t) const override;
		AutodiffGradPt eval_fun(const AutodiffGradPt &pt, const double t) const override;
		AutodiffHessianPt eval_fun(const AutodiffHessianPt &pt, const double t) const override;

		bool is_scalar() const override { return false; }
	};

	class QuadraticElasticProblemExact : public ProblemWithSolution
	{
	public:
		QuadraticElasticProblemExact(const std::string &name);

		VectorNd eval_fun(const VectorNd &pt, const double t) const override;
		AutodiffGradPt eval_fun(const AutodiffGradPt &pt, const double t) const override;
		AutodiffHessianPt eval_fun(const AutodiffHessianPt &pt, const double t) const override;

		bool is_scalar() const override { return false; }
	};

	class LinearElasticProblemExact : public ProblemWithSolution
	{
	public:
		LinearElasticProblemExact(const std::string &name);

		VectorNd eval_fun(const VectorNd &pt, const double t) const override;
		AutodiffGradPt eval_fun(const AutodiffGradPt &pt, const double t) const override;
		AutodiffHessianPt eval_fun(const AutodiffHessianPt &pt, const double t) const override;

		bool is_scalar() const override { return false; }
	};

	class GravityProblem : public Problem
	{
	public:
		GravityProblem(const std::string &name);

		void rhs(const AssemblerUtils &assembler, const std::string &formulation, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		bool is_rhs_zero() const override { return false; }

		void bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		void velocity_bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		void acceleration_bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;

		void initial_solution(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &pts, Eigen::MatrixXd &val) const override;
		void initial_velocity(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &pts, Eigen::MatrixXd &val) const override;
		void initial_acceleration(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &pts, Eigen::MatrixXd &val) const override;

		bool has_exact_sol() const override { return false; }
		bool is_scalar() const override { return false; }
		bool is_time_dependent() const override { return true; }

		void set_parameters(const json &params) override;

	private:
		double force_ = 0.1;
	};

	class WalkProblem : public Problem
	{
	public:
		WalkProblem(const std::string &name);

		void rhs(const AssemblerUtils &assembler, const std::string &formulation, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		bool is_rhs_zero() const override { return true; }

		void bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		void velocity_bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		void acceleration_bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;

		void initial_solution(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &pts, Eigen::MatrixXd &val) const override;
		void initial_velocity(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &pts, Eigen::MatrixXd &val) const override;
		void initial_acceleration(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &pts, Eigen::MatrixXd &val) const override;

		bool has_exact_sol() const override { return false; }
		bool is_scalar() const override { return false; }
		bool is_time_dependent() const override { return true; }
		bool is_constant_in_time() const override { return false; }

		int n_incremental_load_steps(const double diag) const override { return 1 / diag; }
	};
} // namespace polyfem


/*
 * Copyright (c) 2012 Jonathan Perry
 * This code is released under the MIT license (see LICENSE file).
 */
#include "codes/strider/StriderTurboCode.h"

#include <vector>
#include <stdexcept>
#include <boost/static_assert.hpp>

#include "codes/turbo/TurboEncoder.h"
#include "codes/turbo/TurboDecoder.h"
#include "codes/InterleavedEncoder.h"
#include "codes/strider/StriderInterleaver.h"
#include "codes/RandomPermutationGenerator.h"

IEncoderPtr StriderTurboCode::createEncoder(uint32_t lengthBits)
{
	// Make turbo encoder
	IEncoderPtr turbo(new TurboEncoder(lengthBits));

	// Create interleaving sequence
	std::vector<uint16_t> interleavingSequence;
	getInterleaver(lengthBits, interleavingSequence);

	// Create interleaved encoder
	IEncoderPtr strider(new InterleavedEncoder(turbo, interleavingSequence));

	return strider;
}

ILLRDecoder::Ptr StriderTurboCode::createDecoder(uint32_t lengthBits)
{
	// Create interleaving sequence
	std::vector<uint16_t> interleavingSequence;
	getInterleaver(lengthBits, interleavingSequence);

	// Create non-interleaved decoder
	TurboDecoder turbo(lengthBits);

	// Create interleaved decoder
	ILLRDecoder::Ptr strider(
			new InterleavedTurboDecoder(turbo, interleavingSequence));

	return strider;
}

void StriderTurboCode::getInterleaver(
		uint32_t lengthBits,
		std::vector<uint16_t> & interleavingSequence)
{
	if(lengthBits == 1530) {
		interleavingSequence.reserve(7680);
		interleavingSequence.assign(striderInterleaver,
									striderInterleaver + 7680);
	} else {
		BOOST_STATIC_ASSERT(RandomPermutationGenerator::SEED_SIZE_UINTS == 4);

		uint32_t numEncodedBits = (5 * lengthBits) + (6 * 3);
		uint32_t numAfterInterleave = numEncodedBits + (6 * 2);

		// Get permutation generator
		uint32_t seed[4] = {4212754317, 3259287066,  877792689, 2365549148};
		RandomPermutationGenerator permGen(numAfterInterleave, seed);

		// set destination size
		interleavingSequence.clear();
		interleavingSequence.reserve(numAfterInterleave);

		// fill the interleaver sequence
		for(uint32_t i = 0; i < numAfterInterleave; i++) {
			uint16_t permVal = permGen.next();
			if(permVal < numEncodedBits) {
				interleavingSequence.push_back(permVal);
			} else {
				uint16_t repeatedIndex = (permVal - numEncodedBits) / 2;
				uint16_t repeatLocation = (5 * lengthBits) + 3 * repeatedIndex;
				interleavingSequence.push_back(repeatLocation);
			}
		}
	}
}


/******************************************************************************
 * Author:   Laurent Kneip                                                    *
 * Contact:  kneip.laurent@gmail.com                                          *
 * License:  Copyright (c) 2013 Laurent Kneip, ANU. 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 ANU 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 ANU OR THE 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 <opengv/absolute_pose/methods.hpp>
#include <opengv/Indices.hpp>

#include <Eigen/NonLinearOptimization>
#include <Eigen/NumericalDiff>

#include <opengv/absolute_pose/modules/main.hpp>
#include <opengv/absolute_pose/modules/Epnp.hpp>
#include <opengv/OptimizationFunctor.hpp>
#include <opengv/math/cayley.hpp>
#include <opengv/math/quaternion.hpp>
#include <opengv/math/roots.hpp>

#include <iostream>

opengv::translation_t
opengv::absolute_pose::p2p(
    const AbsoluteAdapterBase & adapter,
    const std::vector<int> & indices )
{
  assert(indices.size()>1);
  return p2p( adapter, indices[0], indices[1] );
}

opengv::translation_t
opengv::absolute_pose::p2p(
    const AbsoluteAdapterBase & adapter,
    size_t index0,
    size_t index1)
{
  Eigen::Vector3d e1 = adapter.getBearingVector(index0);
  Eigen::Vector3d e3 = adapter.getBearingVector(index1);
  e3 = e1.cross(e3);
  e3 = e3/e3.norm();
  Eigen::Vector3d e2 = e3.cross(e1);

  rotation_t T;
  T.row(0) = e1.transpose();
  T.row(1) = e2.transpose();
  T.row(2) = e3.transpose();

  Eigen::Vector3d n1 = adapter.getPoint(index1) - adapter.getPoint(index0);
  n1 = n1/n1.norm();
  Eigen::Vector3d n3;
  if( (fabs(n1[0]) > fabs(n1[1])) && (fabs(n1[0]) > fabs(n1[2])) )
  {
    n3[1] = 1.0;
    n3[2] = 0.0;
    n3[0] = -n1[1]/n1[0];
  }
  else
  {
    if( (fabs(n1[1]) > fabs(n1[0])) && (fabs(n1[1]) > fabs(n1[2])) )
    {
      n3[2] = 1.0;
      n3[0] = 0.0;
      n3[1] = -n1[2]/n1[1];
    }
    else
    {
      n3[0] = 1.0;
      n3[1] = 0.0;
      n3[2] = -n1[0]/n1[2];
    }
  }
  n3 = n3 / n3.norm();
  Eigen::Vector3d n2 = n3.cross(n1);

  rotation_t N;
  N.row(0) = n1.transpose();
  N.row(1) = n2.transpose();
  N.row(2) = n3.transpose();

  Eigen::Matrix3d Q = T * adapter.getR().transpose() * N.transpose();
  Eigen::Vector3d temp1 = adapter.getPoint(index1) - adapter.getPoint(index0);
  double d_12 = temp1.norm();

  Eigen::Vector3d temp2 = adapter.getBearingVector(index1);
  double cos_beta = e1.dot(temp2);
  double b = 1/( 1 - pow( cos_beta, 2 ) ) - 1;

  if( cos_beta < 0 )
    b = -sqrt(b);
  else
    b = sqrt(b);

  double temp3 = d_12 * ( Q(1,0) * b - Q(0,0) );

  translation_t solution = -temp3 * Q.row(0).transpose();
  solution = adapter.getPoint(index0) + N.transpose()*solution;

  if(
    solution(0,0) != solution(0,0) ||
    solution(1,0) != solution(1,0) ||
    solution(2,0) != solution(2,0) )
    solution = Eigen::Vector3d::Zero();

  return solution;
}

opengv::transformations_t
opengv::absolute_pose::p3p_kneip(
    const AbsoluteAdapterBase & adapter,
    const std::vector<int> & indices )
{
  assert(indices.size()>2);
  return p3p_kneip( adapter, indices[0], indices[1], indices[2] );
}

opengv::transformations_t
opengv::absolute_pose::p3p_kneip(
    const AbsoluteAdapterBase & adapter,
    size_t index0,
    size_t index1,
    size_t index2)
{
  bearingVectors_t f;
  f.push_back(adapter.getBearingVector(index0));
  f.push_back(adapter.getBearingVector(index1));
  f.push_back(adapter.getBearingVector(index2));
  points_t p;
  p.push_back(adapter.getPoint(index0));
  p.push_back(adapter.getPoint(index1));
  p.push_back(adapter.getPoint(index2));
  transformations_t solutions;
  modules::p3p_kneip_main( f, p, solutions );
  return solutions;
}

opengv::transformations_t
opengv::absolute_pose::p3p_gao(
    const AbsoluteAdapterBase & adapter,
    const std::vector<int> & indices )
{
  assert(indices.size()>2);
  return p3p_gao( adapter, indices[0], indices[1], indices[2] );
}

opengv::transformations_t
opengv::absolute_pose::p3p_gao(
    const AbsoluteAdapterBase & adapter,
    size_t index0,
    size_t index1,
    size_t index2)
{
  bearingVectors_t f;
  f.push_back(adapter.getBearingVector(index0));
  f.push_back(adapter.getBearingVector(index1));
  f.push_back(adapter.getBearingVector(index2));
  points_t p;
  p.push_back(adapter.getPoint(index0));
  p.push_back(adapter.getPoint(index1));
  p.push_back(adapter.getPoint(index2));
  transformations_t solutions;
  modules::p3p_gao_main( f, p, solutions );
  return solutions;
}

opengv::transformations_t
opengv::absolute_pose::gp3p(
    const AbsoluteAdapterBase & adapter,
    const std::vector<int> & indices )
{
  assert(indices.size()>2);

  Eigen::Matrix3d f;
  Eigen::Matrix3d v;
  Eigen::Matrix3d p;

  for(size_t i = 0; i < 3; i++)
  {
    f.col(i) = adapter.getBearingVector(indices[i]);
    rotation_t R = adapter.getCamRotation(indices[i]);

    //unrotate the bearingVectors already so the camera rotation doesn't appear
    //in the problem
    f.col(i) = R * f.col(i);
    v.col(i) = adapter.getCamOffset(indices[i]);
    p.col(i) = adapter.getPoint(indices[i]);
  }

  transformations_t solutions;
  modules::gp3p_main(f,v,p,solutions);

  return solutions;
}

opengv::transformations_t
opengv::absolute_pose::gp3p(
    const AbsoluteAdapterBase & adapter,
    size_t index0,
    size_t index1,
    size_t index2)
{
  std::vector<int> indices;
  indices.push_back(index0);
  indices.push_back(index1);
  indices.push_back(index2);

  return gp3p(adapter,indices);
}

namespace opengv
{
namespace absolute_pose
{

transformation_t epnp(
    const AbsoluteAdapterBase & adapter,
    const Indices & indices )
{
  //starting from 4 points, we have a unique solution
  assert(indices.size() > 5);

  modules::Epnp PnP;
  PnP.set_maximum_number_of_correspondences(indices.size());
  PnP.reset_correspondences();

  for( size_t i = 0; i < indices.size(); i++ )
  {
    point_t p = adapter.getPoint(indices[i]);
    bearingVector_t f = adapter.getBearingVector(indices[i]);
    PnP.add_correspondence(p[0], p[1], p[2], f[0], f[1], f[2]);
  }

  double R_epnp[3][3], t_epnp[3];
  PnP.compute_pose(R_epnp, t_epnp);

  rotation_t rotation;
  translation_t translation;

  for(int r = 0; r < 3; r++)
  {
    for(int c = 0; c < 3; c++)
      rotation(r,c) = R_epnp[r][c];
  }

  translation[0] = t_epnp[0];
  translation[1] = t_epnp[1];
  translation[2] = t_epnp[2];

  //take inverse transformation
  rotation.transposeInPlace();
  translation = -rotation * translation;

  transformation_t transformation;
  transformation.col(3) = translation;
  transformation.block<3,3>(0,0) = rotation;
  return transformation;
}

}
}

opengv::transformation_t
opengv::absolute_pose::epnp( const AbsoluteAdapterBase & adapter )
{
  Indices idx(adapter.getNumberCorrespondences());
  return epnp(adapter,idx);
}

opengv::transformation_t
opengv::absolute_pose::epnp(
    const AbsoluteAdapterBase & adapter,
    const std::vector<int> & indices )
{
  Indices idx(indices);
  return epnp(adapter,idx);
}

namespace opengv
{
namespace absolute_pose
{

transformation_t gpnp(
    const AbsoluteAdapterBase & adapter,
    const Indices & indices )
{
  assert( indices.size() > 5 );

  //compute the centroid
  point_t c0 = Eigen::Vector3d::Zero();
  for( size_t i = 0; i < indices.size(); i++ )
    c0 = c0 + adapter.getPoint(indices[i]);
  c0 = c0 / indices.size();

  //compute the point-cloud
  Eigen::MatrixXd p(3,indices.size());
  for( size_t i = 0; i < indices.size(); i++ )
    p.col(i) = adapter.getPoint(indices[i]) - c0;

  //compute the moment
  Eigen::JacobiSVD< Eigen::MatrixXd > SVD(
      p,
      Eigen::ComputeThinU | Eigen::ComputeThinV );

  //define the control points
  points_t c;
  c.push_back(c0);
  //c.push_back(c0 + SVD.singularValues()[0] * SVD.matrixU().col(0));
  //c.push_back(c0 + SVD.singularValues()[1] * SVD.matrixU().col(1));
  //c.push_back(c0 + SVD.singularValues()[2] * SVD.matrixU().col(2));
  c.push_back(c0 + 15.0 * SVD.matrixU().col(0));
  c.push_back(c0 + 15.0 * SVD.matrixU().col(1));
  c.push_back(c0 + 15.0 * SVD.matrixU().col(2));

  //derive the barycentric frame
  Eigen::Vector3d e1 = c[1]-c0;
  double e1dote1 = e1.dot(e1);
  Eigen::Vector3d e2 = c[2]-c0;
  double e2dote2 = e2.dot(e2);
  Eigen::Vector3d e3 = c[3]-c0;
  double e3dote3 = e3.dot(e3);

  //derive the weighting factors
  Eigen::MatrixXd weights(4,indices.size());
  for( size_t i = 0; i < indices.size(); i++ )
  {
    Eigen::Vector3d temp = p.col(i);
    weights(1,i) = temp.dot(e1)/e1dote1;
    weights(2,i) = temp.dot(e2)/e2dote2;
    weights(3,i) = temp.dot(e3)/e3dote3;
    weights(0,i) = 1.0-(weights(1,i)+weights(2,i)+weights(3,i));
  }

  //setup matrix A and vector b
  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(2*indices.size(),12);
  Eigen::MatrixXd b = Eigen::MatrixXd::Zero(2*indices.size(),1);
  for( size_t i = 0; i < indices.size(); i++ )
  {
    translation_t camOffset = adapter.getCamOffset(indices[i]);
    rotation_t camRotation = adapter.getCamRotation(indices[i]);
    //respect the rotation
    bearingVector_t f = camRotation * adapter.getBearingVector(indices[i]);

    A(2*i,0)  =  weights(0,i)*f[2];
    A(2*i,2)  = -weights(0,i)*f[0];
    A(2*i,3)  =  weights(1,i)*f[2];
    A(2*i,5)  = -weights(1,i)*f[0];
    A(2*i,6)  =  weights(2,i)*f[2];
    A(2*i,8)  = -weights(2,i)*f[0];
    A(2*i,9)  =  weights(3,i)*f[2];
    A(2*i,11) = -weights(3,i)*f[0];

    A(2*i+1,1)  =  weights(0,i)*f[2];
    A(2*i+1,2)  = -weights(0,i)*f[1];
    A(2*i+1,4)  =  weights(1,i)*f[2];
    A(2*i+1,5)  = -weights(1,i)*f[1];
    A(2*i+1,7)  =  weights(2,i)*f[2];
    A(2*i+1,8)  = -weights(2,i)*f[1];
    A(2*i+1,10) =  weights(3,i)*f[2];
    A(2*i+1,11) = -weights(3,i)*f[1];

    b(2*i,0)   = f[2]*camOffset[0]-f[0]*camOffset[2];
    b(2*i+1,0) = f[2]*camOffset[1]-f[1]*camOffset[2];
  }

  //computing the SVD
  Eigen::JacobiSVD< Eigen::MatrixXd > SVD2(
      A,
      Eigen::ComputeThinV | Eigen::ComputeThinU );

  //computing the pseudoinverse
  Eigen::MatrixXd invD = Eigen::MatrixXd::Zero(12,12);
  Eigen::MatrixXd D = SVD2.singularValues();
  for( size_t i = 0; i < 12; i++ )
  {
    if( D(i,0) > 1.e-6 )
      invD(i,i) = 1.0/D(i,0);
    else
      invD(i,i) = 0.0;
  }

  //Extract the nullsapce vectors;
  Eigen::MatrixXd V = SVD2.matrixV();

  //computing the nullspace intercept
  Eigen::MatrixXd pinvA = V * invD * SVD2.matrixU().transpose();

  //compute the intercept
  Eigen::Matrix<double,12,1> a = pinvA * b;

  //compute the solution
  transformation_t transformation;
  modules::gpnp_main( a, V, c, transformation );
  return transformation;
}

}
}

opengv::transformation_t
opengv::absolute_pose::gpnp( const AbsoluteAdapterBase & adapter )
{
  Indices idx(adapter.getNumberCorrespondences());
  return gpnp(adapter,idx);
}

opengv::transformation_t
opengv::absolute_pose::gpnp(
    const AbsoluteAdapterBase & adapter,
    const std::vector<int> & indices )
{
  Indices idx(indices);
  return gpnp(adapter,idx);
}

namespace opengv
{
namespace absolute_pose
{

void fill3x10( const Eigen::Vector3d & x, Eigen::Matrix<double,3,10> & Phi )
{
  double x1 = x[0];
  double x2 = x[1];
  double x3 = x[2];

  Phi << x1,  x1, -x1, -x1,     0.0,  2.0*x3, -2.0*x2, 2.0*x2, 2.0*x3,    0.0,
         x2, -x2,  x2, -x2, -2.0*x3,     0.0,  2.0*x1, 2.0*x1,    0.0, 2.0*x3,
         x3, -x3, -x3,  x3,  2.0*x2, -2.0*x1,     0.0,    0.0, 2.0*x1, 2.0*x2;
}

void f(
    const Eigen::Matrix<double,10,10> & M,
    const Eigen::Matrix<double,1,10> & C,
    double gamma,
    Eigen::Vector3d & f )
{
  f[0] = (2*M(0,4)+2*C(0,4));
  f[1] = (2*M(0,5)+2*C(0,5));
  f[2] = (2*M(0,6)+2*C(0,6));
}

void Jac(
    const Eigen::Matrix<double,10,10> & M,
    const Eigen::Matrix<double,1,10> & C,
    double gamma,
    Eigen::Matrix3d & Jac )
{
  Jac(0,0) = (2*M(4,4)+4*M(0,1)-4*M(0,0)+4*C(0,1)-4*C(0,0));
  Jac(0,1) = (2*M(5,4)+2*M(0,7)+2*C(0,7));
  Jac(0,2) = (2*M(6,4)+2*M(0,8)+2*C(0,8));
  Jac(1,0) = (2*M(4,5)+2*M(0,7)+2*C(0,7));
  Jac(1,1) = (2*M(5,5)+4*M(0,2)-4*M(0,0)+4*C(0,2)-4*C(0,0));
  Jac(1,2) = (2*M(6,5)+2*M(0,9)+2*C(0,9));
  Jac(2,0) = (2*M(4,6)+2*M(0,8)+2*C(0,8));
  Jac(2,1) = (2*M(5,6)+2*M(0,9)+2*C(0,9));
  Jac(2,2) = (2*M(6,6)+4*M(0,3)-4*M(0,0)+4*C(0,3)-4*C(0,0));
}

transformations_t upnp(
    const AbsoluteAdapterBase & adapter,
    const Indices & indices )
{
  assert( indices.size() > 2 );

  Eigen::Matrix<double,3,3> F = Eigen::Matrix3d::Zero();
  for( int i = 0; i < (int) indices.size(); i++ )
  {
    Eigen::Matrix<double,3,1> f = adapter.getCamRotation(indices[i]) * adapter.getBearingVector(indices[i]);
    F += f * f.transpose();
  }

  Eigen::Matrix<double,3,3> H_inv = (indices.size() * Eigen::Matrix<double,3,3>::Identity()) - F;
  Eigen::Matrix<double,3,3> H = H_inv.inverse();

  Eigen::Matrix<double,3,10> I = Eigen::Matrix<double,3,10>::Zero();
  Eigen::Matrix<double,3,1> J = Eigen::Matrix<double,3,1>::Zero();
  Eigen::Matrix<double,3,10> Phi;

  for( int i = 0; i < (int) indices.size(); i++ )
  {
    Eigen::Matrix<double,3,1> f = adapter.getCamRotation(indices[i]) * adapter.getBearingVector(indices[i]);
    Eigen::Matrix<double,3,3> Vk = H * ( f * f.transpose() - Eigen::Matrix<double,3,3>::Identity() );
    Eigen::Matrix<double,3,1> p = adapter.getPoint(indices[i]);
    Eigen::Matrix<double,3,1> v = adapter.getCamOffset(indices[i]);

    fill3x10(p,Phi);
    I += Vk * Phi;
    J += Vk * v;
  }

  Eigen::Matrix<double,10,10> M = Eigen::Matrix<double,10,10>::Zero();
  Eigen::Matrix<double,1,10>  C = Eigen::Matrix<double,1,10>::Zero();
  double gamma = 0.0;

  for(int i = 0; i < (int) indices.size(); i++ )
  {
    Eigen::Matrix<double,3,1> f = adapter.getCamRotation(indices[i]) * adapter.getBearingVector(indices[i]);
    Eigen::Matrix<double,3,1> v = adapter.getCamOffset(indices[i]);
    Eigen::Matrix<double,3,1> p = adapter.getPoint(indices[i]);

    fill3x10(p,Phi);
    Eigen::Matrix<double,3,3> temp = f*f.transpose() - Eigen::Matrix<double,3,3>::Identity();
    Eigen::Matrix<double,3,10> Ai =  temp * (Phi + I);
    Eigen::Matrix<double,3, 1> bi = -temp * (  v + J);

    M     += (Ai.transpose() * Ai);
    C     += (bi.transpose() * Ai);
    gamma += (bi.transpose() * bi);
  }

  //now do the main computation
  std::vector<std::pair<double,Eigen::Vector4d>,Eigen::aligned_allocator< std::pair<double,Eigen::Vector4d> > > quaternions1;
  if( indices.size() > 4 )
    modules::upnp_main_sym( M, C, gamma, quaternions1 );
  else
    modules::upnp_main( M, C, gamma, quaternions1 );

  //prepare the output vector
  transformations_t transformations;

  //Round 1: chirality check
  std::vector<std::pair<double,Eigen::Vector4d>,Eigen::aligned_allocator< std::pair<double,Eigen::Vector4d> > > quaternions2;
  for( int i = 0; i < quaternions1.size(); i++ )
  {
    rotation_t Rinv = math::quaternion2rot(quaternions1[i].second);

    Eigen::Matrix<double,10,1> s;
    modules::upnp_fill_s( quaternions1[i].second, s );
    translation_t tinv = I*s - J;

    if( transformations.size() == 0 )
    {
      transformation_t newTransformation;
      newTransformation.block<3,3>(0,0) = Rinv.transpose();
      newTransformation.block<3,1>(0,3) = -newTransformation.block<3,3>(0,0) * tinv;
      transformations.push_back(newTransformation);
    }

    int count_negative = 0;

    for( int j = 0; j < (int) indices.size(); j++ )
    {
      Eigen::Matrix<double,3,1> f = adapter.getCamRotation(indices[j]) * adapter.getBearingVector(indices[j]);
      Eigen::Matrix<double,3,1> p = adapter.getPoint(indices[j]);
      Eigen::Matrix<double,3,1> v = adapter.getCamOffset(indices[j]);

      Eigen::Vector3d p_est = Rinv*p + tinv - v;

      if( p_est.transpose()*f < 0.0 )
        count_negative++;
    }

    if( count_negative < floor(0.2 * indices.size() + 0.5) )
      quaternions2.push_back(quaternions1[i]);
  }

  if( quaternions2.size() == 0 )
    return transformations;
  else
    transformations.clear();

  //Round 2: Second order optimality (plus polishing)
  Eigen::Matrix<double,3,10> I_cay;
  Eigen::Matrix<double,10,10> M_cay;
  Eigen::Matrix<double,1,10>  C_cay;
  double gamma_cay;

  for( size_t q = 0; q < quaternions2.size(); q++ )
  {
    I_cay = Eigen::Matrix<double,3,10>::Zero();
    rotation_t Rinv = math::quaternion2rot(quaternions2[q].second);

    for( int i = 0; i < (int) indices.size(); i++ )
    {
      Eigen::Matrix<double,3,1> f = adapter.getCamRotation(indices[i]) * adapter.getBearingVector(indices[i]);
      Eigen::Matrix<double,3,3> Vk = H * ( f * f.transpose() - Eigen::Matrix<double,3,3>::Identity() );
      Eigen::Matrix<double,3,1> p = Rinv * adapter.getPoint(indices[i]);

      fill3x10(p,Phi);
      I_cay += Vk * Phi;
    }

    M_cay = Eigen::Matrix<double,10,10>::Zero();
    C_cay = Eigen::Matrix<double,1,10>::Zero();
    gamma_cay = 0.0;

    for(int i = 0; i < (int) indices.size(); i++ )
    {
      Eigen::Matrix<double,3,1> f = adapter.getCamRotation(indices[i]) * adapter.getBearingVector(indices[i]);
      Eigen::Matrix<double,3,1> v = adapter.getCamOffset(indices[i]);
      Eigen::Matrix<double,3,1> p = Rinv * adapter.getPoint(indices[i]);

      fill3x10(p,Phi);
      Eigen::Matrix<double,3,3> temp = f*f.transpose() - Eigen::Matrix<double,3,3>::Identity();
      Eigen::Matrix<double,3,10> Ai =  temp * (Phi + I_cay);
      Eigen::Matrix<double,3,1> bi = -temp * (  v + J);

      M_cay     += (Ai.transpose() * Ai);
      C_cay     += (bi.transpose() * Ai);
      gamma_cay += (bi.transpose() * bi);
    }

    //now analyze the eigenvalues of the "Hessian"
    Eigen::Vector3d val;
    Eigen::Matrix3d Jacobian;
    f( M_cay, C_cay, gamma_cay, val );
    Jac( M_cay, C_cay, gamma_cay, Jacobian );
    std::vector<double> characteristicPolynomial;
    characteristicPolynomial.push_back(-1.0);
    characteristicPolynomial.push_back(Jacobian(2,2)+Jacobian(1,1)+Jacobian(0,0));
    characteristicPolynomial.push_back(-Jacobian(2,2)*Jacobian(1,1)-Jacobian(2,2)*Jacobian(0,0)-Jacobian(1,1)*Jacobian(0,0)+pow(Jacobian(1,2),2)+pow(Jacobian(0,2),2)+pow(Jacobian(0,1),2));
    characteristicPolynomial.push_back(Jacobian(2,2)*Jacobian(1,1)*Jacobian(0,0)+2*Jacobian(1,2)*Jacobian(0,2)*Jacobian(0,1)-Jacobian(2,2)*pow(Jacobian(0,1),2)-pow(Jacobian(1,2),2)*Jacobian(0,0)-Jacobian(1,1)*pow(Jacobian(0,2),2));
    std::vector<double> roots = opengv::math::o3_roots( characteristicPolynomial );

    bool allPositive = true;
    for( size_t i = 0; i < roots.size(); i++ )
    {
      if( roots[i] < 0.0 )
      {
        allPositive = false;
        break;
      }
    }

    if( true )//allPositive)//use all results for the moment
    {
      //perform the polishing step
      Eigen::Vector3d cay = - Jacobian.inverse() * val;
      rotation_t Rinv2 = math::cayley2rot(cay) * Rinv;
      quaternion_t q = math::rot2quaternion(Rinv2);

      Eigen::Matrix<double,10,1> s;
      modules::upnp_fill_s(q,s);
      translation_t tinv = I*s - J;

      transformation_t newTransformation;
      newTransformation.block<3,3>(0,0) = Rinv2.transpose();
      newTransformation.block<3,1>(0,3) = -newTransformation.block<3,3>(0,0) * tinv;
      transformations.push_back(newTransformation);
    }
  }

  //if there are no results, simply add the one with lowest score
  if( transformations.size() == 0 )
  {
    Eigen::Vector4d q = quaternions2[0].second;
    Eigen::Matrix<double,10,1> s;
    modules::upnp_fill_s(q,s);
    translation_t tinv = I*s - J;
    rotation_t Rinv = math::quaternion2rot(q);

    transformation_t newTransformation;
    newTransformation.block<3,3>(0,0) = Rinv.transpose();
    newTransformation.block<3,1>(0,3) = -newTransformation.block<3,3>(0,0) * tinv;
    transformations.push_back(newTransformation);
  }

  return transformations;
}

}
}

opengv::transformations_t
opengv::absolute_pose::upnp( const AbsoluteAdapterBase & adapter )
{
  Indices idx(adapter.getNumberCorrespondences());
  return upnp(adapter,idx);
}

opengv::transformations_t
opengv::absolute_pose::upnp(
    const AbsoluteAdapterBase & adapter,
    const std::vector<int> & indices )
{
  Indices idx(indices);
  return upnp(adapter,idx);
}

namespace opengv
{
namespace absolute_pose
{

struct OptimizeNonlinearFunctor1 : OptimizationFunctor<double>
{
  const AbsoluteAdapterBase & _adapter;
  const Indices & _indices;

  OptimizeNonlinearFunctor1(
      const AbsoluteAdapterBase & adapter,
      const Indices & indices ) :
      OptimizationFunctor<double>(6,indices.size()),
      _adapter(adapter),
      _indices(indices) {}

  int operator()(const VectorXd &x, VectorXd &fvec) const
  {
    assert( x.size() == 6 );
    assert( (unsigned int) fvec.size() == _indices.size());

    //compute the current position
    translation_t translation = x.block<3,1>(0,0);
    cayley_t cayley = x.block<3,1>(3,0);
    rotation_t rotation = math::cayley2rot(cayley);

    //compute inverse transformation
    transformation_t inverseSolution;
    inverseSolution.block<3,3>(0,0) = rotation.transpose();
    inverseSolution.col(3) = -inverseSolution.block<3,3>(0,0)*translation;

    Eigen::Matrix<double,4,1> p_hom;
    p_hom[3] = 1.0;

    for(size_t i = 0; i < _indices.size(); i++)
    {
      //get point in homogeneous form
      p_hom.block<3,1>(0,0) = _adapter.getPoint(_indices[i]);

      //compute the reprojection (this is working for both central and
      //non-central case)
      point_t bodyReprojection = inverseSolution * p_hom;
      point_t reprojection = _adapter.getCamRotation(_indices[i]).transpose() *
          (bodyReprojection - _adapter.getCamOffset(_indices[i]));
      reprojection = reprojection / reprojection.norm();

      //compute the score
      double factor = 1.0;
      fvec[i] = factor *
          (1.0 -
          (reprojection.transpose() * _adapter.getBearingVector(_indices[i])));
    }

    return 0;
  }
};

transformation_t optimize_nonlinear(
    const AbsoluteAdapterBase & adapter,
    const Indices & indices )
{
  const int n=6;
  VectorXd x(n);

  x.block<3,1>(0,0) = adapter.gett();
  x.block<3,1>(3,0) = math::rot2cayley(adapter.getR());

  OptimizeNonlinearFunctor1 functor( adapter, indices );
  NumericalDiff<OptimizeNonlinearFunctor1> numDiff(functor);
  LevenbergMarquardt< NumericalDiff<OptimizeNonlinearFunctor1> > lm(numDiff);

  lm.resetParameters();
  lm.parameters.ftol = 1.E1*NumTraits<double>::epsilon();
  lm.parameters.xtol = 1.E1*NumTraits<double>::epsilon();
  lm.parameters.maxfev = 1000;
  lm.minimize(x);

  transformation_t transformation;
  transformation.col(3) = x.block<3,1>(0,0);
  transformation.block<3,3>(0,0) = math::cayley2rot(x.block<3,1>(3,0));
  return transformation;
}

}
}

opengv::transformation_t
opengv::absolute_pose::optimize_nonlinear( const AbsoluteAdapterBase & adapter )
{
  Indices idx(adapter.getNumberCorrespondences());
  return optimize_nonlinear(adapter,idx);
}

opengv::transformation_t
opengv::absolute_pose::optimize_nonlinear(
    const AbsoluteAdapterBase & adapter,
    const std::vector<int> & indices )
{
  Indices idx(indices);
  return optimize_nonlinear(adapter,idx);
}


// g++ -O3 -fopenmp -lstdc++ -Ic:\eigen main.cpp
#include <omp.h>
#include <Eigen/Dense>
#include <iostream>
#include "Neural_Network.h"

using namespace Eigen;
using namespace std;

int main(){

	initParallel();
	setNbThreads(4);

	int nthreads = nbThreads();
	cout << "Num of Threads: " << nthreads << endl;

	Neural_Network NN(vector<int> {28 * 28, 64, 32, 10}, "");

	NN.print();

	const string address_test_out = "C:/mnist/mnist_small_test_out.txt";
	MatrixXd matrix_test_out = NN.read_txt(address_test_out, 1004, 1);

	const string address_test_in = "C:/mnist/mnist_small_test_in.txt";
	MatrixXd matrix_test_in = NN.read_txt(address_test_in, 1004, 28*28);

	const string address_train_out = "C:/mnist/mnist_small_train_out.txt";
	MatrixXd matrix_train_out = NN.read_txt(address_train_out, 6006, 1);

	const string address_train_in = "C:/mnist/mnist_small_train_in.txt";
	MatrixXd matrix_train_in = NN.read_txt(address_train_in, 6006, 28*28);

	cout << "Load data successful" << endl;

	system("pause");

	NN.train(matrix_train_in, matrix_train_out, 0.3, 500, true, matrix_test_in, matrix_test_out);
	NN.test(matrix_test_in, matrix_test_out);

	system("pause");

	return 0;
}


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

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

  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_ARCH_COMMON_GENERIC_FUNCTION_MINUSONE_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_GENERIC_FUNCTION_MINUSONE_HPP_INCLUDED

#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/valmin.hpp>
#include <boost/simd/function/is_not_equal.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/function/selsub.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( minusone_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_<bd::arithmetic_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0) const BOOST_NOEXCEPT
    {
      return selsub(is_not_equal(a0, Valmin<A0>()), a0, One<A0>());
    }
  };

  BOOST_DISPATCH_OVERLOAD ( minusone_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_<bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0) const BOOST_NOEXCEPT
    {
      return a0-One<A0>();
    }
  };
} } }

#endif


#include <iostream>
#include <fstream>
#include <stdlib.h>
#include <stdio.h>
#include <list>
#include <string>
#include <fcm/fcm.h>
#include "features.h"
#include <Eigen/Dense>
#include <easy_logger/easy_logger.h>
//#include <algorithm>
#include <tabular_parser/parser.h>
#include "tada_swift.h"

using namespace std;

TADASwift::TADASwift(){
    m_parsed_data = nullptr;
    string log_fname="swift.log";
    fopen(log_fname.c_str(),"w");
    m_logger = new EasyLogger("swift.log");
    m_fcm = new FCM(2, 0.005);
    m_enabled_features[0]=false; // mean
    m_enabled_features[1]=true; // median
    m_enabled_features[2]=true; // std
}

void TADASwift::train(string model_dir){
    /* This is to train the model from a given tsv file of features */
    string line;
    std::list<std::list<double>*> *features_list = new std::list<std::list<double>*>;
    std::list<string> *line_elements;
    std::list<double> *features_for_point;
    ifstream in_file(model_dir);
    double value;
    int clus_id=0;
    int feature_idx=0;
    if(in_file.is_open()){
            while(getline(in_file, line)){
                //cout << line <<endl;
                m_logger->log("The line: ");
                m_logger->log(line);
                line_elements = this->parse_line(line);  // get the class, property, num of objects and set of features
                // To store the names of the clusters
                m_clusters_names[clus_id++] = (*(line_elements->cbegin())) + "\t" + (*(++(line_elements->cbegin())));
                features_for_point = new std::list<double>; // features for a class/property pair
                feature_idx=0;
                // Loop for each feature
                for(auto it=++++++(line_elements->cbegin()); it != line_elements->cend(); it++, feature_idx++){
                    // If the feature is enabled
                    if(m_enabled_features[feature_idx]){
                        // check if it is a number
                        if(this->str_to_double(*it, value)){
                            // and store it
                            features_for_point->push_back(value);
                        }
                        else{
                            cout << __func__ << "ERROR: invalid value: " << (*it)<<endl;
                        }
                    }
                }
                // append the list of features
                features_list->push_back(features_for_point);
            }//while
            m_logger->log("getting all clusters names");
            for(auto it = m_clusters_names.begin(); it != m_clusters_names.end(); it++) {
                m_logger->log(to_string(it->first) + "\t=>\t" + (it->second));
                //cout << (it->first) << "\t=>\t"<< (it->second)<<endl;
            }
            return this->set_model_from_features(features_list);
        }//if open
    else{
        cout<< __func__ << " ERROR: unable to open the model: "<< model_dir<<endl;
    }
}

std::list<string>* TADASwift::classify(int column_no){
    double value;
    std::list<double> *l;
    std::list<double> features_for_col;
    int idx=0;

    for(auto it=m_parsed_data->cbegin(); it!=m_parsed_data->cend();it++){
        if(idx==column_no){
            // get the list as numeric list
            l = this->get_numbers_from_list(*it);
            if(l!=nullptr){
                m_logger->log("size: "+to_string(l->size()));

                if(m_enabled_features[0]){
                    m_logger->log("will compute the mean");
                    features_for_col.push_back(Features::mean(l));
                }
                if(m_enabled_features[1]){
                    m_logger->log("will compute the median");
                    features_for_col.push_back(Features::median(l));
                }
                if(m_enabled_features[2]){
                    m_logger->log("will compute the std");
                    if(m_enabled_features[0]){// if mean is computed, don't compute it again
                        features_for_col.push_back(Features::stdev(l,*(features_for_col.cbegin())));
                    }
                    else{// if mean is not computed, then do compute it
                        features_for_col.push_back(Features::stdev(l,Features::mean(l)));
                    }
                }
                m_logger->log("creating the data matrix");
                MatrixXf *data = new MatrixXf;
                data->resize(1, features_for_col.size());
                m_logger->log("row: "+to_string(data->rows()));
                m_logger->log("row: "+to_string(data->cols()));
//                cout << "rows: "<<data->rows()<<endl;
//                cout << "cols: "<<data->cols()<<endl;
                int i=0;
                m_logger->log("filling the features to the matrix");
                for(auto it=features_for_col.cbegin();it!=features_for_col.cend();it++){
                    (*data)(0,i++) = *it;
                    //cout << "fill value: " <<*it<<endl;
                }
                m_logger->log("size of features: "+to_string(features_for_col.size()));
//                for(i=0;i<features_for_col.size();i++){
//                    cout << "stored data: " << (*data)(0,i) << endl;
//                }

                //Just to print the centers before fcm change the data
                stringstream s_centers;
                s_centers << (* (m_fcm->get_cluster_center()));
                m_logger->log("cluster centers from the training: ");
                m_logger->log("\n"+s_centers.str());

                m_logger->log("setting the data to the fcm model");
                m_fcm->set_data(data);

                stringstream sss;
                m_logger->log("getting the data");
                m_logger->log("updating the fcm membership");
                m_fcm->update_membership();
                m_logger->log("creating a stream");
                m_logger->log("feeding the stream with membership");
                sss << (* (m_fcm->get_membership()));
                m_logger->log("stream to string");
                m_logger->log("\n"+sss.str());
                m_logger->log("mem rows: "+to_string(m_fcm->get_membership()->rows()));
                m_logger->log("mem cols: "+to_string(m_fcm->get_membership()->cols()));

                //Just to print the centers after fcm change the data
                stringstream s_centers_after;
                s_centers_after << (* (m_fcm->get_cluster_center()));
                m_logger->log("cluster centers after the classification: ");
                m_logger->log("\n"+s_centers_after.str());
                delete l;
                return this->sorted_clus_membership();
                break;
            }
        }
        idx++;
    }
    return nullptr;
}

void TADASwift::parse(string model_dir){
    if(m_parsed_data!=nullptr){
        delete m_parsed_data;
    }
    m_logger->log("will parse the file: "+model_dir);
    Parser p(model_dir);
    m_parsed_data = p.parse();
}

std::list<string>* TADASwift::parse_line(string line){
    std::list<string> *elements = new std::list<string>;
    int start_idx = 0;
    string t;
    for(int i=0;i<line.size();i++){
        if(line[i]=='\t' || line[i]=='\n' || i==line.length()-1){
            t = line.substr(start_idx,i-start_idx);
            elements->push_back(t);
            start_idx = i+1;
        }//if
    }
    return elements;
}

bool TADASwift::str_to_double(string s, double & val){
        int i;
        string num_str="";
        bool got_dec;
        got_dec=0;
        for(i=0;i<s.length();i++){
            if(s[i]>='0' && s[i]<='9'){
                num_str += s[i];
            }
            else if(s[i]=='.' && !got_dec){
                num_str += s[i];
            }
        }
        if(num_str=="" || num_str=="."){
            return 0;
        }
        else{
            val = strtod(num_str.c_str(),NULL);
            return 1;
        }
}

void TADASwift::set_model_from_features(std::list<std::list<double>*>* features_list){
    MatrixXf* data;
    long i,j;
    data = new MatrixXf;
    m_logger->log(" num of rows: "+to_string(features_list->size()));
    m_logger->log(" num of cols: "+to_string((*(features_list->cbegin()))->size()));
    data->resize(features_list->size(), (*(features_list->cbegin()))->size());
    i=0;
    for(auto it=features_list->cbegin(); it!= features_list->cend(); it++, i++){
        j=0;
        for(auto jt=(*it)->cbegin(); jt!=(*it)->cend() ; jt++, j++){
            (*data)(i,j) = *jt;
        }
    }
    m_fcm->set_data(data);
    MatrixXf* membership;
    membership = new MatrixXf;
    membership->resize(data->rows(), data->rows());
    membership->setZero();
    i=0;
    for(auto it=features_list->cbegin(); it!= features_list->cend(); it++, i++){
        (*membership)(i,i) = 1;
    }
    stringstream s;
    s << (*membership);
    m_logger->log("\n"+s.str());
    m_fcm->set_membership(membership);
    m_fcm->compute_centers();
}

std::list<double> * TADASwift::get_numbers_from_list(std::list<string>* str_list){
    double v;
    std::list<double> *l = new std::list<double>;
    long max_of_nonnum;
    max_of_nonnum = str_list->size()/2;
    m_logger->log("the size of the list: "+to_string(str_list->size()));
    for(auto it=str_list->cbegin();it!=str_list->cend();it++){
        m_logger->log("values in a column: "+(*it));
        if(this->str_to_double(*it, v)){
            l->push_back(v);
        }
        else{
            if(max_of_nonnum==0){
                delete l;
                return nullptr;
            }
            max_of_nonnum--;
        }
    }
    return l;
}

void TADASwift::print_data(){
    if(m_parsed_data!=nullptr){
        for(auto it=m_parsed_data->cbegin();it!=m_parsed_data->cend();it++){
            for(auto jt=(*it)->cbegin();jt!=(*it)->cend();jt++){
                cout << (*jt) << " ";
            }
            cout << endl<<"----------"<<endl;
        }
    }
    else{
        cout << "No parsed data\n";
    }
}

std::list<string>* TADASwift::sorted_clus_membership(){
    /*
     * We assume the membership is for a single column
    */
    std::list<struct clus_memb_pair> clus_membs;
    std::list<string>* results = new std::list<string>;
    MatrixXf *membership = m_fcm->get_membership();
    string s;
    m_logger->log("pre sorted");
    for(int i=0;i<membership->cols();i++){
        clus_membs.push_back({i, (*membership)(0,i)});
        m_logger->log("adding: memb for cluster: "+to_string(i)+" with value: "+to_string(clus_membs.back().memb));
    }
    clus_membs.sort();
    for(auto it=clus_membs.cbegin();it!=clus_membs.cend();it++){
        s = m_clusters_names[(*it).clus];
        m_logger->log("sorted: cluster: "+to_string((*it).clus)+" with value: "+to_string((*it).memb)+" cluster name: "+s);
        results->push_back(s);
    }
    return results;
}