//=======================================================================
// 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 tree_augmentation_long_test.cpp
 * @brief
 * @author Attila Bernath
 * @version 1.0
 * @date 2013-07-08
 */

#include "test_utils/logger.hpp"
#include "test_utils/test_result_check.hpp"
#include "test_utils/get_test_dir.hpp"
#include "test_utils/system.hpp"

#include "paal/iterative_rounding/treeaug/tree_augmentation.hpp"
#include "paal/utils/parse_file.hpp"

#include <boost/graph/adjacency_list.hpp>
#include <boost/test/unit_test.hpp>

#include <fstream>

using namespace paal;
using namespace paal::ir;
using namespace boost;

using Graph = adjacency_list<vecS, vecS, undirectedS,
        no_property,
        property<edge_weight_t, double,
        property<edge_color_t, bool>>>;

using Traits = adjacency_list_traits<vecS, vecS, undirectedS>;
using Vertex = graph_traits<Graph>::vertex_descriptor;
using Edge = graph_traits<Graph>::edge_descriptor;

using Index = property_map<Graph, vertex_index_t>::type;
using Cost = property_map<Graph, edge_weight_t>::type;
using TreeMap = property_map<Graph, edge_color_t>::type;

// Read instance in format
// @nodes 6
// label
// 0
// 1
// 2
// 3
// 4
// 5
// @edges 10
//                 label   intree  cost
// 0       1       0       1       0
// 1       2       1       1       0
// 1       3       2       1       0
// 3       4       3       1       0
// 3       5       4       1       0
// 0       3       0       0       1
// 0       2       1       0       1
// 2       4       2       0       1
// 2       5       3       0       1
// 4       5       4       0       1
void read_tree_aug(std::ifstream & is,
        Graph & g, Cost & cost, TreeMap & tree_map) {
    std::string s;
    std::unordered_map<std::string, Vertex> nodes;
    int vertices_num, edges_num;
    is >> s; is >> vertices_num; is >> s;

    for (int i = 0; i < vertices_num; i++) {
        std::string nlabel;
        is >> nlabel;
        nodes[nlabel] = add_vertex(g);
    }

    LOGLN(num_vertices(g));

    is >> s; is >> edges_num; is >> s; is >> s; is >> s;

    for (int i = 0; i < edges_num; i++) {
        // read from the file
        std::string u, v;
        double c;
        int dummy;
        bool in_t;
        is >> u >> v >> dummy >> in_t >> c;

        bool b;
        Traits::edge_descriptor e;
        std::tie(e, b) = add_edge(nodes[u], nodes[v], g);
        assert(b);
        put(cost, e, c);
        put(tree_map, e, in_t);
    }
}

template <typename TA>
// the copy is intended
    double get_lower_bound(TA ta) {
    tree_augmentation_ir_components<> comps;
    lp::glp lp;
    comps.call<Init>(ta, lp);
    auto prob_type = comps.call<SolveLP>(ta, lp);
    BOOST_CHECK_EQUAL(prob_type, lp::OPTIMAL);
    return lp.get_obj_value();
}

BOOST_AUTO_TEST_CASE(tree_augmentation_long) {
    std::string test_dir = paal::system::get_test_data_dir("TREEAUG");
    using paal::system::build_path;

    parse(build_path(test_dir, "tree_aug.txt"), [&](const std::string & fname, std::istream &) {
        LOGLN(fname);
        std::string filename = build_path(test_dir, "cases/" + fname + ".lgf");
        std::ifstream ifs(filename);
        assert(ifs.good());

        Graph g;
        Cost cost = get(edge_weight, g);
        TreeMap tree_map = get(edge_color, g);

        read_tree_aug(ifs, g, cost, tree_map);

        std::vector<Edge> solution;
        auto treeaug(make_tree_aug(g, std::back_inserter(solution)));

        auto invalid = treeaug.check_input_validity();
        BOOST_CHECK(!invalid);
        LOGLN("Input validation " << filename << " ends.");

        double lplowerbd = get_lower_bound(treeaug);
        auto result = solve_iterative_rounding(
            treeaug, tree_augmentation_ir_components<>());
        BOOST_CHECK_EQUAL(result.first, lp::OPTIMAL);

        double solval = treeaug.get_solution_cost();
        BOOST_CHECK_EQUAL(solval, *(result.second));
        check_result_compare_to_bound(solval, lplowerbd, 2);
    });
}


#pragma once

#include <polyfem/ProblemWithSolution.hpp>

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

namespace polyfem
{

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

		VectorNd eval_fun(const VectorNd &pt) const override;
		AutodiffGradPt eval_fun(const AutodiffGradPt &pt) const override;
		AutodiffHessianPt eval_fun(const AutodiffHessianPt &pt) const override;

		bool is_scalar() const override { return true; }

	};
}


// smooth_feedback: Control theory on Lie groups
// https://github.com/pettni/smooth_feedback
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#ifndef SMOOTH__FEEDBACK__EKF_HPP_
#define SMOOTH__FEEDBACK__EKF_HPP_

#include <Eigen/Cholesky>
#include <boost/numeric/odeint.hpp>
#include <smooth/compat/odeint.hpp>
#include <smooth/diff.hpp>
#include <smooth/lie_group.hpp>

namespace smooth::feedback {

/**
 * @brief Extended Kalman filter on Lie groups.
 *
 * The primary methods are predict() and update().
 * - predict(): propagates filter state through a dynamical model.
 * - update(): Bayesian update of filter state from a measurement.
 *
 * Use this class for information fusion (without dynamics) by solely using update().
 *
 * @tparam G \p smooth::LieGroup type.
 * @tparam DiffType \p smooth::diff::Type method for calculating derivatives.
 * @tparam Stpr \p boost::numeric::odeint templated stepper type (\p euler / \p runge_kutta4 /
 * ...). Defaults to \p euler.
 */
template<
  LieGroup G,
  diff::Type DiffType                 = diff::Type::Default,
  template<typename...> typename Stpr = boost::numeric::odeint::euler>
  requires(Dof<G> > 0)
class EKF
{
public:
  //! Scalar type for computations.
  //! Degrees of freedom.
  using CovT = Eigen::Matrix<Scalar<G>, Dof<G>, Dof<G>>;

  /**
   * @brief Reset the state of the EKF.
   *
   * @param g filter value
   * @param P filter covariance
   */
  void reset(const G & g, const CovT & P)
  {
    g_hat_ = g;
    P_     = P;
  }

  /**
   * @brief Access filter state estimate.
   */
  G estimate() const { return g_hat_; }

  /**
   * @brief Access filter covariance.
   */
  CovT covariance() const { return P_; }

  /**
   * @brief Propagate EKF through dynamics \f$ \mathrm{d}^r x_t = f(t, x) \f$ with covariance
   * \f$Q\f$ over a time interval \f$ [0, \tau] \f$.
   *
   * @param f right-hand side \f$ f : \mathbb{R} \times \mathbb{G} \rightarrow \mathbb{R}^{\dim
   * \mathfrak{g}} \f$ of the dynamics. The time type must be the scalar type of G.
   * @param Q process covariance (size \f$ \dim \mathfrak{g} \times \dim \mathfrak{g} \f$)
   * @param tau amount of time to propagate
   * @param dt maximal ODE solver step size (defaults to \p tau, i.e. one step)
   *
   * @note The time \f$ t \f$ argument of \f$ f(t, x) \f$ ranges over the interval \f$t  \in [0,
   * \tau] \f$.
   *
   * @note The covariance \f$ Q \f$ is infinitesimal, i.e. its SI unit is \f$ S^2/T \f$
   * where \f$S\f$ is the unit of state and \f$T\f$ is the unit of time.
   *
   * @note Only the upper triangular part of Q is used.
   */
  template<typename F, typename QDer>
  void predict(
    F && f, const Eigen::MatrixBase<QDer> & Q, Scalar<G> tau, std::optional<Scalar<G>> dt = {})
  {
    const auto state_ode = [&f](const G & g, Tangent<G> & dg, Scalar<G> t) { dg = f(t, g); };

    const auto cov_ode = [this, &f, &Q](const CovT & cov, CovT & dcov, Scalar<G> t) {
      const auto f_x = [&f, &t]<typename _T>(const CastT<_T, G> & x) -> Tangent<CastT<_T, G>> {
        return f(t, x);
      };
      const auto [fv, dr] = diff::dr<1, DiffType>(f_x, wrt(g_hat_));
      const CovT A        = -ad<G>(fv) + dr;
      dcov = (A * cov + cov * A.transpose() + Q).template selfadjointView<Eigen::Upper>();
    };

    Scalar<G> t          = 0;
    const Scalar<G> dt_v = dt.value_or(2 * tau);
    while (t + dt_v < tau) {
      // step covariance first since it depends on g_hat_
      cst_.do_step(cov_ode, P_, t, dt_v);
      sst_.do_step(state_ode, g_hat_, t, dt_v);
      t += dt_v;
    }

    // last step up to time t
    cst_.do_step(cov_ode, P_, t, tau - t);
    sst_.do_step(state_ode, g_hat_, t, tau - t);
  }

  /**
   * @brief Update EKF with a measurement \f$y = h(x) + w\f$ where \f$w \sim \mathcal N(0, R)\f$.
   *
   * @param h measurement function \f$ h : \mathbb{G} \rightarrow \mathbb{Y} \f$
   * @param y measurement value \f$ y \in \mathbb{Y} \f$
   * @param R measurement covariance (size \f$ \dim \mathbb{Y} \times \dim \mathbb{Y} \f$)
   *
   * @note The function h must be differentiable using the desired method.
   *
   * @note Only the upper triangular part of \f$R\f$ is used
   */
  template<typename F, typename RDev, Manifold Y = std::invoke_result_t<F, G>>
  void update(F && h, const Y & y, const Eigen::MatrixBase<RDev> & R)
  {
    const auto [hval, H] = diff::dr<1, DiffType>(h, wrt(g_hat_));

    using Result = std::decay_t<decltype(hval)>;

    static_assert(Manifold<Result>, "h(x) is not a Manifold");

    static constexpr Eigen::Index Ny = Dof<Result>;

    static_assert(Ny > 0, "h(x) must be statically sized");

    const Eigen::Matrix<Scalar<G>, Ny, Ny> S =
      (H * P_.template selfadjointView<Eigen::Upper>() * H.transpose() + R)
        .template triangularView<Eigen::Upper>();

    // solve for Kalman gain
    const Eigen::Matrix<Scalar<G>, Dof<G>, Ny> K =
      S.template selfadjointView<Eigen::Upper>().ldlt().solve(H * P_).transpose();

    // update estimate and covariance
    g_hat_ += K * (y - hval);
    P_ = ((CovT::Identity() - K * H) * P_).template selfadjointView<Eigen::Upper>();
  }

private:
  // filter estimate and covariance
  G g_hat_ = Default<G>();
  CovT P_  = CovT::Identity();

  // steppers for numerical ODE solutions
  Stpr<G, Scalar<G>, Tangent<G>, Scalar<G>, boost::numeric::odeint::vector_space_algebra> sst_{};
  Stpr<CovT, Scalar<G>, CovT, Scalar<G>, boost::numeric::odeint::vector_space_algebra> cst_{};
};

}  // namespace smooth::feedback

#endif  // SMOOTH__FEEDBACK__EKF_HPP_


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008-2009 Gael Guennebaud <gael.guennebaud@inria.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "main.h"
#include <Eigen/Geometry>
#include <Eigen/LU>
#include <Eigen/SVD>

/* this test covers the following files:
   Geometry/OrthoMethods.h
*/

template<typename Scalar> void orthomethods_3()
{
  typedef typename NumTraits<Scalar>::Real RealScalar;
  typedef Matrix<Scalar,3,3> Matrix3;
  typedef Matrix<Scalar,3,1> Vector3;

  typedef Matrix<Scalar,4,1> Vector4;

  Vector3 v0 = Vector3::Random(),
          v1 = Vector3::Random(),
          v2 = Vector3::Random();

  // cross product
  VERIFY_IS_MUCH_SMALLER_THAN(v1.cross(v2).dot(v1), Scalar(1));
  VERIFY_IS_MUCH_SMALLER_THAN(v1.dot(v1.cross(v2)), Scalar(1));
  VERIFY_IS_MUCH_SMALLER_THAN(v1.cross(v2).dot(v2), Scalar(1));
  VERIFY_IS_MUCH_SMALLER_THAN(v2.dot(v1.cross(v2)), Scalar(1));
  Matrix3 mat3;
  mat3 << v0.normalized(),
         (v0.cross(v1)).normalized(),
         (v0.cross(v1).cross(v0)).normalized();
  VERIFY(mat3.isUnitary());

  // colwise/rowwise cross product
  mat3.setRandom();
  Vector3 vec3 = Vector3::Random();
  Matrix3 mcross;
  int i = internal::random<int>(0,2);
  mcross = mat3.colwise().cross(vec3);
  VERIFY_IS_APPROX(mcross.col(i), mat3.col(i).cross(vec3));
  mcross = mat3.rowwise().cross(vec3);
  VERIFY_IS_APPROX(mcross.row(i), mat3.row(i).cross(vec3));

  // cross3
  Vector4 v40 = Vector4::Random(),
          v41 = Vector4::Random(),
          v42 = Vector4::Random();
  v40.w() = v41.w() = v42.w() = 0;
  v42.template head<3>() = v40.template head<3>().cross(v41.template head<3>());
  VERIFY_IS_APPROX(v40.cross3(v41), v42);

  // check mixed product
  typedef Matrix<RealScalar, 3, 1> RealVector3;
  RealVector3 rv1 = RealVector3::Random();
  VERIFY_IS_APPROX(v1.cross(rv1.template cast<Scalar>()), v1.cross(rv1));
  VERIFY_IS_APPROX(rv1.template cast<Scalar>().cross(v1), rv1.cross(v1));
}

template<typename Scalar, int Size> void orthomethods(int size=Size)
{
  typedef typename NumTraits<Scalar>::Real RealScalar;
  typedef Matrix<Scalar,Size,1> VectorType;
  typedef Matrix<Scalar,3,Size> Matrix3N;
  typedef Matrix<Scalar,Size,3> MatrixN3;
  typedef Matrix<Scalar,3,1> Vector3;

  VectorType v0 = VectorType::Random(size);

  // unitOrthogonal
  VERIFY_IS_MUCH_SMALLER_THAN(v0.unitOrthogonal().dot(v0), Scalar(1));
  VERIFY_IS_APPROX(v0.unitOrthogonal().norm(), RealScalar(1));

  if (size>=3)
  {
    v0.template head<2>().setZero();
    v0.tail(size-2).setRandom();

    VERIFY_IS_MUCH_SMALLER_THAN(v0.unitOrthogonal().dot(v0), Scalar(1));
    VERIFY_IS_APPROX(v0.unitOrthogonal().norm(), RealScalar(1));
  }

  // colwise/rowwise cross product
  Vector3 vec3 = Vector3::Random();
  int i = internal::random<int>(0,size-1);

  Matrix3N mat3N(3,size), mcross3N(3,size);
  mat3N.setRandom();
  mcross3N = mat3N.colwise().cross(vec3);
  VERIFY_IS_APPROX(mcross3N.col(i), mat3N.col(i).cross(vec3));

  MatrixN3 matN3(size,3), mcrossN3(size,3);
  matN3.setRandom();
  mcrossN3 = matN3.rowwise().cross(vec3);
  VERIFY_IS_APPROX(mcrossN3.row(i), matN3.row(i).cross(vec3));
}

void test_geo_orthomethods()
{
  for(int i = 0; i < g_repeat; i++) {
    CALL_SUBTEST_1( orthomethods_3<float>() );
    CALL_SUBTEST_2( orthomethods_3<double>() );
    CALL_SUBTEST_4( orthomethods_3<std::complex<double> >() );
    CALL_SUBTEST_1( (orthomethods<float,2>()) );
    CALL_SUBTEST_2( (orthomethods<double,2>()) );
    CALL_SUBTEST_1( (orthomethods<float,3>()) );
    CALL_SUBTEST_2( (orthomethods<double,3>()) );
    CALL_SUBTEST_3( (orthomethods<float,7>()) );
    CALL_SUBTEST_4( (orthomethods<std::complex<double>,8>()) );
    CALL_SUBTEST_5( (orthomethods<float,Dynamic>(36)) );
    CALL_SUBTEST_6( (orthomethods<double,Dynamic>(35)) );
  }
}


#include<stdio.h>
#include"mex.h"

#include<ceres/ceres.h>
using namespace std;

#include <Eigen/Core>
#include <Eigen/Geometry>

struct NORMAL_COST
{
  NORMAL_COST(Eigen::Matrix<double, 1, 3> parent_normal, Eigen::Matrix<double, 1, 3> child_normal):_parent_normal(parent_normal),_child_normal(child_normal){}
  template <typename T>
  bool operator()(const T *child_to_parent_q, T *residual)const
  {
    // get the transformation from the child_to_parent_q [qw qx qy qz]
    // Quaternion [w, x, y, z]
    Eigen::Quaternion<T> child_to_parent_q_(child_to_parent_q[0], child_to_parent_q[1], child_to_parent_q[2], child_to_parent_q[3]);
    Eigen::Matrix<T, 3, 3> child_to_parent_rotationalMatrix = child_to_parent_q_.toRotationMatrix();

    // get the vector from the parent_normal and child_normal [nx ny nz]
    Eigen::Matrix<T, 3, 1> parent_normal_v(T(_parent_normal(0, 0)), T(_parent_normal(0, 1)), T(_parent_normal(0, 2)));

    Eigen::Matrix<T, 3, 1> child_normal_v(T(_child_normal(0, 0)), T(_child_normal(0, 1)), T(_child_normal(0, 2)));

    Eigen::Matrix<T, 3, 1> _residual_normal_v = child_to_parent_rotationalMatrix * child_normal_v - parent_normal_v;

    residual[0] = _residual_normal_v(0, 0);
    residual[1] = _residual_normal_v(1, 0);
    residual[2] = _residual_normal_v(2, 0);

    #ifdef DEBUG
      mexPrintf("child_normal_v: [%f, %f, %f]\n", _child_normal(0, 0), _child_normal(0, 1), _child_normal(0, 2));
      mexPrintf("parent_normal_v: [%f, %f, %f]\n", _parent_normal(0, 0), _parent_normal(0, 1), _parent_normal(0, 2));
      mexPrintf("child_to_parent_rotationalMatrix: \n[%f, %f, %f]\n[%f, %f, %f]\n[%f, %f, %f]\n", \
              child_to_parent_rotationalMatrix(0, 0), child_to_parent_rotationalMatrix(0, 1), child_to_parent_rotationalMatrix(0, 2), \
              child_to_parent_rotationalMatrix(1, 0), child_to_parent_rotationalMatrix(1, 1), child_to_parent_rotationalMatrix(1, 2), \
              child_to_parent_rotationalMatrix(2, 0), child_to_parent_rotationalMatrix(2, 1), child_to_parent_rotationalMatrix(2, 2) \
              );
      mexPrintf("residual: [%f, %f, %f]\n", residual[0], residual[1], residual[2]);
    #endif

    return true;
  }
  const Eigen::Matrix<double, 1, 3> _parent_normal, _child_normal;
};

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]){
    // nlhs represent the number of parameters of the output
    // plhs is a array of the mxarray pointers, each pointing to the output
    // nrhs represents the number of parameters of the input
    // prhs is a array of the mxarray pointers, each pointing to the input

    // prhs[0], nx3 double, parent_normal, [nx ny nz]
    // prhs[1], nx3 double, child_normal, [nx ny nz]
    // prhs[2], 1x4 double, child_to_parent_q, [qx qy qz qw]
    // prhs[3], 1x1 double, verbose

    if(nrhs < 4){
        mexErrMsgIdAndTxt( "estimatePitchRollByCoNormalCeresMex:invalidNumInputs", "at least 4 input arguments required");
        return;
    }

    /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    // get the parent_normals
    const size_t *dimArrayOfParentNormals = mxGetDimensions(prhs[0]);
    size_t sizeRowsParentNormals = *(dimArrayOfParentNormals + 0);
    size_t sizeColsParentNormals = *(dimArrayOfParentNormals + 1);

    if(sizeColsParentNormals != 3){
        mexErrMsgIdAndTxt( "estimatePitchRollByCoNormalCeresMex:invalidInputs", "the 1st param should be Nx3");
        return;
    }

    double *ptrParentNormals = (double *)(mxGetPr(prhs[0]));
    std::vector<Eigen::Matrix<double, 1, 3>>parent_normals_all;
    for(int i = 0; i < sizeRowsParentNormals; i++){
        Eigen::Matrix<double, 1, 3> current_parent_normal;
        for(int j = 0; j < 3; j++){
            current_parent_normal(0, j) = ptrParentNormals[j * sizeRowsParentNormals + i];
        }
        parent_normals_all.push_back(current_parent_normal);
    }

    /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    // get the child_normals
    const size_t *dimArrayOfChildNormals = mxGetDimensions(prhs[1]);
    size_t sizeRowsChildNormals = *(dimArrayOfChildNormals + 0);
    size_t sizeColsChildNormals = *(dimArrayOfChildNormals + 1);
    if(sizeColsChildNormals != 3){
        mexErrMsgIdAndTxt( "estimatePitchRollByCoNormalCeresMex:invalidInputs", "the 2st param should be Nx3");
        return;
    }

    double *ptrChildNormals = (double *)(mxGetPr(prhs[1]));
    std::vector<Eigen::Matrix<double, 1, 3>>child_normals_all;
    for(int i = 0; i < sizeRowsChildNormals; i++){
        Eigen::Matrix<double, 1, 3> current_child_normal;
        for(int j = 0; j < 3; j++){
            current_child_normal(0, j) = ptrChildNormals[j * sizeRowsChildNormals + i];
        }
        child_normals_all.push_back(current_child_normal);
    }

    if(sizeRowsParentNormals != sizeRowsChildNormals){
        mexErrMsgIdAndTxt( "estimatePitchRollByCoNormalCeresMex:invalidInputs", "the 1st param and the 2st param should have the same size");
        return;
    }

    #ifdef DEBUG
        mexPrintf("observation number: %d\n", sizeRowsParentNormals);
    #endif

    /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    // get the init_child_to_parent_q
    double child_to_parent_q[4] = {0};
    const size_t *dimArrayOfChildToParentQ = mxGetDimensions(prhs[2]);
    size_t sizeRowsChildToParentQ = *(dimArrayOfChildToParentQ + 0);
    size_t sizeColsChildToParentQ = *(dimArrayOfChildToParentQ + 1);
    if(sizeColsChildToParentQ != 4 && sizeRowsChildToParentQ != 1){
        mexErrMsgIdAndTxt( "estimatePitchRollByCoNormalCeresMex:invalidInputs", "the 3st param should be 1x4");
        return;
    }
    double *ptrChildToParentQ = (double *)(mxGetPr(prhs[2]));
    for(int i = 0; i < 4; i++){
        child_to_parent_q[i] = ptrChildToParentQ[i];
    }

    #ifdef DEBUG
        mexPrintf("init child_to_parent_q: [%.4f,%.4f,%.4f,%.4f]\n", \
                    child_to_parent_q[0], \
                    child_to_parent_q[1], \
                    child_to_parent_q[2], \
                    child_to_parent_q[3]);
    #endif

    /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    // get the verbose
    bool verbose = false;
    const size_t *dimArrayOfVerbose = mxGetDimensions(prhs[3]);
    size_t sizeRowsVerbose = *(dimArrayOfVerbose + 0);
    size_t sizeColsVerbose = *(dimArrayOfVerbose + 1);
    if(sizeRowsVerbose != 1 && sizeColsVerbose != 1){
        mexErrMsgIdAndTxt( "estimatePitchRollByCoNormalCeresMex:invalidInputs", "the 4st param should be 1x1");
        return;
    }
    double *ptrVerbose = (double *)(mxGetPr(prhs[3]));
    verbose = bool(ptrVerbose[0]);

    /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    ceres::Problem problem;
    for(int i=0; i<sizeRowsParentNormals; i++)
    {
        problem.AddResidualBlock(
          new ceres::AutoDiffCostFunction<NORMAL_COST, 3, 4>(
              new NORMAL_COST(parent_normals_all[i], child_normals_all[i])
          ),
          NULL,
          child_to_parent_q
        );
    }

    #ifdef DEBUG
        mexPrintf("AddResidualBlock done!\n");
    #endif

    //配置求解器并求解，输出结果
    ceres::Solver::Options options;
    options.linear_solver_type=ceres::DENSE_QR;
    options.minimizer_progress_to_stdout=verbose;
    ceres::Solver::Summary summary;
    ceres::Solve(options,&problem,&summary);

    #ifdef DEBUG
        mexPrintf("estimated child_to_parent_q: [%.4f,%.4f,%.4f,%.4f]\n", \
                    child_to_parent_q[0], \
                    child_to_parent_q[1], \
                    child_to_parent_q[2], \
                    child_to_parent_q[3]);
    #endif

    // the output q will be 1x4
    size_t dimArrayOfParams[2] = { 1, 4 };
    plhs[0] = mxCreateNumericArray(2, dimArrayOfParams, mxDOUBLE_CLASS, mxREAL);
    double *out_params = (double *)mxGetData(plhs[0]);

    for(int i = 0; i < 4; i++){
        out_params[i] = child_to_parent_q[i];
    }
}


#include "itkImage.h"
#include "itkImageFileReader.h"
#include "itkImageFileWriter.h"
#include "vnl/vnl_matrix_ref.h"
#include "vnl/vnl_matrix.h"
#include "itkNumericTraits.h"

#include "tkdCmdParser.h"

#include <iostream>
#include <utility>
#include <fstream>

#include <boost/utility.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/betweenness_centrality.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/adjacency_matrix.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>

/**
 * Minimum spanning tree.
 */
class MST
{
public:

	typedef double PixelType;

	typedef boost::property< boost::vertex_index_t, unsigned int, boost::property< boost::vertex_centrality_t, PixelType > >
			VertexPropertyType;
	typedef boost::property< boost::edge_weight_t, PixelType, boost::property< boost::edge_centrality_t, PixelType > > EdgePropertyType;
	typedef boost::adjacency_matrix< boost::undirectedS, VertexPropertyType, EdgePropertyType > GraphMatrixType;
	typedef boost::adjacency_list< boost::vecS, boost::vecS, boost::undirectedS, boost::no_property, boost::property< boost::edge_weight_t,
			PixelType > > GraphListType;

	typedef itk::Image< PixelType, 2 > ImageType;
	typedef itk::ImageFileWriter< ImageType > WriterType;
	typedef vnl_matrix_ref< PixelType > DataMatrixType;
	typedef vnl_vector< PixelType > VectorType;

public:

	/**
	 * Run.
	 */
	void Run( const std::string& inputFileName,
			PixelType lowerThreshold, PixelType upperThreshold, const std::string& outputFileName )
	{
		WriterType::Pointer writer = WriterType::New();

		writer->SetFileName( outputFileName.c_str() );
		writer->SetInput( GetMinimumSpanningTree( inputFileName, lowerThreshold, upperThreshold  ) );

		try
		{
			writer->Update();
		} catch ( itk::ExceptionObject& e )
		{
			std::cerr << "Error writing: " << outputFileName << std::endl;
			std::cerr << e.GetDescription() << std::endl;
		}

	}

protected:

	/**
	 * Return minimum spanning tree as matrix image.
	 */
	ImageType::Pointer GetMinimumSpanningTree( const std::string& filename, PixelType lowerThreshold, PixelType upperThreshold )
	{

		//Example();

		typedef itk::ImageFileReader< ImageType > ReaderType;
		ReaderType::Pointer reader = ReaderType::New();
		reader->SetFileName( filename.c_str() );
		reader->Update();

		ImageType::Pointer image = reader->GetOutput();
		reader = 0;

		PixelType* buffer = image->GetPixelContainer()->GetBufferPointer();
		ImageType::RegionType region = image->GetLargestPossibleRegion();
		ImageType::SizeType size = region.GetSize();
		int rows = size[0];
		int cols = size[1];

		DataMatrixType data( rows, cols, buffer );

		GraphMatrixType g( rows );

		for ( int i = 0; i < rows; ++i )
		{
			for ( int j = i + 1; j < cols; ++j )
			{
				if ( i == j )
				{
					continue;
				}

				PixelType weight = data( i, j );

				if ( weight <= upperThreshold && weight > 0 && weight > lowerThreshold )
				{
					// invert weight ( e.g. correlation coefficients ... )
					boost::add_edge( i, j, EdgePropertyType( 1.0 / weight ), g );
				}
			}
		}

		// [ 2 ]: Calculate minimum spanning tree ...

		typedef boost::graph_traits< GraphMatrixType >::edge_descriptor EdgeType;
		std::vector< EdgeType > spanningTree;

		// get spanning tree ...
		kruskal_minimum_spanning_tree( g, std::back_inserter( spanningTree ) );

		GraphMatrixType spanningTreeGraph();

		// [ 3 ]: Write minimum spanning tree to image ...

		ImageType::Pointer output = ImageType::New();
		output->CopyInformation( image );
		output->SetRegions( image->GetLargestPossibleRegion() );
		output->Allocate();
		output->FillBuffer( 0 );

		boost::graph_traits< GraphMatrixType >::edge_iterator eiter, eiter_end;
		for ( boost::tie( eiter, eiter_end ) = boost::edges( g ); eiter != eiter_end; ++eiter )
		{
			if ( std::find( spanningTree.begin(), spanningTree.end(), *eiter ) != spanningTree.end() )
			{
				PixelType w = boost::get( boost::edge_weight, g, *eiter );

				ImageType::IndexType index;
				index[0] = source( *eiter, g );
				index[1] = target( *eiter, g );
				output->SetPixel( index, w );

				index[1] = source( *eiter, g );
				index[0] = target( *eiter, g );
				output->SetPixel( index, w );
			}
		}
		return output;
	}
};

/**
 * Main.
 */
int main( int argc, char ** argv )
{
	tkd::CmdParser p( "minimum spanning tree", "Calculate minimum spanning tree on undirected graph" );

	std::string inputFileName;
	std::string outputFileName;

	float lowerThreshold = 0;
	float upperThreshold = 1.0;

	p.AddArgument( inputFileName, "input" ) ->AddAlias( "i" ) ->SetDescription( "Input image: 2D undirected adjacency matrix" ) ->SetRequired( true );

	p.AddArgument( upperThreshold, "threshold" ) ->AddAlias( "thu" ) ->AddAlias( "t" ) ->SetDescription(
			"Threshold; only include paths with weight in (0, threshold] (default: 1.0)" );

	p.AddArgument( lowerThreshold, "threshold-lower" ) ->AddAlias( "thl" ) ->SetDescription(
			"Lower threshold; only include paths with weight > threshold (default: 0)" );

	p.AddArgument( outputFileName, "output" ) ->AddAlias( "o" ) ->SetDescription(
			"Output image: minimum spanning tree with weights" ) ->SetRequired( true );

	if ( !p.Parse( argc, argv ) )
	{
		p.PrintUsage( std::cout );
		return -1;
	}

	MST mst;
	mst.Run( inputFileName, lowerThreshold, upperThreshold, outputFileName );

	return EXIT_SUCCESS;
}


#include "stdafx.h"

#include "problem.hpp"
#include "utility.hpp"

#include <fstream>
#include <sstream>
#include <iomanip>
#include <array>
#include <functional>
#include <vector>
#include <type_traits>
#include <boost/algorithm/string.hpp>

struct advent_2017_10 : problem
{
	advent_2017_10() noexcept : problem(2017, 10) {
	}

protected:
	std::string raw_input;

	void prepare_input(std::ifstream& fin) override {
		std::getline(fin, raw_input);
	}

	std::array<std::size_t, 256> partial_hash(std::size_t iterations, std::vector<std::size_t> input) {
		std::array<std::size_t, 256> sparse;
		std::iota(std::begin(sparse), std::end(sparse), 0);
		using cyclic = utility::cyclic_iterator<std::array<std::size_t, 256>::iterator>;
		cyclic first = cyclic(std::begin(sparse), std::end(sparse));
		std::size_t skip_size = 0;
		while(iterations--) {
			for(const std::size_t length : input) {
				std::reverse(first, first + gsl::narrow<std::ptrdiff_t>(length));
				first += gsl::narrow<std::ptrdiff_t>(length + skip_size);
				++skip_size;
			}
		}
		return sparse;
	}

	std::string part_1() override {
		std::vector<std::string> words;
		boost::split(words, raw_input, [](char c) { return c == ','; });
		std::vector<std::size_t> lengths;
		std::transform(std::begin(words), std::end(words), std::back_inserter(lengths), [](const std::string& s) {
			return std::stoull(s);
		});

		std::array<std::size_t, 256> single_round = partial_hash(1, lengths);

		const std::size_t first_hash = single_round[0] * single_round[1];
		return std::to_string(first_hash);
	}

	std::string part_2() override {
		std::vector<std::size_t> lengths;
		std::copy(std::begin(raw_input), std::end(raw_input), std::back_inserter(lengths));
		lengths.insert(lengths.end(), { 17u, 31u, 73u, 47u, 23u });

		std::array<std::size_t, 256> sparse = partial_hash(64, lengths);

		std::vector<std::size_t> dense;
		dense.reserve(16);
		for(auto it = std::begin(sparse); it != std::end(sparse); std::advance(it, 16)) {
			dense.push_back(std::accumulate(it, std::next(it, 16), 0ull, std::bit_xor<void>{}));
		}
		std::stringstream res;
		res << std::hex << std::setfill('0');
		for(std::size_t i = 0; i < 16; ++i) {
			res << std::setw(2) << dense[i];
		}
		return res.str();
	}
};

REGISTER_SOLVER(2017, 10);


#include <Eigen/Dense>
#include <iostream>

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  Matrix3f m;
m << 1, 2, 3,
     4, 5, 6,
     7, 8, 9;
std::cout << m;

  return 0;
}


/*
! Program for 2D explicit finite element analysis of incompressible Navier-Stokes
!
!
! Author: Dr. Chennakesava Kadapa
! Date  : 17-May-2018
! Place : Swansea, UK
!
!
*/

#include "headersVTK.h"
#include "headersBasic.h"
#include "headersEigen.h"
#include "elementutilitiescfd.h"
#include "SolutionData.h"
#include "BernsteinElem2DINSTria6Node.h"
#include "BernsteinElem2DINSQuad9Node.h"
#include <Eigen/SuperLUSupport>

using namespace std;

int main(int argc, char* argv[])
{
    double tstart, tend;

    int ndim=2, ndof=3, npElem=6;
    double fact, xNode[50], yNode[50];

    vector<vector<int> > ElemDofArray;

    int  nElem, nNode, nDBC, nFBC;
    int  ee, ii, jj, kk, ind, count, row, col;
    int  n1, n2, n3, n4, n5, n6, nsize;
    int  nn, dof;

    string  infileNodes, infileElems, infileDBCs, infileOutput;
    string  infileFBCs, charTemp, outFileName;

    //Set file names
    //The file names are specified as inputs from the command line
    if(argc == 0)
    {
        cerr << " Error in input data " << endl;
        cerr <<  "Number of input files is not sufficient " << endl;
        cerr <<  "You must enter names of THREE files" << endl;
        cerr <<  "a.) Node file, b.) Element file, and c.) Dirichlet BC file" << endl;
        cerr << "Aborting..." << endl;
    }
    else
    {
       infileNodes = argv[1];
       infileElems = argv[2];
       infileDBCs  = argv[3];

       //if(argc == 5)
         //infileFBCs  = argv[3];

       if(argc == 5)
         infileOutput = argv[4];
    }

    // Read nodal data files
    /////////////////////////////////////

    std::ifstream  infile_nodes(infileNodes);
    std::ifstream  infile_elems(infileElems);
    std::ifstream  infile_DBCs(infileDBCs);
    std::ifstream  infile_FBCs(infileFBCs);

    if(infile_nodes.fail())
    {
       cout << " Could not open the input nodes file " << endl;
       exit(1);
    }

    double  val[50];
    int  val2[50];

    std::string line;

    // read nodal coordinates
    ////////////////////////////////////////////

    cout << " reading nodes " << endl;

    nNode = 0;
    while (std::getline(infile_nodes, line))
      ++nNode;

    vector<vector<double> >  node_coords(nNode, vector<double>(3));

    infile_nodes.clear();
    infile_nodes.seekg(0, infile_nodes.beg);

      ii=0;
      while(infile_nodes >> val[0] >> val[1] >> val[2] )
      {
        //printf("%12.6f \t %12.6f \t %12.6f \n", val[0], val[1], val[2]);

        node_coords[ii][0] = val[1];
        node_coords[ii][1] = val[2];
        node_coords[ii][2] = 0.0;

        ii++;
      }

    // read elements
    ////////////////////////////////////////////
    cout << " reading elements " << endl;

    if(infile_elems.fail())
    {
       cout << " Could not open the input elements file " << endl;
       exit(1);
    }

    nElem = 0;
    while (std::getline(infile_elems, line))
      ++nElem;

    cout << " nElem   " << nElem << endl;

    infile_elems.clear();
    infile_elems.seekg(0, infile_elems.beg);

    vector<vector<int> >  elemNodeConn(nElem, vector<int>(npElem));

    ee=0;
    if(npElem == 6)
    {
      while(infile_elems >> val2[0] >> val2[1] >> val2[2] >> val2[3] >> val2[4] >> val2[5] >> val2[6] >> val2[7] >> val2[8] >> val2[9] )
      {
        //printf("%6d \t %6d \t %6d \t %6d \n", val2[4], val2[5], val2[6], val2[7]);

        for(ii=0; ii<npElem; ii++)
          elemNodeConn[ee][ii] = val2[4+ii]-1;

        ee++;
      }
    }
    else if(npElem == 9)
    {
      while(infile_elems >> val2[0] >> val2[1] >> val2[2] >> val2[3] >> val2[4] >> val2[5] >> val2[6] >> val2[7] >> val2[8] >> val2[9] >> val2[10] >> val2[11] >> val2[12] )
      {
        //printf("%6d \t %6d \t %6d \t %6d \n", val2[4], val2[5], val2[6], val2[7]);

        for(ii=0; ii<npElem; ii++)
          elemNodeConn[ee][ii] = val2[4+ii]-1;

        ee++;
      }
    }
    else
    {
      cerr << " Invalid npElem " << npElem << endl;
      exit(-1);
    }
    //
    // Read Dirichlet BC data
    //
    ////////////////////////////////////////////
    cout << " reading DBCs " << endl;

    if(infile_DBCs.fail())
    {
       cout << " Could not open the input elements file " << endl;
       exit(1);
    }

    vector<vector<double> >  DirichletBCs;
    vector<double>  vecDblTemp(3);

    nDBC = 0;
    while(infile_DBCs >> val[0] >> val[1] >> val[2] )
    {
      vecDblTemp[0] = val[0]-1;
      vecDblTemp[1] = val[1]-1;
      vecDblTemp[2] = val[2];

      //if( val[1] <= ndof )
      //{
        DirichletBCs.push_back(vecDblTemp);
        nDBC++;
      //}
    }

    cout << " nDBC  = " << '\t' << nDBC << endl;

    infile_nodes.close();
    infile_elems.close();
    infile_DBCs.close();

    //
    // Read Output data
    //
    ////////////////////////////////////////////

    vector<int>  OutputData;

    if( !infileOutput.empty() )
    {
      std::ifstream  infile_output(infileOutput);

      cout << " reading output data " << endl;

      if(infile_output.fail())
      {
        cout << " Could not open the input elements file " << endl;
        exit(1);
      }

      ii = 0;
      while (std::getline(infile_output, line))
        ++ii;

      infile_output.clear();
      infile_output.seekg(0, infile_output.beg);

      OutputData.resize(ii);

      ee=0;
      while(infile_output >> val[0] )
      {
        OutputData[ee] = val[0]-1;
        ee++;
      }

      infile_output.close();
    }

    cout << " Input files have been read successfully \n\n " << endl;

      vector<vector<int> >  midNodeData;

      midNodeData.resize(nNode);

      for(ii=0; ii<nNode; ii++)
      {
        midNodeData[ii].resize(3);

        midNodeData[ii][0] = 0;  midNodeData[ii][1] = 0;  midNodeData[ii][2] = 0;
      }

    if(npElem == 6)
    {
      for(ee=0; ee<nElem; ee++)
      {
        ii = elemNodeConn[ee][3];
        midNodeData[ii][0] = 1;
        midNodeData[ii][1] = elemNodeConn[ee][0];
        midNodeData[ii][2] = elemNodeConn[ee][1];

        ii = elemNodeConn[ee][4];
        midNodeData[ii][0] = 1;
        midNodeData[ii][1] = elemNodeConn[ee][1];
        midNodeData[ii][2] = elemNodeConn[ee][2];

        ii = elemNodeConn[ee][5];
        midNodeData[ii][0] = 1;
        midNodeData[ii][1] = elemNodeConn[ee][2];
        midNodeData[ii][2] = elemNodeConn[ee][0];
      }
    }
    else if(npElem == 9)
    {
      for(ee=0; ee<nElem; ee++)
      {
        ii = elemNodeConn[ee][4];
        midNodeData[ii][0] = 1;
        midNodeData[ii][1] = elemNodeConn[ee][0];
        midNodeData[ii][2] = elemNodeConn[ee][1];

        ii = elemNodeConn[ee][5];
        midNodeData[ii][0] = 1;
        midNodeData[ii][1] = elemNodeConn[ee][1];
        midNodeData[ii][2] = elemNodeConn[ee][2];

        ii = elemNodeConn[ee][6];
        midNodeData[ii][0] = 1;
        midNodeData[ii][1] = elemNodeConn[ee][2];
        midNodeData[ii][2] = elemNodeConn[ee][3];

        ii = elemNodeConn[ee][7];
        midNodeData[ii][0] = 1;
        midNodeData[ii][1] = elemNodeConn[ee][3];
        midNodeData[ii][2] = elemNodeConn[ee][0];
      }
    }

    vector<vector<int> >  ID;
    vector<vector<bool> >  NodeType;
    vector<int>  assyForSoln;

    NodeType.resize(nNode);

    ID.resize(nNode);

    for(ii=0;ii<nNode;ii++)
    {
      NodeType[ii].resize(ndof);
      ID[ii].resize(ndof);

      for(jj=0;jj<ndof;jj++)
      {
        NodeType[ii][jj] = false;
        ID[ii][jj] = -1;
      }
    }

    // fix the pressure at all the mid nodes
    for(ii=0; ii<nNode; ii++)
    {
      if(midNodeData[ii][0])
        NodeType[ii][2] = true;
    }

    if(npElem == 9)
    {
      for(ee=0; ee<nElem; ee++)
      {
        NodeType[elemNodeConn[ee][8]][2] = true;
      }
    }

    // fix the specified Dirichlet BCs
    for(ii=0; ii<nDBC; ii++)
    {
      //cout << ii << '\t' << DirichletBCs[ii][0] << '\t' << DirichletBCs[ii][1] << endl;
      NodeType[DirichletBCs[ii][0]][DirichletBCs[ii][1]] = true;
    }

    //for(ii=0; ii<totalDOF; ii++)
      //cout << ii << '\t' << assyForSoln[ii] << endl;

    int totalDOF = 0;
    for(ii=0;ii<nNode;ii++)
    {
      for(jj=0;jj<ndof;jj++)
      {
        //cout << ii << '\t' << jj << '\t' << NodeType[ii][jj] << endl;
        if(!NodeType[ii][jj])
        {
          ID[ii][jj] = totalDOF++;
          assyForSoln.push_back(ii*ndof+jj);
        }
      }
    }

      cout << " Mesh statistics .....\n" << endl;
      cout << " nElem          = " << '\t' << nElem << endl;
      cout << " nNode          = " << '\t' << nNode  << endl;
      cout << " npElem         = " << '\t' << npElem << endl;
      cout << " ndof           = " << '\t' << ndof << endl;
      cout << " Total DOF      = " << '\t' << totalDOF << endl;

      vector<vector<int> >  LM, forAssyMat;

      LM.resize(nElem);

      for(ee=0;ee<nElem;ee++)
      {
        npElem = elemNodeConn[ee].size();

        //printVector(IEN[ee]);

        ind = ndof*npElem;
        LM[ee].resize(ind);

        for(ii=0;ii<npElem;ii++)
        {
          ind = ndof*ii;

          kk = elemNodeConn[ee][ii];

          for(jj=0;jj<ndof;jj++)
          {
            LM[ee][ind+jj] = ID[kk][jj];
          }
        }
      }

      printf("\n element DOF values initialised \n\n");
      printf("\n Preparing matrix pattern \n\n");

      // compute matrix pattern needs to be prepared only for the implicit solver

      forAssyMat.clear();
      forAssyMat.resize(totalDOF);

      int *tt, r, c;

    for(ee=0;ee<nElem;ee++)
    {
      tt = &(LM[ee][0]);
      nsize = LM[ee].size();

      for(ii=0;ii<nsize;ii++)
      {
        r = tt[ii];

        if(r != -1)
        {
          for(jj=0;jj<nsize;jj++)
          {
            if(tt[jj] != -1)
            {
              //printf("ii.... %5d \t %5d \t %5d \t %5d \n",ii, jj, r, tt[jj]);
              forAssyMat[r].push_back(tt[jj]);
            }
          }
        }
      }
    }

    printf("\n Preparing matrix pattern DONE \n\n");

    VectorXi  nnzVec(totalDOF);

    int nnz = 0;
    for(ii=0;ii<totalDOF;ii++)
    {
      findUnique(forAssyMat[ii]);

      nnzVec[ii] = forAssyMat[ii].size();
      nnz += nnzVec[ii];
    }
    cout << " nnz " << nnz << endl;

    bool pp1=false;
    //pp1=true;
    if(pp1)
    {
       printf("   Number of non-zeros = %5d \n\n", nnz);
       printf("   dof to dof connectivity ...:  \n\n");
       for(ii=0;ii<totalDOF;ii++)
       {
          cout << " dof # " << ii << " : ";
          for(jj=0;jj<forAssyMat[ii].size();jj++)
            cout << '\t' << forAssyMat[ii][jj];
          cout << endl;
       }
       printf("\n\n\n");
    }

      /////////////////////////////////////////
      //
      // Eigen based solver
      //
      /////////////////////////////////////////

      cout << " Eigen based solver " << totalDOF << endl;

      //solver->rhsVec.resize(nRow);

      SparseMatrixXd  mtx(totalDOF, totalDOF);

      mtx.reserve(nnz);
      mtx.reserve(nnzVec);

      for(ii=0;ii<totalDOF;ii++)
      {
        for(jj=0;jj<forAssyMat[ii].size();jj++)
        {
          mtx.coeffRef(ii, forAssyMat[ii][jj]) = 0.0;
        }
      }

      mtx.makeCompressed();

////////////////////////////////////////////
////////////////////////////////////////////
////////////////////////////////////////////

      VectorXd  pres, presCur, presPrev, presPrev2, presPrev3;
      VectorXd  presDot, presDotPrev, presDotCur, presDiff;
      VectorXd  velo, veloCur, veloDiff, veloPrev, veloPrev2, veloPrev3;
      VectorXd  veloDot, veloDotPrev, acceCur;

      VectorXd  solnApplied, solnVTK, soln, solnCur, solnTemp, rhsVec;

      ind = nNode*ndim;

      velo.resize(ind);
      velo.setZero();

      veloPrev  = velo;
      veloCur   = velo;
      veloPrev2 = velo;
      veloPrev3 = velo;

      veloDot     = velo;
      veloDotPrev = veloDot;
      acceCur  = veloDot;

      pres.resize(nNode);
      pres.setZero();

      presPrev  = pres;
      presPrev2 = pres;
      presPrev3 = pres;
      presCur   = pres;

      presDot     = velo;
      presDotPrev = pres;
      presDotCur  = pres;

      soln.resize(nNode*ndof);
      soln.setZero();

      solnCur = soln;
      solnApplied = soln;

      solnTemp.resize(totalDOF);
      solnTemp.setZero();

      rhsVec = solnTemp;

      ////////////////////////////////////////////////
      // Computations start from here
      ////////////////////////////////////////////////

      char fname[200];
      //sprintf(fname,"(),"-explicit.dat");

      ofstream fout("convergence-data2.dat");

      if(fout.fail())
      {
        cout << " Could not open the Output file" << endl;
        exit(1);
      }

      fout.setf(ios::fixed);
      fout.setf(ios::showpoint);
      fout.precision(14);

      double  xx, yy;
//
      // loop over the nodes and adjust nodal coordinates
      for(nn=0; nn<nNode; nn++)
      {
        if( midNodeData[nn][0] )
        {
          n1 = midNodeData[nn][1];
          n2 = midNodeData[nn][2];

          xx = 0.25*node_coords[n1][0] + 0.25*node_coords[n2][0];
          yy = 0.25*node_coords[n1][1] + 0.25*node_coords[n2][1];

          node_coords[nn][0] = 2.0*(node_coords[nn][0] - xx);
          node_coords[nn][1] = 2.0*(node_coords[nn][1] - yy);
        }
      }
//

      double  elemData[50], timeData[50];
      double  norm_rhs=1000.0, norm_pres=1000.0;

      //time integration parameters
      timeData[1] = 1.0;   timeData[2] = 0.0;

      //density
      elemData[0] = 1.0;
      //viscosity
      //elemData[1] = 1.0;
      //elemData[1] = 0.025;
      elemData[1] = 1.0/229.0;
      //Body force in X-, Y- and Z- direction
      elemData[2] = 0.0;   elemData[3] = 0.0; elemData[4] = 0.0;
      //beta
      elemData[5] = 2.0;

      double  rhoInf;
      double  am = 1.0;
      double  gamm = 0.5+am;

      int  stepsMax = 4000000;
      int  stepsCompleted=0;
      int  outputFreq = 10;
      int  fileCount=1;

      double  dt = 0.01;
      double  timeFact=0.0;
      double  timeNow=0.0;
      double  timeFinal = 4000.0;
      double  num, denom;
      double  fact1, fact2;

      // create elements and prepare element data
      BernsteinElem2DINSTria6Node   **elems;
      //BernsteinElem2DINSQuad9Node   **elems;

      elems = new BernsteinElem2DINSTria6Node* [nElem];
      //elems = new BernsteinElem2DINSQuad9Node* [nElem];

      for(ee=0;ee<nElem;ee++)
      {
        elems[ee] = new BernsteinElem2DINSTria6Node;
        //elems[ee] = new BernsteinElem2DINSQuad9Node;

        elems[ee]->nodeNums = elemNodeConn[ee];

        //elems[ee]->SolnData = &(SolnData);

        elems[ee]->prepareElemData(node_coords);

        elems[ee]->forAssyVec = LM[ee];
      }

      cout << " elements are created and prepated " << endl;
      cout << " Computing the solution \n" << endl;

      //SimplicialLDLT<SparseMatrix<double> > solver;
      SuperLU<SparseMatrixXd > solver;

      ///////////////////////////////////////////////////////////////
      ///////////////////////////////////////////////////////////////
      ///////////////////////////////////////////////////////////////

      velo.setZero();      veloPrev.setZero();      veloCur.setZero();
      veloDot.setZero();      veloDotPrev.setZero();      acceCur.setZero();
      pres.setZero();      presPrev.setZero();      presCur.setZero();
      presDot.setZero();   presDotPrev.setZero();   presDotCur.setZero();

      //for(ii=0; ii<nNode_Velo; ii++)
      //{
        //veloPrev(ii*2) = 1.0;
      //}

      //velo = veloPrev;

        //SolnData.applyDirichletBCs(timeFact);

        solnApplied.setZero();

        //Add specified Dirichlet BC
        for(ii=0; ii<nDBC; ii++)
        {
          nn  = DirichletBCs[ii][0];
          dof = DirichletBCs[ii][1];

          jj = nn*ndof+dof;

          solnApplied(jj) = DirichletBCs[ii][2];
        }

        //printVector(soln);

        for(ii=0; ii<nDBC; ii++)
        {
          nn   = (int) (DirichletBCs[ii][0]);
          dof  = (int) (DirichletBCs[ii][1]);

          if( midNodeData[nn][0] )
          {
            fact = 0.25*solnApplied(midNodeData[nn][1]*ndof+dof) + 0.25*solnApplied(midNodeData[nn][2]*ndof+dof);

            solnApplied[nn*ndof+dof] = 2.0*(solnApplied(nn*ndof+dof) - fact);
          }
        }
        //printVector(solnApplied);

      //Time loop
      //while( (stepsCompleted < stepsMax ) && (timeNow < timeFinal) )

      vector<int>  vecTempInt;

      ind = npElem*ndof;

      VectorXd  Flocal(ind);
      MatrixXd  Klocal(ind, ind);

      int iter=0, loadIncr;

      soln.setZero();

      for(loadIncr=0; loadIncr<10; loadIncr++)
      {
        //timeFact = 0.5*(1-cos(PI*stepsCompleted/2000));
        timeFact = 0.1*(loadIncr+1);

        for(ii=0; ii<nDBC; ii++)
        {
          nn   = (int) (DirichletBCs[ii][0]);
          dof  = (int) (DirichletBCs[ii][1]);

          jj = nn*ndof + dof;

          soln[jj] = timeFact*solnApplied(jj);
        }

        //cout << " aaaaaaaaaaa " << endl;

      for(iter=0; iter<20; iter++)
      {
        //Loop over elements and compute the RHS

        mtx *= 0.0;
        rhsVec.setZero();

        for(ee=0; ee<nElem; ee++)
        {
            fact = timeNow - dt;

            //Compute the element force vector, including residual force
            elems[ee]->StiffnessAndResidual(node_coords, elemData, timeData, soln, Klocal, Flocal, fact);

            vecTempInt = elems[ee]->forAssyVec;
            ind = vecTempInt.size();

            //Assemble the element vector
            for(ii=0; ii<ind; ii++)
            {
              r = vecTempInt[ii];
              if( r != -1)
              {
                rhsVec(r)  += Flocal(ii);

                for(jj=0; jj<ind; jj++)
                {
                  c = vecTempInt[jj];
                  if( c != -1)
                  {
                    mtx.coeffRef(r, c)  += Klocal(ii, jj);
                  }
                }
              }
            }
        } //LoopElem

        // Add specified nodal force
        //SolnData.addNodalForces(timeFact);

        norm_rhs = rhsVec.norm();

        cout << " RHS norm = " << norm_rhs << endl;

        if(norm_rhs < 1.0e-8)
        {
          cout << " Solution converged below the specified tolerance " << endl;
          break;
        }
        else
        {
          solver.compute(mtx);

          solnTemp = solver.solve(rhsVec);

          for(ii=0; ii<totalDOF; ii++)
          {
            soln(assyForSoln[ii]) += solnTemp(ii);
          }

          //printVector(soln);
        }
      } // iteration loop

          // write the solution to the VTK file

          velo.resize(nNode*3);
          velo.setZero();
          pres.setZero();

          /*
          for(ii=0; ii<nNode; ii++)
          {
            n1 = ii*2;
            n2 = ii*3;

            velo(n1)   = soln(n2);
            velo(n1+1) = soln(n2+1);

            pres(ii)   = soln(n2+2);

            //pres(ii)   = soln(ii);
          }

          for(ii=0; ii<nNode; ii++)
          {
            if(midNodeData[ii][0])
            {
              jj = ii*2;
              n1 = midNodeData[ii][1]*2;
              n2 = midNodeData[ii][2]*2;

              velo(jj)    = 0.5*velo(jj)   + 0.25*velo(n1)   + 0.25*velo(n2);
              velo(jj+1)  = 0.5*velo(jj+1) + 0.25*velo(n1+1) + 0.25*velo(n2+1);

              pres(ii)    = 0.5*(pres(midNodeData[ii][1]) + pres(midNodeData[ii][2]) );
            }
          }
          */

          //writevtk(ndim, node_coords, elemNodeConn, midNodeData, soln, fileCount);

          fileCount = fileCount+1;

          //fact1 = 0.0;
          //fact2 = 0.0;
          //for(ii=0; ii<OutputData.size(); ii++)
          //{
            //jj = OutputData[ii]*2;

            //fact1 += rhsVec(jj);
            //fact2 += rhsVec(jj+1);
          //}

          //fout << timeNow << '\t' << stepsCompleted << '\t' << norm_rhs << '\t' << norm_pres ;
          //fout << '\t' << fact1 << '\t' << fact2 << endl;

        // store the variables
        //SolnData.timeUpdate();
        veloPrev3  = veloPrev2;
        veloPrev2  = veloPrev;
        veloPrev   = velo;

        veloDotPrev  = veloDot;

        presPrev3  = presPrev2;
        presPrev2  = presPrev;
        presPrev   = pres;
        presDotPrev  = presDot;

      } //Time loop

    //
    cout << " Computing errors \n " << endl;
    double totalError = 0.0;
    //cout << " index = " << index << endl;
    for(int index=0; index<4; index++)
    {
      totalError = 0.0;
      for(ee=0; ee<nElem; ee++)
      {
        //Compute the element force vector, including residual force
        totalError += elems[ee]->CalculateError(node_coords, elemData, timeData, soln, veloDot, pres, timeNow, index);
      }

      totalError = sqrt(totalError);

      if(index == 0)
        printf(" \n\n \t L2 Error in X-velocity = %12.6E \n\n " , totalError);
      else if(index == 1)
        printf(" \n\n \t L2 Error in Y-velocity = %12.6E \n\n " , totalError);
      else if(index == 2)
        printf(" \n\n \t L2 Error in pressure   = %12.6E \n\n " , totalError);
      else
        printf(" \n\n \t H1 Error in velocity   = %12.6E \n\n " , totalError);
    }
    //

    //tend = MPI_Wtime()
    //write(*,*) "That took ", (tend-tstart), "seconds"

    if(elems != NULL)
    {
      for(ii=0;ii<nElem;ii++)
        delete elems[ii];

      delete [] elems;
      elems = NULL;
    }

    cout << " Program is successful \n " << endl;

    return 1;
}


/**
 *  @file eigen_isnan.cpp
 *  @author Maximilian Harr <maximilian.harr@daimler.com>
 *  @date 29.06.2017
 *
 *  @brief Check if matrix has nan values
 *
 *
 *
 *          Coding Standard:
 *          wiki.ros.org/CppStyleGuide
 *          https://google.github.io/styleguide/cppguide.html
 *
 *
 *  @bug
 *
 *
 *  @todo
 *
 *
 */

// PRAGMA

// SYSTEM INCLUDES
#include <Eigen/Core>
#include <iostream>

// PROJECT INCLUDES

// LOCAL INCLUDES

// FORWARD REFERENCES

// FUNCTION PROTOTYPES

// GLOBAL VARIABLES

using namespace Eigen;
using namespace std;

//// MAIN //////////////////////////////////////////////////////////////////////////////////////////
int main(int argc, char* argv[])
{
	Eigen::Matrix3d v;
	v << 0, 1, 2, 3, 4, 5, 6, 7, 8;

	if( !isnan(v.array()).isZero(0) || !isinf(v.array()).isZero(0) ){
		std::cout << "Matrix has a nan/inf value." << std::endl;
	}
	else{
		std::cout << "Matrix has no nan/inf value." << std::endl;
	}
	v(0,0) *= 0.0/0.0;
	v(1,1) /= 0.0;
	std::cout << v << std::endl << std::endl;
	std::cout << "isinf:" << std::endl << Eigen::isinf(v.array()) << std::endl;
	std::cout << "isnan:" << std::endl << Eigen::isnan(v.array()) << std::endl;
	if( !isnan(v.array()).isZero(0) ){
		std::cout << "Matrix has a nan/inf value." << std::endl;
	}
	else{
		std::cout << "Matrix has no nan/inf value." << std::endl;
	}

}

//// FUNCTION DEFINITIONS //////////////////////////////////////////////////////////////////////////


// Copyright (C) 2014 The Regents of the University of California (Regents).
// 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 Regents or University of California 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 HOLDERS 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.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/filter_view_pairs_from_orientation.h"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <glog/logging.h>
#include <unordered_map>
#include <unordered_set>

#include "theia/math/rotation.h"
#include "theia/math/util.h"
#include "theia/util/hash.h"
#include "theia/util/map_util.h"
#include "theia/sfm/twoview_info.h"
#include "theia/sfm/types.h"
#include "theia/sfm/view_graph/view_graph.h"

namespace theia {

namespace {

bool AngularDifferenceIsAcceptable(
    const Eigen::Vector3d& orientation1,
    const Eigen::Vector3d& orientation2,
    const Eigen::Vector3d& relative_orientation,
    const double sq_max_relative_rotation_difference_radians) {
  const Eigen::Vector3d composed_relative_rotation =
      MultiplyRotations(orientation2, -orientation1);
  const Eigen::Vector3d loop_rotation =
      MultiplyRotations(-relative_orientation, composed_relative_rotation);
  const double sq_rotation_angular_difference_radians =
      loop_rotation.squaredNorm();
  return sq_rotation_angular_difference_radians <=
         sq_max_relative_rotation_difference_radians;
}

}  // namespace

void FilterViewPairsFromOrientation(
    const std::unordered_map<ViewId, Eigen::Vector3d>& orientations,
    const double max_relative_rotation_difference_degrees,
    ViewGraph* view_graph) {
  CHECK_NOTNULL(view_graph);
  CHECK_GE(max_relative_rotation_difference_degrees, 0.0);

  // Precompute the squared threshold in radians.
  const double max_relative_rotation_difference_radians =
      DegToRad(max_relative_rotation_difference_degrees);
  const double sq_max_relative_rotation_difference_radians =
      max_relative_rotation_difference_radians *
      max_relative_rotation_difference_radians;

  std::unordered_set<ViewIdPair> view_pairs_to_remove;
  const auto& view_pairs = view_graph->GetAllEdges();
  for (const auto& view_pair : view_pairs) {
    const Eigen::Vector3d* orientation1 =
        FindOrNull(orientations, view_pair.first.first);
    const Eigen::Vector3d* orientation2 =
        FindOrNull(orientations, view_pair.first.second);

    // If the view pair contains a view that does not have an orientation then
    // remove it.
    if (orientation1 == nullptr || orientation2 == nullptr) {
      LOG(WARNING)
          << "View pair (" << view_pair.first.first << ", "
          << view_pair.first.second
          << ") contains a view that does not exist! Removing the view pair.";
      view_pairs_to_remove.insert(view_pair.first);
      continue;
    }

    // Remove the view pair if the relative rotation estimate is not within the
    // tolerance.
    if (!AngularDifferenceIsAcceptable(
            *orientation1,
            *orientation2,
            view_pair.second.rotation_2,
            sq_max_relative_rotation_difference_radians)) {
      view_pairs_to_remove.insert(view_pair.first);
    }
  }

  // Remove all the "bad" relative poses.
  for (const ViewIdPair view_id_pair : view_pairs_to_remove) {
    view_graph->RemoveEdge(view_id_pair.first, view_id_pair.second);
  }
  VLOG(1) << "Removed " << view_pairs_to_remove.size()
          << " view pairs by rotation filtering.";
}

}  // namespace theia


#include "ConnectFour.hpp"
#include "Graph.hpp"

#include <set>
#include <fstream>
#include <boost/timer/timer.hpp>
#include <boost/range/adaptor/reversed.hpp>

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

#include <boost/variant.hpp>

/*
        We can solve connect as follows. First to comprehand the computational
        complexity, note that for every square on a 7x6 board we can have
        either {empty,hero,villian}. This means the number of boards is
        bounded above by 3^{6*7} < 2^66, and I observe about
        2.1^n number.
                All in all, this means that we can't simply let my computer
        solve as it won't fit in memory (I need to store the result
        somehow, because of the way boards are constructed I would
        be recomputing the same board from different paths).
                The way solve solve this computation problem, is to first
        work forwards, creating a database of boards, starting from
        an empty board db_0, and then iterativley produce
                        db_{i+1} = f(db_i),
        untill we reach the end of the game tree. One we have the set
                        DB = {db_i},
        we then create another database
                        s_i = f(db_i, s_{i+1}),
        where s_i as a database, evaulating each configuration. This
        iterative step.

        The algebra for evaluating the result database, is as follows.
        For each board b, where we have i moves, if b is a terminal
        board, we assign r(b) one of {Win,Lose,Draw} depending on the
        evaulation, and we are done.
                If b is a non-terminal, we then take the set all

 */

void Driver0(){
        //BoardInputOutput io;
        //auto board = io.ParseBoard(6,5,"      "
                                       //"      "
                                       //"      "
                                       //"      "
                                       //"      ");
        //auto root = GenerateGameTree(board.get());
        auto root = GenerateGameTree();
        NodeMarker marker;
        marker.Run(root);

        PRINT( marker.Lookup(root.GetStart() ) );
        Profiler prof;
        prof.Run(root);

}

/*
        This represents the level of the board,
        ie for
                (width,height,n)

                (7,6,0) => P =  {(0,0,0,0,0,0,0)}
                (7,6,1) => P =  {(1,0,0,0,0,0,0),
                                 (0,1,0,0,0,0,0),
                                 (0,1,1,0,0,0,0),
                                 ...
                                 }
                (7,6,2) => P =  {(1,0,0,0,0,0,0),
                                 (0,1,0,0,0,0,0),
                                 (0,1,1,0,0,0,0),
                                 ...
                                 }
 */

template<class T>
std::string tostring(T const& iter){
        std::stringstream sstr;
        sstr << "{";
        bool first = false;
        for( auto const& _ : iter){
                sstr
                        << ( first ? (first=false,"") : ", ")
                        << (int)_;
        }
        sstr << "}";
        return sstr.str();
}

struct LevelCombinationSet{
        using CombinationType = std::vector<char>;

        explicit LevelCombinationSet(char width, char height, char n)
                :width_{width}
                ,height_{height}
        {
                CombinationType iter(width, 0 );
                Recurse_( iter, n );
                #if 0
                char tmp = n;
                for(auto& _ : iter){
                        _ = std::min(height, tmp);
                        tmp -= _;
                        if( _ == 0 )
                                break;
                }
                #endif

        }
        auto begin()const{ return set_.begin(); }
        auto end()const{ return set_.end(); }
private:
        void Recurse_( std::vector<char> proto, char n ){
                if( n == 0 ){
                        set_.insert(std::move(proto));
                        return;
                }
                for(size_t i=0;i!=proto.size();++i){
                        if( ! (proto[i] < height_ ) )
                                continue;
                        auto next = proto;
                        ++next[i];
                        Recurse_(std::move(next), n-1);
                }
        }
        char width_;
        char height_;
        std::set<CombinationType> set_;
};

std::string Bits(unsigned long long val){
        std::bitset<sizeof(val)*8> aux{val};
        return aux.to_string();
}

void PermuattionTest(){

        #if 0
        for(int k=0;;++k){
                boost::timer::auto_cpu_timer at;
                std::vector<int> proto(7*6, 0);
                for(int i=0;i!=k;++i)
                        proto[proto.size()-1-i] = 1;
                for(;;){
                        if( ! std::next_permutation(proto.begin(), proto.end()))
                                break;
                }
                PRINT(k);
                std::cout << "--------------\n";
        }
        #endif
        unsigned long long upper = 1;
        for(int i=0;i!=6*7;++i)
                upper *= 2;
        for(int k=0;;++k){
                boost::timer::auto_cpu_timer at;
                unsigned long long iter = 0;
                int n = 0;
                PRINT_SEQ((k)(iter)(upper));
                for(;iter < upper;++iter){
                        if( __builtin_popcountll(iter) == k ){
                                PRINT_SEQ( (Bits(iter))(__builtin_popcountll(iter)) );
                                ++n;
                        }
                }
                PRINT_SEQ((upper)(k)(n));
        }

        #if 0
        LevelCombinationSet ps(7,6,10);
        for( auto const& _ : ps )
                PRINT( tostring(_));
                #endif
        /*
                want sequence
                        s1 .. sn
                where there are only K sn for sn = Empty.
                This imples can take sequence
                        t1 ... tm  where m = n -k
                where ti \in {Hero, Villian},
                        and take
                        q1 .. qk
                are index positions for inset the Empy ones

         */

}

struct SubGraph{
        void Push(Node* ptr){
                start_.push_back(ptr);
        }
        auto begin()const{ return start_.begin(); }
        auto end()const{ return start_.end(); }
        void Debug()const{
                PRINT( start_.size() );
                #if 0
                for( auto ptr : start_){
                        ptr->Ctx().Display();
                }
                #endif
        }
private:
        std::vector<Node*> start_;
};

struct Graph{
        void Push(SubGraph* sub){
                subs_.push_back(sub);
        }
        auto begin()const{ return subs_.begin(); }
        auto end()const{ return subs_.end(); }
        auto begin(){ return subs_.begin(); }
        auto end(){ return subs_.end(); }
        auto Back(){  return subs_.back(); }
        // Assume first one only have one entry
        auto Start(){ return *subs_.front()->begin(); }
        auto At(size_t idx)const{ return subs_[idx]; }
        auto Size()const{ return subs_.size(); }
private:
        // increasing popcount
        std::vector<SubGraph*> subs_;
};

#include <boost/pool/object_pool.hpp>

/*
        The essence of the problem is that I can't easily work backwards
        in this game. This is because I can't git the game tree
        with depth of 42 into memory. I also have to keep results in
        memory, because otherwise I'll be re-computing the result
        for a certain board for each path from it.
                What I need to do is create auxiallary sets of all the
        board at each depth. From this, I can work backwards.

*/

#if 0
struct BoardAllocator{
        using hash_t = decltype(std::declval<Board>().Hash());
        // (was_allocated, ptr)
        std::pair<bool, Board*> FindOrAllocate(Board const& brd){
                auto h = brd.Hash();
                auto iter = nodes_.find(h);
                if( iter == nodes_.end()){
                        auto tmp = pool_.construct(brd);
                        nodes_.emplace(h, tmp);
                        return std::make_pair(true, tmp);
                }
                return std::make_pair(false, iter->second);
        }
private:
        boost::object_pool<Board> pool_;
        std::map<hash_t, Board*> nodes_;
};

SubGraph* CreateSubGraph(BoardAllocator& alloc, SubGraph const& entry){
        ConnectFourLogic logic;

        SubGraph* result = new SubGraph;

        for( auto node : entry){
                auto const& ctx{node->Ctx()};

                // try to place a tile
                for( unsigned x=0;x!=ctx.BoardWidth();++x){
                        if( ! CanPlace( ctx.GetBoard(), x) ){
                                continue;
                        }
                        GameContext nextCtx{ctx};
                        nextCtx.Place(logic, x);

                        //nextCtx.Display();

                        auto ret = alloc.FindOrAllocate(nextCtx);
                        if( ret.first) {
                                result->Push(ret.second);
                        }

                        node->AddEdge( x, ret.second );
                }
        }

        return result;
}
int main(){

        Graph graph;
        SubGraph* start = new SubGraph;
        start->Push( new Node{GameContext{}} );
        //start->Push( alloc.FindOrAllocate(GameContext{}).second );
        graph.Push(start);

        std::vector< std::unique_ptr<BoardAllocator> > alloc;

        for(int i=0;;++i){
                //if( alloc.size())
                        //alloc.back().reset();
                alloc.emplace_back( std::make_unique<BoardAllocator>() );
                boost::timer::auto_cpu_timer at;
                graph.Back()->Debug();

                auto next = CreateSubGraph(*alloc.back(), *graph.Back() );

                graph.Push(next);
                //if( i == 10 )
                        //break;
                PRINT(i);

                if( alloc.size() >= 3 ){
                        alloc[alloc.size()-2].reset();
                        delete graph.At(alloc.size()-2);
                }
                PRINT( sizeof(Node) );
        }

}
#endif

template<class BoardType>
struct FileBackedBoardSet{
private:
        std::vector<BoardType> boards_;
};

template<class BoardType>
struct LevelGroup{
        using HashType = decltype(std::declval<BoardType>().Hash());
        LevelGroup():player_{Player_NotAPlayer}{}
        template<class IterType>
        LevelGroup(Player p, IterType first, IterType last)
                :player_{p}
                ,boards_{first,last}
        {
                std::sort(boards_.begin(), boards_.end());
        }
        auto GetPlayer()const{ return player_; }
        BoardType const& Find(HashType const& hash)const{
                auto iter = std::lower_bound( boards_.begin(),
                                              boards_.end(),
                                              hash );
                if( iter->Hash() == hash )
                        return *iter;
                throw std::domain_error("hash doesn't exist");
        }
        auto begin()const{ return boards_.begin(); }
        auto end()const{ return boards_.end(); }
        auto Size()const{ return boards_.size(); }
        auto const& operator[](size_t idx)const{
                return boards_.at(idx);
        }
private:
        friend class boost::serialization::access;

        template <typename Archive>
        void serialize(Archive &ar, const unsigned int version){
                ar & player_;
                ar & boards_;
        }
private:
        Player player_;
        std::vector<BoardType> boards_;
};

#if 0
template<class BoardType>
struct LevelGroupProducer{
        using LevelGroupType = LevelGroup<BoardType>;
        virtual ~LevelGroupProducer()=default;

        virtual
        std::unique_ptr<LevelGroupType> ProduceNextInSequence(LevelGroupType const& prev)const=0;
};

template<class BoardType>
struct LevelGroupProducerFactory{
        using LevelGroupProducerType = LevelGroupProducer<BoardType>;
        std::unique_ptr<LevelGroupProducerType> MakeForLevel(int level)const=0;
};
#endif

/*
        G0 -> G1 -> G2

        G1 = Generate(G0)
        G2 = Generate(G1)
        ...

        Because a single group is too large to have in memory
 */
template<class BoardType>
struct Group
{
        using LevelType = LevelGroup<BoardType>;

        struct MemoryManager{
                MemoryManager(std::string filename,
                              std::unique_ptr<LevelType> ptr):
                        filename_{std::move(filename)},
                        ptr_{std::move(ptr)}
                {
                        //PRINT(filename_);
                }
                void Close(){
                        //std::cerr << "Closing " << filename_ << "\n";
                        ptr_.reset();
                }
                bool Load(){
                        if( !! ptr_.get() )
                                return false;
                        //std::cerr << "Loading " << filename_ << "\n";
                        std::ifstream ifs(filename_);
                        if( ! ifs.is_open()){
                                std::cerr << "Unable to open " << filename_ << "\n";
                                return false;
                        }
                        boost::archive::text_iarchive ia{ifs};
                        ptr_.reset(new LevelType{});
                        ia >> *ptr_;
                        return true;
                }
                bool Save()const{
                        //std::cerr << "Saving " << filename_ << "\n";
                        if( ! ptr_.get() ){
                                std::cerr << "Nothing to save\n";
                                return false;
                        }
                        std::ofstream of(filename_);
                        if( ! of.is_open()){
                                std::cerr << "Unable to open " << filename_ << "\n";
                                return false;
                        }
                        boost::archive::text_oarchive oa{of};
                        oa << *ptr_;
                        return true;
                }
                LevelType const* operator->(){
                        // Autoload
                        Load();
                        return ptr_.get();
                }
                LevelType const& operator*(){
                        // Autoload
                        Load();
                        return *ptr_;
                }
        private:
                std::string filename_;
                mutable std::unique_ptr<LevelType> ptr_;
        };

        auto& DeclSubGroup(std::string const& filename, std::unique_ptr<LevelType> ptr){
                groups_.emplace_back(filename, std::move(ptr));
                return groups_.back();
        }
        auto Size()const{ return groups_.size(); }
        auto& operator[](size_t idx){
                return groups_.at(idx);
        }

private:
        mutable std::vector<MemoryManager> groups_;
};

void Driver2(){
        using BoardType = GenericBoard<7,6>;
        //using BoardType = GenericBoard<4,4>;
        ConnectFourLogic logic;
        BoardInputOutput io;

        std::vector<Player> p;
        std::vector< std::set< BoardType > > b;

        p.emplace_back(Player_Hero);
        b.emplace_back();
        b.back().emplace();

        Group<BoardType> group;

        int level=0;

        auto namebroker = [](int lvl){
                std::stringstream sstr;
                sstr << BoardType::Width() << "x" << BoardType::Height() << "Depth" << lvl << ".bin";
                return sstr.str();
        };

        auto& firstSub = group.DeclSubGroup(namebroker(level++), std::make_unique<LevelGroup<BoardType> >(p.back(), b.back().begin(), b.back().end() ));
        firstSub.Save();

        for(;;){
                boost::timer::auto_cpu_timer at;

                p.emplace_back( NextPlayer(p.back()));
                b.emplace_back();

                auto width = b.back().begin()->Width();
                auto height = b.back().begin()->Height();

                auto t = TileForPlayer(NextPlayer(p.back()));

                for( auto const& board : b[b.size()-2]){

                        auto e = logic.Evaluate(board);
                        if( e != Eval_NotFinished )
                                continue;

                        for( unsigned x=0;x!=width;++x){
                                auto level = Level(board, x);
                                if( level == height )
                                        continue;
                                BoardType next(board);
                                next.Set(x, level, t);

                                b.back().emplace(next);

                                //io.Display(next);
                                //std::stringstream sstr;
                                //sstr
                                        //<< io.ToString(board) << " => "
                                        //<< io.ToString(next);
                                //std::cout << sstr.str() << "\n";
                        }
                }

                if( b.back().size() == 0 ){
                        p.pop_back();
                        b.pop_back();
                        break;
                }

                auto& sub = group.DeclSubGroup(namebroker(level++), std::make_unique<LevelGroup<BoardType> >(p.back(), b.back().begin(), b.back().end() ));
                sub.Save();

                if( group.Size() > 2 ){
                        group[group.Size()-2].Close();
                }

                PRINT(level);
                PRINT(b.back().size());
                io.Display(*b.back().begin());
                PRINT(EvalToString(logic.Evaluate(*b.back().begin())));
        }

        std::vector<
                std::map<
                        decltype( std::declval<BoardType>().Hash() ),
                        int
                >
        > m(b.size());

        auto marking_lt = [](auto left, auto right){
                static std::map<int, int> order = [](){
                        int precedence[] = {
                                Marked_Win,
                                Marked_Win  | Marked_Draw,
                                Marked_Draw,
                                Marked_Win  | Marked_Draw | Marked_Lose,
                                Marked_Win  | Marked_Lose,
                                Marked_Draw | Marked_Lose,
                                Marked_Lose,
                                Marked_Zero,
                        };
                        std::map<int, int> ret;
                        int index = 0;
                        for( auto mark : precedence ){
                                ret.emplace(mark, index++);
                        }
                        return std::move(ret);
                }();
                return order[left] < order[right];
        };

        for(size_t idx=group.Size();idx!=0;){
                --idx;

                auto& ctrl = group[idx];
                ctrl.Load();
                auto const& sub = *ctrl;

                auto cp = sub.GetPlayer();

                PRINT(sub.Size());
                PRINT(cp);

                for(size_t i=0;i!=sub.Size();++i){
                        auto const& board = sub[i];
                        auto h = board.Hash();
                        auto e = logic.Evaluate(board);

                        //std::cout << "processing " << io.ToString(board) << "\n";
                        //PRINT(EvalToString(e));
                        switch(e){
                        case Eval_Hero:
                                m[idx][h] = Marked_Win;
                                break;
                        case Eval_Villian:
                                m[idx][h] = Marked_Lose;
                                break;
                        case Eval_Draw:
                                m[idx][h] = Marked_Draw;
                                break;
                        case Eval_NotFinished:{
                                int aggregate = 0;
                                for( unsigned x=0;x!=board.Width();++x){
                                        auto level = Level(board, x);
                                        if( level == board.Height() )
                                                continue;
                                        BoardType next(board);
                                        next.Set(x, level, TileForPlayer(cp));

                                        auto nh = next.Hash();

                                        if( m[idx+1][nh] == 0 ){
                                                PRINT(x);
                                                std::cout << "####### FROM #######\n";
                                                io.Display(board);
                                                std::cout << io.ToString(board) << "\n";
                                                std::cout << "####### TO   #######\n";
                                                io.Display(next);
                                                std::cout << io.ToString(next) << "\n";
                                                PRINT_SEQ((std::get<0>(nh))(std::get<1>(nh)));
                                                std::cout << "bad marking----------------\n";
                                                std::cout << "####### END  #######\n";
                                        }
                                        if( cp == Player_Hero ){
                                                auto cand = m[idx+1][nh];
                                                if( marking_lt( cand, aggregate) )
                                                        aggregate = cand;
                                        } else {
                                                aggregate |= m[idx+1][nh];
                                        }
                                }
                                m[idx][h] = aggregate;
                        }
                                break;
                        }
                        //io.Display(board);
                        //PRINT_SEQ((std::get<0>(h))(std::get<1>(h))(m[idx][h]));
                }
                std::cout << "\n\n";
                ctrl.Close();

                //if( idx == group.Size()-4)
                        //break;
        }

        PRINT( m[0].size() );
        PRINT( m[0].begin()->second );

}

int main(){
        Driver2();
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          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
//==============================================================================
#ifndef NT2_SWAR_FUNCTIONS_COMPLEX_SIMD_COMMON_SORT_HPP_INCLUDED
#define NT2_SWAR_FUNCTIONS_COMPLEX_SIMD_COMMON_SORT_HPP_INCLUDED
#include <nt2/swar/functions/sort.hpp>
#include <nt2/include/functions/simd/aligned_load.hpp>
#include <nt2/include/functions/simd/aligned_store.hpp>
#include <boost/simd/preprocessor/aligned_type.hpp>
#include <boost/simd/sdk/meta/cardinal_of.hpp>
#include <boost/simd/sdk/meta/scalar_of.hpp>
#include <nt2/sdk/complex/meta/as_complex.hpp>
#include <nt2/include/functions/real.hpp>

#include <algorithm>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( sort_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<complex_<floating_<A0> >,X>))
                            )
  {
    typedef A0 result_type;
    typedef typename meta::scalar_of<A0>::type stype;
    NT2_FUNCTOR_CALL(1)
    {
      static const size_t size = nt2::meta::cardinal_of<A0>::value;
      BOOST_SIMD_ALIGNED_TYPE(stype) tmp[size];
      aligned_store(a0, &tmp[0], 0);
      std::sort(tmp, tmp + size, cmp);
      return aligned_load<A0>(&tmp[0], 0);
    }
   private:
    static bool cmp(const stype& a0, const stype& a1)
    {
      //  when x is complex, the elements are sorted by abs(x).  complex
      //  matches are further sorted by arg(x).
      return (nt2::abs(a0) < nt2::abs(a1)) || ((nt2::abs(a0) == nt2::abs(a1))&& (arg(a0) <= arg(a1)));
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( sort_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<dry_<floating_<A0> >,X>))
                            )
  {
    typedef A0 result_type;
    typedef typename meta::scalar_of<A0>::type stype;
    NT2_FUNCTOR_CALL(1)
    {
      result_type(nt2::sort(nt2::real(a0)));
    }
  };

 } } }

#endif


/*
 * \file knight_solver.cpp
 * Implements the KnightSolver interface.
 *
 * @author Daniel Grigg
 *
 */
#include "knight_solver.h"
#include "position.h"
#include "io.h"

#include <iostream>
#include <vector>
#include <list>
#include <algorithm>
#include <boost/bind.hpp>
#include <boost/iterator/counting_iterator.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>

namespace kt {

  // Legal, relative knight movements
  Position knight_jumps[KT::KNIGHT_MOVE_DEGREE] = {
  Position(2, -1),
  Position(1, -2),
  Position(-1, -2),
  Position(-2, -1),
  Position(-2, 1),
  Position(-1, 2),
  Position(1, 2),
  Position(2, 1)
};

// Convert a node into a Position (0 <= node <= 63).
Position node_to_position(int n) {
  return Position(n / KT::BOARD_SIZE, n % KT::BOARD_SIZE);
}

// Convert a Position into a node (0 <= node <= 63).
size_t position_to_node(Position p) {
  return KT::BOARD_SIZE * p[0] + p[1];
}

// List all legal edges adjacent to vertex n.
std::vector<Edge> vertex_adjacent_edges(int n) {
  std::vector<Edge> edges;
  edges.reserve(KT::BOARD_SIZE);
  Position vertex_pos = node_to_position(n);

  // Add all possible knight moves that stay on the board.
  for (int i = 0; i < KT::BOARD_SIZE; ++i) {
    Position jump_position = vertex_pos + knight_jumps[i];
    if (jump_position.valid_position()) {
      edges.push_back(std::make_pair(n, position_to_node(jump_position)));
    }
  }
  return edges;
}

// Append a sequence to a sequence.
template <typename SequenceT>
void concat(SequenceT& result, SequenceT& src) {
  result.insert(result.end(), src.begin(), src.end());
}

KnightSolver::KnightSolver() {
  std::list<std::vector<Edge> > edges_by_vertex;

  // Generate the adjacent-edges for every vertex.
  std::transform(boost::counting_iterator<int>(0),
      boost::counting_iterator<int>(KT::NUM_BOARD_VERTICES),
      std::back_inserter(edges_by_vertex),
      vertex_adjacent_edges);

  // Flatten into a single, consecutive edge list
  std::vector<Edge> edges;
  std::for_each(edges_by_vertex.begin(), edges_by_vertex.end(),
      boost::bind(concat<std::vector<Edge> >, boost::ref(edges), _1));

  // All weights are one and yes, we could use BFS instead.  But dijkstra
  // is a little better optimised in boost.
  std::vector<int> weights(edges.size(), 1);
  _g = graph_t_ptr(new graph_t(edges.begin(), edges.end(), weights.begin(),
        KT::NUM_BOARD_VERTICES));
}

bool KnightSolver::shortest_path(const Position& start_at,
    const Position& end_at,
    std::vector<Position>& path) {
  path.clear();

  if (!start_at.valid_position() || !end_at.valid_position()) {
    return false;
  }

  using namespace boost;

  std::vector<vertex_descriptor> pred(num_vertices(*_g));
  std::vector<int> dist(num_vertices(*_g));
  vertex_descriptor src = vertex(position_to_node(start_at), *_g);
  dijkstra_shortest_paths(*_g, src,
      predecessor_map(&pred[0]).distance_map(&dist[0]));

  // Walk the predecessors and construct the backwards path.
  vertex_descriptor v(position_to_node(end_at));
  path.reserve(dist[v]);
  do {
    path.push_back(node_to_position(v));
    v = pred[v];
  } while (dist[v] > 0);

  reverse(path.begin(), path.end());
  return true;
}

bool KnightSolver::knight_travail(const std::string& input, std::string& output) {

  Position start_at, end_at;
  if (parse_input(input, start_at, end_at)) {

    std::vector<Position> route;

    if (!shortest_path(start_at, end_at, route)) {
      std::cerr << "Unable to find route between "
        << start_at.chess_notation()
        << " and " << end_at.chess_notation() << std::endl;
      return false;
    }
    output = route_to_string(route);
  } else {
    std::cerr << "Input must be a pair of valid chesssboard positions"
      << " in chess notation, eg, B2 E5" << std::endl;
    return false;

  }
  return true;
}
}


#include <dlib/optimization/max_cost_assignment.h>
#include <iostream>
#include <vector>

#include <dlib/optimization.h>

int main() {
  int n;
  std::cin >> n;

  dlib::matrix<int> matrix(n, n);

  for (int i = 0; i < n; i++) {
    for (int j = 0; j < n; j++) {
      int l;
      std::cin >> l;
      matrix(i, j) = -l;
    }
  }

  auto result = dlib::max_cost_assignment(matrix);

  for (auto i : result) {
    std::cout << i << ' ';
  }

  std::cout << '\n';

  int sum = 0;

  for (int i = 0; i < n; i++) {
    sum += matrix(i, result[i]);
  }

  std::cout << sum;

  return 0;
}