// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2014 Daniele Panozzo <daniele.panozzo@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.

#include "ViewerData.h"
#include "ViewerCore.h"

#include "../per_face_normals.h"
#include "../material_colors.h"
#include "../parula.h"
#include "../per_vertex_normals.h"
#include "igl/png/texture_from_png.h"
#include <iostream>
//#include "external/stb/igl_stb_image.h"

// OUR imports
#include <igl/edge_flaps.h>
#include <igl/parallel_for.h>
#include <igl/shortest_edge_and_midpoint.h>
#include <igl/collapse_edge.h>
#include <igl/per_face_normals.h>
#include <Eigen/Core>
#include <limits>
#include <igl/edge_collapse_is_valid.h>
#include <igl/circulation.h>

IGL_INLINE igl::opengl::ViewerData::ViewerData()
: dirty(MeshGL::DIRTY_ALL),
  show_faces(true),
  show_lines(true),
  invert_normals(false),
  show_overlay(true),
  show_overlay_depth(true),
  show_vertid(false),
  show_faceid(false),
  show_texture(false),
  point_size(30),
  line_width(0.5f),
  line_color(0,0,0,1),
  label_color(0,0,0.04,1),
  shininess(35.0f),
  id(-1),
  is_visible(1)
{
  clear();
};

// OUR functions
IGL_INLINE void igl::opengl::ViewerData::init_simplify() {
    edge_flaps(F, E, EMAP, EF, EI);

    init_quad_costs();
}

IGL_INLINE void igl::opengl::ViewerData::init_quad_costs() {
    // ASSUMPTION: the ith F_normals corresponds to the ith F face
    compute_normals();
    Eigen::MatrixXd planes = F_normals.normalized();
    planes.conservativeResize(F.rows(), 4);

    // calculating the d's of each plane
    for (int i = 0; i < F.rows(); i++) {
        Eigen::VectorXd vertex = V.row(F(i, 0));
        planes(i, 3) = -(planes(i, 0) * vertex(0) + planes(i, 1) * vertex(1) + planes(i, 2) * vertex(2));
    }

    // init the map
    for (int i = 0; i < V.rows(); i++) {
        Q_quad[i] = Eigen::MatrixXd::Zero(4, 4);
    }

    // calculating Q for each vertex
    for (int i = 0; i < F.rows(); i++) {
        Eigen::MatrixXd Kp = planes.row(i).transpose() * planes.row(i);
        Q_quad[F(i, 0)] += Kp;
        Q_quad[F(i, 1)] += Kp;
        Q_quad[F(i, 2)] += Kp;
    }

    // calculating the contractions and their costs
    Eigen::Vector4d last_row { 0.0, 0.0, 0.0, 1.0 };
    Eigen::VectorXd optimal_vertex;
    double optimal_cost;
    for (int i = 0; i < E.rows(); i++) {
        Eigen::MatrixXd Q_roof = Q_quad[E(i, 0)] + Q_quad[E(i, 1)];

        Eigen::MatrixXd Q_roof_prime = Q_roof;
        Q_roof_prime.row(3) = last_row;
        Q_roof_prime = Q_roof_prime.inverse(); // inverting
        if (Q_roof_prime(0, 0) != std::numeric_limits<float>::infinity()) { // if there's inf then the matrix is not invertible
            optimal_vertex = Q_roof_prime * last_row; // 4 x 1
            optimal_cost = optimal_vertex.transpose() * Q_roof * optimal_vertex;
            E_cost.push(std::pair<double, int>(optimal_cost, i));
            optimal_vertex.conservativeResize(3, 1);
            contractions[i] = optimal_vertex;
        }
        else { // the matrix is not invertible and we have to choose one of 3 options
            // choosing optimal???

            Eigen::VectorXd vertex1 = V.row(E(i, 0)); // v1
            double v1_cost = vertex1.transpose() * Q_roof * vertex1; // v1 cost

            Eigen::VectorXd vertex2 = V.row(E(i, 1)); // v2
            double v2_cost = vertex2.transpose() * Q_roof * vertex1; // v2 cost

            Eigen::VectorXd vertex12 = (V.row(E(i, 0)) + V.row(E(i, 1))) / 2; //]the middle of v1 and v2
            double v12_cost = vertex12.transpose() * Q_roof * vertex12; // v2 cost

            if (v1_cost < v2_cost) {
                if (v1_cost < v12_cost) {
                    E_cost.push(std::pair<double, int>(v1_cost, i));
                    contractions[i] = vertex1;
                }
                else {
                    E_cost.push(std::pair<double, int>(v12_cost, i));
                    contractions[i] = vertex12;
                }
            }
            else {
                if (v2_cost < v12_cost) {
                    E_cost.push(std::pair<double, int>(v2_cost, i));
                    contractions[i] = vertex2;
                }
                else {
                    E_cost.push(std::pair<double, int>(v12_cost, i));
                    contractions[i] = vertex12;
                }
            }
        }
    }

    /*  Eigen::Matrix2f A;
    A << 9, 6,
        12, 8;

    std::cout << A.inverse() << std::endl;*/

    /* E_cost.push(std::pair<double, int>(200, 1));
     E_cost.push(std::pair<double, int>(0.1, 2));
     E_cost.push(std::pair<double, int>(150, 2));
     std::pair<double, int> top = E_cost.top();
     std::cout << top.first << " " << top.second << std::endl;
     E_cost.pop();
     top = E_cost.top();
     std::cout << top.first << " " << top.second << std::endl;
     E_cost.pop();
     top = E_cost.top();
     std::cout << top.first << " " << top.second << std::endl;*/
}

IGL_INLINE void igl::opengl::ViewerData::simplify_mesh(int edges_to_remove) {
    // setups
    Eigen::MatrixXd V_proc = V;
    Eigen::MatrixXi F_proc = F;

    if (!init_costs_flag) {
        C.resize(E.rows(), V_proc.cols());

        Eigen::VectorXd costs(E.rows());
        igl::parallel_for(E.rows(), [&](const int e)
            {

                double cost = e;
                Eigen::RowVectorXd p(1, 3);
                shortest_edge_and_midpoint(e, V_proc, F_proc, E, EMAP, EF, EI, cost, p);
                C.row(e) = p;
                costs(e) = cost;

            }, 10000);

        for (int e = 0;e < E.rows();e++)
        {
            std::set<std::pair<double, int> >::iterator ret = Q.insert(std::pair<double, int>(costs(e), e)).first;
            Qit.push_back(ret);
        }

        init_costs_flag = true;
    }

    if (!Q.empty())
    {
        bool something_collapsed = false;
        // collapse edge
        for (int j = 0;j < edges_to_remove; j++)
        {
            if (!collapse_edge(shortest_edge_and_midpoint, V_proc, F_proc, E, EMAP, EF, EI, Q, Qit, C))
            {
                break;
            }
            something_collapsed = true;
        }

        if (something_collapsed)
        {
            clear();
            set_mesh(V_proc, F_proc);
            set_face_based(true);
            dirty = 157;
        }
    }
}

IGL_INLINE void igl::opengl::ViewerData::update_costs() {

    // calculating the contractions and their costs
    Eigen::Vector4d last_row{ 0.0, 0.0, 0.0, 1.0 };
    Eigen::VectorXd optimal_vertex;
    double optimal_cost;
    E_cost = std::priority_queue<std::pair<double, int>, std::vector<std::pair<double, int>>, std::greater<std::pair<double, int>> >();
    for (int i = 0; i < E.rows(); i++) {
        Eigen::MatrixXd Q_roof = Q_quad[E(i, 0)] + Q_quad[E(i, 1)];

        Eigen::MatrixXd Q_roof_prime = Q_roof;
        Q_roof_prime.row(3) = last_row;
        Q_roof_prime = Q_roof_prime.inverse(); // inverting
        if (Q_roof_prime(0, 0) != std::numeric_limits<float>::infinity()) { // if there's inf then the matrix is not invertible
            optimal_vertex = Q_roof_prime * last_row; // 4 x 1
            optimal_cost = optimal_vertex.transpose() * Q_roof * optimal_vertex;
            E_cost.push(std::pair<double, int>(optimal_cost, i));
            optimal_vertex.conservativeResize(3, 1);
            contractions[i] = optimal_vertex;
        }
        else { // the matrix is not invertible and we have to choose one of 3 options
            // choosing optimal???

            Eigen::VectorXd vertex1 = V.row(E(i, 0)); // v1
            double v1_cost = vertex1.transpose() * Q_roof * vertex1; // v1 cost

            Eigen::VectorXd vertex2 = V.row(E(i, 1)); // v2
            double v2_cost = vertex2.transpose() * Q_roof * vertex1; // v2 cost

            Eigen::VectorXd vertex12 = (V.row(E(i, 0)) + V.row(E(i, 1))) / 2; //]the middle of v1 and v2
            double v12_cost = vertex12.transpose() * Q_roof * vertex12; // v2 cost

            if (v1_cost < v2_cost) {
                if (v1_cost < v12_cost) {
                    E_cost.push(std::pair<double, int>(v1_cost, i));
                    contractions[i] = vertex1;
                }
                else {
                    E_cost.push(std::pair<double, int>(v12_cost, i));
                    contractions[i] = vertex12;
                }
            }
            else {
                if (v2_cost < v12_cost) {
                    E_cost.push(std::pair<double, int>(v2_cost, i));
                    contractions[i] = vertex2;
                }
                else {
                    E_cost.push(std::pair<double, int>(v12_cost, i));
                    contractions[i] = vertex12;
                }
            }
        }
    }
}

IGL_INLINE void igl::opengl::ViewerData::simplify_mesh_quad_err(int edges_to_remove) {

    for (int i = 0; i < edges_to_remove; i++) {
        if (E_cost.empty()) {
            break;
        }
        std::pair<double, int> lowest_cost = E_cost.top(); // getting the lowest cost (cost, edge)
        E_cost.pop();
        int edge_to_collapse = lowest_cost.second;
       Eigen::VectorXd new_vetrex = contractions[edge_to_collapse];

        const int eflip = E(edge_to_collapse, 0) > E(edge_to_collapse, 1);
        // source and destination
        const int s = eflip ? E(edge_to_collapse, 1) : E(edge_to_collapse, 0);
        const int d = eflip ? E(edge_to_collapse, 0) : E(edge_to_collapse, 1);

        if (!edge_collapse_is_valid(edge_to_collapse, F, E, EMAP, EF, EI))
        {
            i--; // we still want to collapse the correct number of edges
            continue;
        }

        // Important to grab neighbors of d before monkeying with edges
        const std::vector<int> nV2Fd = circulation(edge_to_collapse, !eflip, EMAP, EF, EI);

        // The following implementation strongly relies on s<d
        assert(s < d && "s should be less than d");
        // move source and destination to midpoint
        Q_quad[s] = Q_quad[s] + Q_quad[d];
        V.row(s) = new_vetrex;
        V.row(d) = new_vetrex;

        // update edge info
        // for each flap
        const int m = F.rows();
        for (int side = 0;side < 2;side++)
        {
            const int f = EF(edge_to_collapse, side);
            const int v = EI(edge_to_collapse, side);
            const int sign = (eflip == 0 ? 1 : -1) * (1 - 2 * side);
            // next edge emanating from d
            const int e1 = EMAP(f + m * ((v + sign * 1 + 3) % 3));
            // prev edge pointing to s
            const int e2 = EMAP(f + m * ((v + sign * 2 + 3) % 3));
            assert(E(e1, 0) == d || E(e1, 1) == d);
            assert(E(e2, 0) == s || E(e2, 1) == s);
            // face adjacent to f on e1, also incident on d
            const bool flip1 = EF(e1, 1) == f;
            const int f1 = flip1 ? EF(e1, 0) : EF(e1, 1);
            assert(f1 != f);
            assert(F(f1, 0) == d || F(f1, 1) == d || F(f1, 2) == d);
            // across from which vertex of f1 does e1 appear?
            const int v1 = flip1 ? EI(e1, 0) : EI(e1, 1);
            // Kill e1
            E(e1, 0) = IGL_COLLAPSE_EDGE_NULL;
            E(e1, 1) = IGL_COLLAPSE_EDGE_NULL;
            EF(e1, 0) = IGL_COLLAPSE_EDGE_NULL;
            EF(e1, 1) = IGL_COLLAPSE_EDGE_NULL;
            EI(e1, 0) = IGL_COLLAPSE_EDGE_NULL;
            EI(e1, 1) = IGL_COLLAPSE_EDGE_NULL;
            // Kill f
            F(f, 0) = IGL_COLLAPSE_EDGE_NULL;
            F(f, 1) = IGL_COLLAPSE_EDGE_NULL;
            F(f, 2) = IGL_COLLAPSE_EDGE_NULL;
            // map f1's edge on e1 to e2
            assert(EMAP(f1 + m * v1) == e1);
            EMAP(f1 + m * v1) = e2;
            // side opposite f2, the face adjacent to f on e2, also incident on s
            const int opp2 = (EF(e2, 0) == f ? 0 : 1);
            assert(EF(e2, opp2) == f);
            EF(e2, opp2) = f1;
            EI(e2, opp2) = v1;
            // remap e2 from d to s
            E(e2, 0) = E(e2, 0) == d ? s : E(e2, 0);
            E(e2, 1) = E(e2, 1) == d ? s : E(e2, 1);
        }

        // finally, reindex faces and edges incident on d. Do this last so asserts
        // make sense.
        //
        // Could actually skip first and last, since those are always the two
        // collpased faces.
        for (auto f : nV2Fd)
        {
            for (int v = 0;v < 3;v++)
            {
                if (F(f, v) == d)
                {
                    const int flip1 = (EF(EMAP(f + m * ((v + 1) % 3)), 0) == f) ? 1 : 0;
                    const int flip2 = (EF(EMAP(f + m * ((v + 2) % 3)), 0) == f) ? 0 : 1;
                    assert(
                        E(EMAP(f + m * ((v + 1) % 3)), flip1) == d ||
                        E(EMAP(f + m * ((v + 1) % 3)), flip1) == s);
                    E(EMAP(f + m * ((v + 1) % 3)), flip1) = s;
                    assert(
                        E(EMAP(f + m * ((v + 2) % 3)), flip2) == d ||
                        E(EMAP(f + m * ((v + 2) % 3)), flip2) == s);
                    E(EMAP(f + m * ((v + 2) % 3)), flip2) = s;
                    F(f, v) = s;
                    break;
                }
            }
        }
        // Finally, "remove" this edge and its information
           E(edge_to_collapse, 0) = IGL_COLLAPSE_EDGE_NULL;
           E(edge_to_collapse, 1) = IGL_COLLAPSE_EDGE_NULL;
           EF(edge_to_collapse, 0) = IGL_COLLAPSE_EDGE_NULL;
           EF(edge_to_collapse, 1) = IGL_COLLAPSE_EDGE_NULL;
           EI(edge_to_collapse, 0) = IGL_COLLAPSE_EDGE_NULL;
           EI(edge_to_collapse, 1) = IGL_COLLAPSE_EDGE_NULL;

        // setting up the mesh again after the changes
        Eigen::MatrixXd V_temp = V;
        Eigen::MatrixXi F_temp = F;

        clear();
        set_mesh(V_temp, F_temp);
        set_face_based(true);
        dirty = 157;

        if (i % 10 == 0) {
            update_costs();
        }

        std::cout << "edge " << edge_to_collapse << ", cost = " << lowest_cost.first << ", new v position (" << new_vetrex << ")" << std::endl;

    }

    update_costs();
}

IGL_INLINE void igl::opengl::ViewerData::set_face_based(bool newvalue)
{
  if (face_based != newvalue)
  {
    face_based = newvalue;
    dirty = MeshGL::DIRTY_ALL;
  }
}

// Helpers that draws the most common meshes
IGL_INLINE void igl::opengl::ViewerData::set_mesh(
    const Eigen::MatrixXd& _V, const Eigen::MatrixXi& _F)
{
  using namespace std;

  Eigen::MatrixXd V_temp;

  // If V only has two columns, pad with a column of zeros
  if (_V.cols() == 2)
  {
    V_temp = Eigen::MatrixXd::Zero(_V.rows(),3);
    V_temp.block(0,0,_V.rows(),2) = _V;
  }
  else
    V_temp = _V;

  if (V.rows() == 0 && F.rows() == 0)
  {
    V = V_temp;
    F = _F;

    compute_normals();
    uniform_colors(
      Eigen::Vector3d(GOLD_AMBIENT[0], GOLD_AMBIENT[1], GOLD_AMBIENT[2]),
      Eigen::Vector3d(GOLD_DIFFUSE[0], GOLD_DIFFUSE[1], GOLD_DIFFUSE[2]),
      Eigen::Vector3d(GOLD_SPECULAR[0], GOLD_SPECULAR[1], GOLD_SPECULAR[2]));
	image_texture("D:/UniversityAssiments/Animation/CleanAssignment1/EngineForAnimationCourse/tutorial/textures/snake1.png");
//    grid_texture();
  }
  else
  {
    if (_V.rows() == V.rows() && _F.rows() == F.rows())
    {
      V = V_temp;
      F = _F;
    }
    else
      cerr << "ERROR (set_mesh): The new mesh has a different number of vertices/faces. Please clear the mesh before plotting."<<endl;
  }
  dirty |= MeshGL::DIRTY_FACE | MeshGL::DIRTY_POSITION;

  // our addition to the function
  if (!init_ds_flag) {
      init_simplify();
      init_ds_flag = true;
  }
}

IGL_INLINE void igl::opengl::ViewerData::set_vertices(const Eigen::MatrixXd& _V)
{
  V = _V;
  assert(F.size() == 0 || F.maxCoeff() < V.rows());
  dirty |= MeshGL::DIRTY_POSITION;
}

IGL_INLINE void igl::opengl::ViewerData::set_normals(const Eigen::MatrixXd& N)
{
  using namespace std;
  if (N.rows() == V.rows())
  {
    set_face_based(false);
    V_normals = N;
  }
  else if (N.rows() == F.rows() || N.rows() == F.rows()*3)
  {
    set_face_based(true);
    F_normals = N;
  }
  else
    cerr << "ERROR (set_normals): Please provide a normal per face, per corner or per vertex."<<endl;
  dirty |= MeshGL::DIRTY_NORMAL;
}

IGL_INLINE void igl::opengl::ViewerData::set_visible(bool value, unsigned int core_id /*= 1*/)
{
  if (value)
    is_visible |= core_id;
  else
  is_visible &= ~core_id;
}

//IGL_INLINE void igl::opengl::ViewerData::copy_options(const ViewerCore &from, const ViewerCore &to)
//{
//  to.set(show_overlay      , from.is_set(show_overlay)      );
//  to.set(show_overlay_depth, from.is_set(show_overlay_depth));
//  to.set(show_texture      , from.is_set(show_texture)      );
//  to.set(show_faces        , from.is_set(show_faces)        );
//  to.set(show_lines        , from.is_set(show_lines)        );
//}

IGL_INLINE void igl::opengl::ViewerData::set_colors(const Eigen::MatrixXd &C)
{
  using namespace std;
  using namespace Eigen;
  if(C.rows()>0 && C.cols() == 1)
  {
    Eigen::MatrixXd C3;
    igl::parula(C,true,C3);
    return set_colors(C3);
  }
  // Ambient color should be darker color
  const auto ambient = [](const MatrixXd & C)->MatrixXd
  {
    MatrixXd T = 0.1*C;
    T.col(3) = C.col(3);
    return T;
  };
  // Specular color should be a less saturated and darker color: dampened
  // highlights
  const auto specular = [](const MatrixXd & C)->MatrixXd
  {
    const double grey = 0.3;
    MatrixXd T = grey+0.1*(C.array()-grey);
    T.col(3) = C.col(3);
    return T;
  };
  if (C.rows() == 1)
  {
    for (unsigned i=0;i<V_material_diffuse.rows();++i)
    {
      if (C.cols() == 3)
        V_material_diffuse.row(i) << C.row(0),1;
      else if (C.cols() == 4)
        V_material_diffuse.row(i) << C.row(0);
    }
    V_material_ambient = ambient(V_material_diffuse);
    V_material_specular = specular(V_material_diffuse);

    for (unsigned i=0;i<F_material_diffuse.rows();++i)
    {
      if (C.cols() == 3)
        F_material_diffuse.row(i) << C.row(0),1;
      else if (C.cols() == 4)
        F_material_diffuse.row(i) << C.row(0);
    }
    F_material_ambient = ambient(F_material_diffuse);
    F_material_specular = specular(F_material_diffuse);
  }
  else if (C.rows() == V.rows())
  {
    set_face_based(false);
    for (unsigned i=0;i<V_material_diffuse.rows();++i)
    {
      if (C.cols() == 3)
        V_material_diffuse.row(i) << C.row(i), 1;
      else if (C.cols() == 4)
        V_material_diffuse.row(i) << C.row(i);
    }
    V_material_ambient = ambient(V_material_diffuse);
    V_material_specular = specular(V_material_diffuse);
  }
  else if (C.rows() == F.rows())
  {
    set_face_based(true);
    for (unsigned i=0;i<F_material_diffuse.rows();++i)
    {
      if (C.cols() == 3)
        F_material_diffuse.row(i) << C.row(i), 1;
      else if (C.cols() == 4)
        F_material_diffuse.row(i) << C.row(i);
    }
    F_material_ambient = ambient(F_material_diffuse);
    F_material_specular = specular(F_material_diffuse);
  }
  else
    cerr << "ERROR (set_colors): Please provide a single color, or a color per face or per vertex."<<endl;
  dirty |= MeshGL::DIRTY_DIFFUSE;

}

IGL_INLINE void igl::opengl::ViewerData::set_uv(const Eigen::MatrixXd& UV)
{
  using namespace std;
  if (UV.rows() == V.rows())
  {
    set_face_based(false);
    V_uv = UV;
  }
  else
    cerr << "ERROR (set_UV): Please provide uv per vertex."<<endl;;
  dirty |= MeshGL::DIRTY_UV;
}

IGL_INLINE void igl::opengl::ViewerData::set_uv(const Eigen::MatrixXd& UV_V, const Eigen::MatrixXi& UV_F)
{
  set_face_based(true);
  V_uv = UV_V.block(0,0,UV_V.rows(),2);
  F_uv = UV_F;
  dirty |= MeshGL::DIRTY_UV;
}

IGL_INLINE void igl::opengl::ViewerData::set_texture(
  const Eigen::Matrix<unsigned char,Eigen::Dynamic,Eigen::Dynamic>& R,
  const Eigen::Matrix<unsigned char,Eigen::Dynamic,Eigen::Dynamic>& G,
  const Eigen::Matrix<unsigned char,Eigen::Dynamic,Eigen::Dynamic>& B)
{
  texture_R = R;
  texture_G = G;
  texture_B = B;
  texture_A = Eigen::Matrix<unsigned char,Eigen::Dynamic,Eigen::Dynamic>::Constant(R.rows(),R.cols(),255);
  dirty |= MeshGL::DIRTY_TEXTURE;
}

IGL_INLINE void igl::opengl::ViewerData::set_texture(
  const Eigen::Matrix<unsigned char,Eigen::Dynamic,Eigen::Dynamic>& R,
  const Eigen::Matrix<unsigned char,Eigen::Dynamic,Eigen::Dynamic>& G,
  const Eigen::Matrix<unsigned char,Eigen::Dynamic,Eigen::Dynamic>& B,
  const Eigen::Matrix<unsigned char,Eigen::Dynamic,Eigen::Dynamic>& A)
{
  texture_R = R;
  texture_G = G;
  texture_B = B;
  texture_A = A;
  dirty |= MeshGL::DIRTY_TEXTURE;
}

IGL_INLINE void igl::opengl::ViewerData::set_points(
  const Eigen::MatrixXd& P,
  const Eigen::MatrixXd& C)
{
  // clear existing points
  points.resize(0,0);
  add_points(P,C);
}

IGL_INLINE void igl::opengl::ViewerData::add_points(const Eigen::MatrixXd& P,  const Eigen::MatrixXd& C)
{
  Eigen::MatrixXd P_temp;

  // If P only has two columns, pad with a column of zeros
  if (P.cols() == 2)
  {
    P_temp = Eigen::MatrixXd::Zero(P.rows(),3);
    P_temp.block(0,0,P.rows(),2) = P;
  }
  else
    P_temp = P;

  int lastid = points.rows();
  points.conservativeResize(points.rows() + P_temp.rows(),6);
  for (unsigned i=0; i<P_temp.rows(); ++i)
    points.row(lastid+i) << P_temp.row(i), i<C.rows() ? C.row(i) : C.row(C.rows()-1);

  dirty |= MeshGL::DIRTY_OVERLAY_POINTS;
}

IGL_INLINE void igl::opengl::ViewerData::set_edges(
  const Eigen::MatrixXd& P,
  const Eigen::MatrixXi& E,
  const Eigen::MatrixXd& C)
{
  using namespace Eigen;
  lines.resize(E.rows(),9);
  assert(C.cols() == 3);
  for(int e = 0;e<E.rows();e++)
  {
    RowVector3d color;
    if(C.size() == 3)
    {
      color<<C;
    }else if(C.rows() == E.rows())
    {
      color<<C.row(e);
    }
    lines.row(e)<< P.row(E(e,0)), P.row(E(e,1)), color;
  }
  dirty |= MeshGL::DIRTY_OVERLAY_LINES;
}

IGL_INLINE void igl::opengl::ViewerData::add_edges(const Eigen::MatrixXd& P1, const Eigen::MatrixXd& P2, const Eigen::MatrixXd& C)
{
  Eigen::MatrixXd P1_temp,P2_temp;

  // If P1 only has two columns, pad with a column of zeros
  if (P1.cols() == 2)
  {
    P1_temp = Eigen::MatrixXd::Zero(P1.rows(),3);
    P1_temp.block(0,0,P1.rows(),2) = P1;
    P2_temp = Eigen::MatrixXd::Zero(P2.rows(),3);
    P2_temp.block(0,0,P2.rows(),2) = P2;
  }
  else
  {
    P1_temp = P1;
    P2_temp = P2;
  }

  int lastid = lines.rows();
  lines.conservativeResize(lines.rows() + P1_temp.rows(),9);
  for (unsigned i=0; i<P1_temp.rows(); ++i)
    lines.row(lastid+i) << P1_temp.row(i), P2_temp.row(i), i<C.rows() ? C.row(i) : C.row(C.rows()-1);

  dirty |= MeshGL::DIRTY_OVERLAY_LINES;
}

IGL_INLINE void igl::opengl::ViewerData::add_label(const Eigen::VectorXd& P,  const std::string& str)
{
  Eigen::RowVectorXd P_temp;

  // If P only has two columns, pad with a column of zeros
  if (P.size() == 2)
  {
    P_temp = Eigen::RowVectorXd::Zero(3);
    P_temp << P.transpose(), 0;
  }
  else
    P_temp = P;

  int lastid = labels_positions.rows();
  labels_positions.conservativeResize(lastid+1, 3);
  labels_positions.row(lastid) = P_temp;
  labels_strings.push_back(str);
}

IGL_INLINE void igl::opengl::ViewerData::clear_labels()
{
  labels_positions.resize(0,3);
  labels_strings.clear();
}

IGL_INLINE void igl::opengl::ViewerData::clear()
{
  V                       = Eigen::MatrixXd (0,3);
  F                       = Eigen::MatrixXi (0,3);

  F_material_ambient      = Eigen::MatrixXd (0,4);
  F_material_diffuse      = Eigen::MatrixXd (0,4);
  F_material_specular     = Eigen::MatrixXd (0,4);

  V_material_ambient      = Eigen::MatrixXd (0,4);
  V_material_diffuse      = Eigen::MatrixXd (0,4);
  V_material_specular     = Eigen::MatrixXd (0,4);

  F_normals               = Eigen::MatrixXd (0,3);
  V_normals               = Eigen::MatrixXd (0,3);

  V_uv                    = Eigen::MatrixXd (0,2);
  F_uv                    = Eigen::MatrixXi (0,3);

  lines                   = Eigen::MatrixXd (0,9);
  points                  = Eigen::MatrixXd (0,6);
  labels_positions        = Eigen::MatrixXd (0,3);
  labels_strings.clear();

  face_based = false;
}

IGL_INLINE void igl::opengl::ViewerData::compute_normals()
{
  igl::per_face_normals(V, F, F_normals);
  igl::per_vertex_normals(V, F, F_normals, V_normals);
  dirty |= MeshGL::DIRTY_NORMAL;
}

IGL_INLINE void igl::opengl::ViewerData::uniform_colors(
  const Eigen::Vector3d& ambient,
  const Eigen::Vector3d& diffuse,
  const Eigen::Vector3d& specular)
{
  Eigen::Vector4d ambient4;
  Eigen::Vector4d diffuse4;
  Eigen::Vector4d specular4;

  ambient4 << ambient, 1;
  diffuse4 << diffuse, 1;
  specular4 << specular, 1;

  uniform_colors(ambient4,diffuse4,specular4);
}

IGL_INLINE void igl::opengl::ViewerData::uniform_colors(
  const Eigen::Vector4d& ambient,
  const Eigen::Vector4d& diffuse,
  const Eigen::Vector4d& specular)
{
  V_material_ambient.resize(V.rows(),4);
  V_material_diffuse.resize(V.rows(),4);
  V_material_specular.resize(V.rows(),4);

  for (unsigned i=0; i<V.rows();++i)
  {
    V_material_ambient.row(i) = ambient;
    V_material_diffuse.row(i) = diffuse;
    V_material_specular.row(i) = specular;
  }

  F_material_ambient.resize(F.rows(),4);
  F_material_diffuse.resize(F.rows(),4);
  F_material_specular.resize(F.rows(),4);

  for (unsigned i=0; i<F.rows();++i)
  {
    F_material_ambient.row(i) = ambient;
    F_material_diffuse.row(i) = diffuse;
    F_material_specular.row(i) = specular;
  }
  dirty |= MeshGL::DIRTY_SPECULAR | MeshGL::DIRTY_DIFFUSE | MeshGL::DIRTY_AMBIENT;
}

IGL_INLINE void igl::opengl::ViewerData::image_texture(const std::string fileName)
{
	//unsigned int texId;
	//if (igl::png::texture_from_png(fileName, false, texId))
	if(igl::png::texture_from_png(fileName,texture_R, texture_G, texture_B, texture_A))

		dirty |= MeshGL::DIRTY_TEXTURE;
	else
		std::cout<<"can't open texture file"<<std::endl;

}

IGL_INLINE void igl::opengl::ViewerData::grid_texture()
{
  // Don't do anything for an empty mesh
  if(V.rows() == 0)
  {
    V_uv.resize(V.rows(),2);
    return;
  }
  if (V_uv.rows() == 0)
  {
    V_uv = V.block(0, 0, V.rows(), 2);
    V_uv.col(0) = V_uv.col(0).array() - V_uv.col(0).minCoeff();
    V_uv.col(0) = V_uv.col(0).array() / V_uv.col(0).maxCoeff();
    V_uv.col(1) = V_uv.col(1).array() - V_uv.col(1).minCoeff();
    V_uv.col(1) = V_uv.col(1).array() / V_uv.col(1).maxCoeff();
    V_uv = V_uv.array() * 10;
    dirty |= MeshGL::DIRTY_TEXTURE;
  }

  unsigned size = 4;
  unsigned size2 = size/2;
  texture_R.resize(size, size);
  for (unsigned i=0; i<size; ++i)
  {
    for (unsigned j=0; j<size; ++j)
    {
      texture_R(i,j) = 0;
      if ((i<size2 && j<size2) || (i>=size2 && j>=size2))
        texture_R(i,j) = 255;
    }
  }

  texture_G = texture_R;
  texture_B = texture_R;
  texture_A = Eigen::Matrix<unsigned char,Eigen::Dynamic,Eigen::Dynamic>::Constant(texture_R.rows(),texture_R.cols(),255);
  dirty |= MeshGL::DIRTY_TEXTURE;
}

IGL_INLINE void igl::opengl::ViewerData::updateGL(
  const igl::opengl::ViewerData& data,
  const bool invert_normals,
  igl::opengl::MeshGL& meshgl
  )
{
  if (!meshgl.is_initialized)
  {
    meshgl.init();
  }

  bool per_corner_uv = (data.F_uv.rows() == data.F.rows());
  bool per_corner_normals = (data.F_normals.rows() == 3 * data.F.rows());

  meshgl.dirty |= data.dirty;

  // Input:
  //   X  #F by dim quantity
  // Output:
  //   X_vbo  #F*3 by dim scattering per corner
  const auto per_face = [&data](
      const Eigen::MatrixXd & X,
      MeshGL::RowMatrixXf & X_vbo)
  {
    assert(X.cols() == 4);
    X_vbo.resize(data.F.rows()*3,4);
    for (unsigned i=0; i<data.F.rows();++i)
      for (unsigned j=0;j<3;++j)
        X_vbo.row(i*3+j) = X.row(i).cast<float>();
  };

  // Input:
  //   X  #V by dim quantity
  // Output:
  //   X_vbo  #F*3 by dim scattering per corner
  const auto per_corner = [&data](
      const Eigen::MatrixXd & X,
      MeshGL::RowMatrixXf & X_vbo)
  {
    X_vbo.resize(data.F.rows()*3,X.cols());
    for (unsigned i=0; i<data.F.rows();++i)
      for (unsigned j=0;j<3;++j)
        X_vbo.row(i*3+j) = X.row(data.F(i,j)).cast<float>();
  };

  if (!data.face_based)
  {
    if (!(per_corner_uv || per_corner_normals))
    {
      // Vertex positions
      if (meshgl.dirty & MeshGL::DIRTY_POSITION)
        meshgl.V_vbo = data.V.cast<float>();

      // Vertex normals
      if (meshgl.dirty & MeshGL::DIRTY_NORMAL)
      {
        meshgl.V_normals_vbo = data.V_normals.cast<float>();
        if (invert_normals)
          meshgl.V_normals_vbo = -meshgl.V_normals_vbo;
      }

      // Per-vertex material settings
      if (meshgl.dirty & MeshGL::DIRTY_AMBIENT)
        meshgl.V_ambient_vbo = data.V_material_ambient.cast<float>();
      if (meshgl.dirty & MeshGL::DIRTY_DIFFUSE)
        meshgl.V_diffuse_vbo = data.V_material_diffuse.cast<float>();
      if (meshgl.dirty & MeshGL::DIRTY_SPECULAR)
        meshgl.V_specular_vbo = data.V_material_specular.cast<float>();

      // Face indices
      if (meshgl.dirty & MeshGL::DIRTY_FACE)
        meshgl.F_vbo = data.F.cast<unsigned>();

      // Texture coordinates
      if (meshgl.dirty & MeshGL::DIRTY_UV)
      {
        meshgl.V_uv_vbo = data.V_uv.cast<float>();
      }
    }
    else
    {

      // Per vertex properties with per corner UVs
      if (meshgl.dirty & MeshGL::DIRTY_POSITION)
      {
        per_corner(data.V,meshgl.V_vbo);
      }

      if (meshgl.dirty & MeshGL::DIRTY_AMBIENT)
      {
        meshgl.V_ambient_vbo.resize(data.F.rows()*3,4);
        for (unsigned i=0; i<data.F.rows();++i)
          for (unsigned j=0;j<3;++j)
            meshgl.V_ambient_vbo.row(i*3+j) = data.V_material_ambient.row(data.F(i,j)).cast<float>();
      }
      if (meshgl.dirty & MeshGL::DIRTY_DIFFUSE)
      {
        meshgl.V_diffuse_vbo.resize(data.F.rows()*3,4);
        for (unsigned i=0; i<data.F.rows();++i)
          for (unsigned j=0;j<3;++j)
            meshgl.V_diffuse_vbo.row(i*3+j) = data.V_material_diffuse.row(data.F(i,j)).cast<float>();
      }
      if (meshgl.dirty & MeshGL::DIRTY_SPECULAR)
      {
        meshgl.V_specular_vbo.resize(data.F.rows()*3,4);
        for (unsigned i=0; i<data.F.rows();++i)
          for (unsigned j=0;j<3;++j)
            meshgl.V_specular_vbo.row(i*3+j) = data.V_material_specular.row(data.F(i,j)).cast<float>();
      }

      if (meshgl.dirty & MeshGL::DIRTY_NORMAL)
      {
        meshgl.V_normals_vbo.resize(data.F.rows()*3,3);
        for (unsigned i=0; i<data.F.rows();++i)
          for (unsigned j=0;j<3;++j)
            meshgl.V_normals_vbo.row(i*3+j) =
                         per_corner_normals ?
               data.F_normals.row(i*3+j).cast<float>() :
               data.V_normals.row(data.F(i,j)).cast<float>();

        if (invert_normals)
          meshgl.V_normals_vbo = -meshgl.V_normals_vbo;
      }

      if (meshgl.dirty & MeshGL::DIRTY_FACE)
      {
        meshgl.F_vbo.resize(data.F.rows(),3);
        for (unsigned i=0; i<data.F.rows();++i)
          meshgl.F_vbo.row(i) << i*3+0, i*3+1, i*3+2;
      }

      if (meshgl.dirty & MeshGL::DIRTY_UV)
      {
        meshgl.V_uv_vbo.resize(data.F.rows()*3,2);
        for (unsigned i=0; i<data.F.rows();++i)
          for (unsigned j=0;j<3;++j)
            meshgl.V_uv_vbo.row(i*3+j) =
              data.V_uv.row(per_corner_uv ?
                data.F_uv(i,j) : data.F(i,j)).cast<float>();
      }
    }
  }
  else
  {
    if (meshgl.dirty & MeshGL::DIRTY_POSITION)
    {
      per_corner(data.V,meshgl.V_vbo);
    }
    if (meshgl.dirty & MeshGL::DIRTY_AMBIENT)
    {
      per_face(data.F_material_ambient,meshgl.V_ambient_vbo);
    }
    if (meshgl.dirty & MeshGL::DIRTY_DIFFUSE)
    {
      per_face(data.F_material_diffuse,meshgl.V_diffuse_vbo);
    }
    if (meshgl.dirty & MeshGL::DIRTY_SPECULAR)
    {
      per_face(data.F_material_specular,meshgl.V_specular_vbo);
    }

    if (meshgl.dirty & MeshGL::DIRTY_NORMAL)
    {
      meshgl.V_normals_vbo.resize(data.F.rows()*3,3);
      for (unsigned i=0; i<data.F.rows();++i)
        for (unsigned j=0;j<3;++j)
          meshgl.V_normals_vbo.row(i*3+j) =
             per_corner_normals ?
               data.F_normals.row(i*3+j).cast<float>() :
               data.F_normals.row(i).cast<float>();

      if (invert_normals)
        meshgl.V_normals_vbo = -meshgl.V_normals_vbo;
    }

    if (meshgl.dirty & MeshGL::DIRTY_FACE)
    {
      meshgl.F_vbo.resize(data.F.rows(),3);
      for (unsigned i=0; i<data.F.rows();++i)
        meshgl.F_vbo.row(i) << i*3+0, i*3+1, i*3+2;
    }

    if (meshgl.dirty & MeshGL::DIRTY_UV)
    {
        meshgl.V_uv_vbo.resize(data.F.rows()*3,2);
        for (unsigned i=0; i<data.F.rows();++i)
          for (unsigned j=0;j<3;++j)
            meshgl.V_uv_vbo.row(i*3+j) = data.V_uv.row(per_corner_uv ? data.F_uv(i,j) : data.F(i,j)).cast<float>();
    }
  }

  if (meshgl.dirty & MeshGL::DIRTY_TEXTURE)
  {
    meshgl.tex_u = data.texture_R.rows();
    meshgl.tex_v = data.texture_R.cols();
    meshgl.tex.resize(data.texture_R.size()*4);
    for (unsigned i=0;i<data.texture_R.size();++i)
    {
      meshgl.tex(i*4+0) = data.texture_R(i);
      meshgl.tex(i*4+1) = data.texture_G(i);
      meshgl.tex(i*4+2) = data.texture_B(i);
      meshgl.tex(i*4+3) = data.texture_A(i);
    }
  }

  if (meshgl.dirty & MeshGL::DIRTY_OVERLAY_LINES)
  {
    meshgl.lines_V_vbo.resize(data.lines.rows()*2,3);
    meshgl.lines_V_colors_vbo.resize(data.lines.rows()*2,3);
    meshgl.lines_F_vbo.resize(data.lines.rows()*2,1);
    for (unsigned i=0; i<data.lines.rows();++i)
    {
      meshgl.lines_V_vbo.row(2*i+0) = data.lines.block<1, 3>(i, 0).cast<float>();
      meshgl.lines_V_vbo.row(2*i+1) = data.lines.block<1, 3>(i, 3).cast<float>();
      meshgl.lines_V_colors_vbo.row(2*i+0) = data.lines.block<1, 3>(i, 6).cast<float>();
      meshgl.lines_V_colors_vbo.row(2*i+1) = data.lines.block<1, 3>(i, 6).cast<float>();
      meshgl.lines_F_vbo(2*i+0) = 2*i+0;
      meshgl.lines_F_vbo(2*i+1) = 2*i+1;
    }
  }

  if (meshgl.dirty & MeshGL::DIRTY_OVERLAY_POINTS)
  {
    meshgl.points_V_vbo.resize(data.points.rows(),3);
    meshgl.points_V_colors_vbo.resize(data.points.rows(),3);
    meshgl.points_F_vbo.resize(data.points.rows(),1);
    for (unsigned i=0; i<data.points.rows();++i)
    {
      meshgl.points_V_vbo.row(i) = data.points.block<1, 3>(i, 0).cast<float>();
      meshgl.points_V_colors_vbo.row(i) = data.points.block<1, 3>(i, 3).cast<float>();
      meshgl.points_F_vbo(i) = i;
    }
  }
}


#pragma once

#include <Eigen/Dense>

namespace polyfem
{
	///
	/// Generate a canonical triangle/quad subdivided from a regular grid
	///
	/// @param[in]  n  			 { n grid quads }
	/// @param[in]  tri			 { is a tri or a quad }
	/// @param[out] V            { #V x 2 output vertices positions }
	/// @param[out] F            { #F x 3 output triangle indices }
	///
	void regular_2d_grid(const int n, bool tri, Eigen::MatrixXd &V, Eigen::MatrixXi &F);

	///
	/// Generate a canonical tet/hex subdivided from a regular grid
	///
	/// @param[in]  n  			 { n grid quads }
	/// @param[in]  tet			 { is a tet or a hex }
	/// @param[out] V            { #V x 3 output vertices positions }
	/// @param[out] F            { #F x 3 output triangle indices }
	/// @param[out] T            { #F x 4 output tet indices }
	///
	void regular_3d_grid(const int nn, bool tet, Eigen::MatrixXd &V, Eigen::MatrixXi &F, Eigen::MatrixXi &T);

	class RefElementSampler
	{
	public:
		RefElementSampler() {}
		void init(const bool is_volume, const int n_elements, const double target_rel_area);

		const Eigen::MatrixXd &cube_corners() const { return cube_corners_; }
		const Eigen::MatrixXd &cube_points() const { return cube_points_; }
		const Eigen::MatrixXi &cube_faces() const { return cube_faces_; }
		const Eigen::MatrixXi &cube_volume() const { return is_volume_ ? cube_tets_ : cube_faces_; }
		const Eigen::MatrixXi &cube_edges() const { return cube_edges_; }

		const Eigen::MatrixXd &simplex_corners() const { return simplex_corners_; }
		const Eigen::MatrixXd &simplex_points() const { return simplex_points_; }
		const Eigen::MatrixXi &simplex_faces() const { return simplex_faces_; }
		const Eigen::MatrixXi &simplex_volume() const { return is_volume_ ? simplex_tets_ : simplex_faces_; }
		const Eigen::MatrixXi &simplex_edges() const { return simplex_edges_; }

		void sample_polygon(const Eigen::MatrixXd &poly, Eigen::MatrixXd &pts, Eigen::MatrixXi &faces) const;
		void sample_polyhedron(const Eigen::MatrixXd &vertices, const Eigen::MatrixXi &f, Eigen::MatrixXd &pts, Eigen::MatrixXi &faces) const;

		inline int num_samples() const
		{
			return is_volume_ ? std::max(2., round(1. / pow(area_param_, 1. / 3.) + 1)) : std::max(2., round(1. / sqrt(area_param_) + 1));
		}

	private:
		void build();

		Eigen::MatrixXi cube_tets_;
		Eigen::MatrixXi simplex_tets_;

		Eigen::MatrixXd cube_corners_;
		Eigen::MatrixXd cube_points_;
		Eigen::MatrixXi cube_faces_;
		Eigen::MatrixXi cube_edges_;

		Eigen::MatrixXd simplex_corners_;
		Eigen::MatrixXd simplex_points_;
		Eigen::MatrixXi simplex_faces_;
		Eigen::MatrixXi simplex_edges_;

		double area_param_;
		double is_volume_;
	};

} // namespace polyfem


/* Boost libs/numeric/odeint/examples/integrate_times.cpp

 Copyright 2009-2014 Karsten Ahnert
 Copyright 2009-2014 Mario Mulansky

 example for the use of integrate_times

 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)
 */

#include <iostream>
#include <boost/numeric/odeint.hpp>

using namespace std;
using namespace boost::numeric::odeint;

/*
 * simple 1D ODE
 */

void rhs( const double x , double &dxdt , const double t )
{
    dxdt = 3.0/(2.0*t*t) + x/(2.0*t);
}

void write_cout( const double &x , const double t )
{
    cout << t << '\t' << x << endl;
}

// state_type = double
typedef runge_kutta_dopri5< double > stepper_type;

const double dt = 0.1;

int main()
{
    // create a vector with observation time points
    std::vector<double> times( 91 );
    for( size_t i=0 ; i<times.size() ; ++i )
        times[i] = 1.0 + dt*i;

    double x = 0.0; //initial value x(1) = 0
    // we can provide the observation time as a boost range (i.e. the vector)
    integrate_times( make_controlled( 1E-12 , 1E-12 , stepper_type() ) , rhs ,
                     x , times , dt , write_cout );
    // or as two iterators
    //integrate_times( make_controlled( 1E-12 , 1E-12 , stepper_type() ) , rhs ,
    //                   x , times.begin() , times.end() , dt , write_cout );
}


/**
 * Created by Beck on 18/6/2018
 * Indirect Extended Kalman Filter for the pose
 * to get an estimate of the pose of the shield
 * fusing visual information with the gyroscope
 * With IMU fused reading comes in 100Hz and camera translation in 30Hz
 * V1: works for rotation only
 */
#include <iostream>
#include <ros/ros.h>
#include <ros/console.h>
#include <std_msgs/String.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/Vector3Stamped.h>
// #include <Eigen/Eigen>
#include <Eigen/Geometry>
#include <queue>
//#include "rm_cv/ArmorRecord.h"

using namespace std;
using namespace Eigen;
ros::Publisher pose_pub, debug_pub, debug_gyro_pub;
string omg_topic, visual_topic, publisher_topic;
double gyro_weight;
double visual_q_weight, visual_t_weight;
int sleep_time;

/**
 * Define states:
 *      x = [rotation_quaternion]
 * Define inputs:
 *      u = [omg], filtered angular velocity
 * Define noises:
 *      n = [n_gyro]
 */
VectorXd x(4);                        // state
// MatrixXd P = MatrixXd::Zero(3, 3);     // covariance
MatrixXd P = MatrixXd::Identity(3, 3); // covariance
MatrixXd R = MatrixXd::Identity(3, 3); // prediction noise covariance
MatrixXd Q = MatrixXd::Identity(3, 3); // observation noise covariance

// buffers to save gyro and visual reading
queue<geometry_msgs::Vector3Stamped::ConstPtr> gyro_buf;
queue<geometry_msgs::TwistStamped::ConstPtr> visual_buf;
queue<Matrix<double, 4, 1>> x_history;
queue<Matrix<double, 3, 3>> P_history;
Vector3d G_body = {0, 0, 9.8}; // Consider to add initialization later

// previous propagated time
double t_prev;

// Initialization
const int GYRO_INIT_COUNT = 10;
const int MAX_GYRO_QUEUE_SIZE = 500;
int gyro_count  = 0;
bool gyro_initialized = false;
bool visual_initialized = false;
bool visual_valid = false;
MatrixXd imu_R_camera = MatrixXd::Identity(3, 3); // rotation matrix from camera to imu
Vector3d imu_T_camera = MatrixXd::Zero(3, 1);

void pub_fused_pose(std_msgs::Header header)
{
    geometry_msgs::PoseWithCovarianceStamped pose;
    pose.header = header;
    pose.pose.pose.orientation.w = x(0);
    pose.pose.pose.orientation.x = x(1);
    pose.pose.pose.orientation.y = x(2);
    pose.pose.pose.orientation.z = x(3);
    pose.pose.covariance[0] = P(0, 0);
    pose.pose.covariance[1] = P(1, 1);
    pose.pose.covariance[2] = P(2, 2);

    pose_pub.publish(pose);
}

void pub_debug_pose(std_msgs::Header header, const Quaterniond& q)
{
    geometry_msgs::PoseStamped pose;
    pose.header = header;
    pose.pose.orientation.w = q.w();
    pose.pose.orientation.x = q.x();
    pose.pose.orientation.y = q.y();
    pose.pose.orientation.z = q.z();
    debug_pub.publish(pose);
}

void pub_fused_angleAxis(std_msgs::Header header)
{
    geometry_msgs::PoseWithCovarianceStamped angleAxis;
    angleAxis.header = header;

    Quaterniond pose_state(x(0), x(1), x(2), x(3));
    AngleAxisd angle_state(pose_state);

    angleAxis.pose.pose.orientation.w = angle_state.angle();
    angleAxis.pose.pose.orientation.x = angle_state.axis()[0];
    angleAxis.pose.pose.orientation.y = angle_state.axis()[1];
    angleAxis.pose.pose.orientation.z = angle_state.axis()[2];
    angleAxis.pose.covariance[0] = P(0, 0);
    angleAxis.pose.covariance[1] = P(1, 1);
    angleAxis.pose.covariance[2] = P(2, 2);
    pose_pub.publish(angleAxis);
}

void pub_debug_angleAxis(std_msgs::Header header, const Eigen::Vector3d axis, const double angle)
{
    geometry_msgs::PoseStamped angleAxis;
    angleAxis.header = header;
    angleAxis.pose.orientation.w = angle;
    angleAxis.pose.orientation.x = axis.x();
    angleAxis.pose.orientation.y = axis.y();
    angleAxis.pose.orientation.z = axis.z();
    debug_pub.publish(angleAxis);
}

void propagate(const geometry_msgs::Vector3Stamped &gyro)
{
    double cur_t = gyro.header.stamp.toSec();
    // ROS_INFO("propagate");
    Vector3d w;
    w(0) = gyro.vector.x;
    w(1) = gyro.vector.y;
    w(2) = gyro.vector.z;

    double dt = cur_t - t_prev;
    // ROS_INFO("dt in propagate is %f, at the cur_t %f, with t_prev at %f", dt, cur_t, t_prev);
	// ROS_INFO("propagate, with dt %f", dt);
    Vector3d domg = 0.5 * dt * w;

    Quaterniond dq(sqrt(1 - domg.squaredNorm()), domg(0), domg(1), domg(2));
    // Quaterniond dq(1, domg(0), domg(1), domg(2));
	// ROS_INFO("dq w x y z %f %f %f %f", dq.w(), dq.x(), dq.y(), dq.z() );
    Quaterniond q_state(x(0), x(1), x(2), x(3));
    Quaterniond q = (q_state * dq).normalized();

    x(0) = q.w();
    x(1) = q.x();
    x(2) = q.y();
    x(3) = q.z();

    Matrix3d w_hat;
    w_hat << 0,-w(2), w(1),
            w(2), 0, -w(0),
           -w(1), w(0), 0;
    Matrix3d A = -w_hat;

    MatrixXd U = -MatrixXd::Identity(3, 3);

    MatrixXd F, V;
    F = MatrixXd::Identity(3, 3) + dt * A;
    V = dt * U;

    P = F * P * F.transpose() + V * R * V.transpose();

    t_prev = cur_t;
	// cout << "P " << endl << P << endl;
	// cout << "x " << endl << x.transpose() << endl;
}

static void update(const geometry_msgs::TwistStamped::ConstPtr &pnp)
{
    Vector3d camera_T_shield;
    ROS_INFO("Update, at time %f", pnp->header.stamp.toSec());

    camera_T_shield[0] = pnp->twist.linear.x;
    camera_T_shield[1] = pnp->twist.linear.y;
    camera_T_shield[2] = pnp->twist.linear.z;
    Vector3d imu_T_shield = imu_R_camera * camera_T_shield + imu_T_camera;

    Vector3d T_norm = imu_T_shield.normalized();
    Vector3d x_axis = Vector3d::UnitX();
    // Vector3d axis = x_axis.cross(T_norm).normalized();
    // double angle = acos( x_axis.dot(T_norm) );
    Vector3d axis = T_norm.cross(x_axis).normalized();
    double angle = acos( T_norm.dot(x_axis) );
    AngleAxisd camera_R_shield(angle, axis);
    Quaterniond camera_q_shield(camera_R_shield);

/*
    Quaterniond camera_q_shield;
    camera_q_shield.w() = x_axis.dot(T_norm);
    camera_q_shield.x() = sin(0.5 * angle) * axis(0);
    camera_q_shield.y() = sin(0.5 * angle) * axis(1);
    camera_q_shield.z() = sin(0.5 * angle) * axis(2);
    camera_q_shield.normalize();
    // Debug original pose from camera
    pub_debug_pose(pnp->header, camera_q_shield);

    */

    pub_debug_angleAxis(pnp->header, axis, angle);

    Matrix3d C = Matrix3d::Identity();

    MatrixXd K(3, 3);
    K = P * C.transpose() * (C * P* C.transpose() + Q).inverse();

    VectorXd r(3); // residual
    Quaterniond qm = camera_q_shield;
    Quaterniond q  = Quaterniond(x(0), x(1), x(2), x(3));
    Quaterniond dq = q.conjugate() * qm;
    // ROS_INFO("dq w x y z %f %f %f %f", dq.w(), dq.x(), dq.y(), dq.z() );
    r = 2 * dq.vec();
    Vector3d _r = K * r;
    // ROS_INFO("dr x y z %f %f %f", _r[0], _r[1], _r[2] );
    Vector3d dw(_r(0) * 0.5, _r(1) * 0.5, _r(2) * 0.5);
    dq = Quaterniond(sqrt(1 - dw.squaredNorm()), dw(0), dw(1), dw(2)).normalized();
    q  = q * dq;

    x(0) = q.w();
    x(1) = q.x();
    x(2) = q.y();
    x(3) = q.z();

    P = P - K * C * P;
	// cout << "P " << endl << P << endl;
	// cout << "x " << endl << x.transpose() << endl;
}

/**
 * initialization of the gyroscope
 * @param gyro
 */
static void initialize_gyro(const geometry_msgs::Vector3Stamped::ConstPtr &gyro)
{
    t_prev = gyro->header.stamp.toSec();
    x_history.push(x);
    P_history.push(P);
    gyro_buf.push(gyro);
    gyro_count++;
    if (gyro_count == GYRO_INIT_COUNT) {
        gyro_initialized = true;
    }
}

/**
 * initialization of the state and convariance from visual
 * @param pnp
 */
static void initialize_visual(const geometry_msgs::TwistStamped::ConstPtr &pnp)
{
    double cur_t = pnp->header.stamp.toSec();
    Vector3d camera_T_shield;

    camera_T_shield[0] = pnp->twist.linear.x;
    camera_T_shield[1] = pnp->twist.linear.y;
    camera_T_shield[2] = pnp->twist.linear.z;
    Vector3d imu_T_shield = imu_R_camera * camera_T_shield + imu_T_camera;

    Vector3d T_norm = imu_T_shield.normalized();
    Vector3d x_axis = Vector3d::UnitX();
    // Vector3d axis = x_axis.cross(T_norm).normalized();
    // double angle = acos( x_axis.dot(T_norm) );
    Vector3d axis = T_norm.cross(x_axis).normalized();
    double angle = acos( T_norm.dot(x_axis) );
    AngleAxisd camera_R_shield(angle, axis);
    Quaterniond camera_q_shield(camera_R_shield);
/*
    Quaterniond camera_q_shield;
    camera_q_shield.w() = x_axis.dot(T_norm);
    camera_q_shield.x() = sin(0.5 * angle) * axis(0);
    camera_q_shield.y() = sin(0.5 * angle) * axis(1);
    camera_q_shield.z() = sin(0.5 * angle) * axis(2);
    camera_q_shield.normalize();
*/

    x(0) = camera_q_shield.w();
    x(1) = camera_q_shield.x();
    x(2) = camera_q_shield.y();
    x(3) = camera_q_shield.z();
    P = MatrixXd::Identity(3, 3);
	ROS_INFO("visual init at %f", cur_t);
    cout << "DEBUG: x initialized with " << endl << x.transpose() << endl;

    // pub_fused_pose(pnp->header);
    pub_fused_angleAxis(pnp->header);
    t_prev = cur_t;

    visual_initialized = true;
}

/**
 * Processing the valid visual and gyro information
 */
static void state_machine_process(void)
{
    if (!gyro_initialized)
        return;
    //if (!visual_initialized)
    if (!visual_valid)
        return;

    while (!visual_buf.empty()) {
        while(!gyro_buf.empty() &&
              gyro_buf.front()->header.stamp < visual_buf.front()->header.stamp)
        {
            // trace backward the time to the imu timestamp
            t_prev = gyro_buf.front()->header.stamp.toSec();
            ROS_INFO("throw state with time: %f", t_prev);
            gyro_buf.pop();
            x_history.pop();
            P_history.pop();
        }

        geometry_msgs::TwistStamped::ConstPtr visual_msg = visual_buf.front();

        update(visual_msg);
        visual_buf.pop();
        // pub_fused_angleAxis(visual_msg->header);

        // Repropagate
        while (!x_history.empty()) x_history.pop();
        while (!P_history.empty()) P_history.pop();

        queue <geometry_msgs::Vector3Stamped::ConstPtr> temp_gyro_buf;
        while (!gyro_buf.empty())
        {
            propagate(*gyro_buf.front());
            temp_gyro_buf.push(gyro_buf.front());
            x_history.push(x);
            P_history.push(P);
            gyro_buf.pop();
        }
        swap(gyro_buf, temp_gyro_buf);

        // pub_fused_angleAxis();
    }
}

/**
 * handle, save, and process visual messages
 * @param pnp
 */
void visual_callback(const geometry_msgs::TwistStamped::ConstPtr &pnp)
{
    visual_valid = !(pnp->twist.linear.x == 0 &&
        pnp->twist.linear.y == 0 &&
        pnp->twist.linear.z == 0 );

    if (visual_valid && visual_initialized) {

        visual_buf.push(pnp);
    }
    else if (visual_valid && !visual_initialized) {
        initialize_visual(pnp);
        visual_buf.push(pnp);
    }

    state_machine_process();
}

/**
 * handle 100Hz angular filtered gyroscope
 * @param gyro
 */
void gyro_callback(const geometry_msgs::Vector3Stamped::ConstPtr &gyro)
{
    if (!gyro_initialized) {
        initialize_gyro(gyro);
    }
    else {
        propagate(*gyro);
        // pub_fused_pose(gyro->header);
        pub_fused_angleAxis(gyro->header);
        x_history.push(x);
        P_history.push(P);
        gyro_buf.push(gyro);
		if (gyro_buf.size() > MAX_GYRO_QUEUE_SIZE) {
			x_history.pop();
			P_history.pop();
			gyro_buf.pop();
		}
    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "visual_gyro_fused");
    ros::NodeHandle n("~");

    n.param("angular_fused", omg_topic, string("/dji_sdk/angular_velocity_fused")); // 100Hz
    n.param("visual_topic", visual_topic, string("/pnp_twist"));
    n.param("publisher_topic", publisher_topic, string("/visual_ekf/shield_pose_fused"));
    n.param("gyroscope_noise_weight", gyro_weight, 0.1);
    n.param("visual_pose_weight", visual_q_weight, 1.0);
    n.param("node_sleep_time", sleep_time, 10);

    // TODO: initalize the R and Q matrix
    R =     gyro_weight * MatrixXd::Identity(3, 3); // gyro noise
    Q = visual_q_weight * MatrixXd::Identity(3, 3); // observation noise

    x.setZero();
    x(0) = 1; // set quaternion to identity
    P = 1.0 * P;

    imu_R_camera <<  0, 0, 1,
                    -1, 0, 0,
                     0,-1, 0;

    imu_T_camera <<  200, 50, 0; // in millimeter

    ros::Subscriber s2 = n.subscribe(visual_topic, 10, visual_callback);
    ros::Subscriber s3 = n.subscribe(omg_topic, 100, gyro_callback);
//    pose_pub = n.advertise<geometry_msgs::PoseStamped>(publisher_topic, 100);
    pose_pub = n.advertise<geometry_msgs::PoseWithCovarianceStamped>(publisher_topic, 100);
    debug_pub= n.advertise<geometry_msgs::PoseStamped>(string("/visual_ekf/visual_ekf_debug"), 100);
//    debug_gyro_pub= n.advertise<geometry_msgs::Vector3Stamped>(string("/visual_ekf/gyro_debug"), 100);

    ros::Rate r(100);
    ros::spin();
}

/**
 *  0   q_w     shield frame --> camera frame
 *  1   q_x
 *  2   q_y
 *  3   q_z
 */

/*
        // DEBUG geometry relationship in visual_callback()
        update(pnp);
        pub_fused_angleAxis(pnp->header);

        geometry_msgs::TwistStamped::ConstPtr prev_pnp = visual_buf.front();

        Vector3d camera_T_shield;
        camera_T_shield[0] = prev_pnp->twist.linear.x;
        camera_T_shield[1] = prev_pnp->twist.linear.y;
        camera_T_shield[2] = prev_pnp->twist.linear.z;
        Vector3d imu_T_shield = imu_R_camera * camera_T_shield;

        Vector3d T_norm = imu_T_shield.normalized();
        Vector3d x_axis = Vector3d::UnitX();
    //    Vector3d axis = x_axis.cross(T_norm).normalized();
    //    double angle = acos( x_axis.dot(T_norm) );
        Vector3d axis = T_norm.cross(x_axis).normalized();
        double angle = acos( T_norm.dot(x_axis) );
        AngleAxisd R_prev(angle, axis);
        Quaterniond q_prev(R_prev);

        camera_T_shield[0] = pnp->twist.linear.x;
        camera_T_shield[1] = pnp->twist.linear.y;
        camera_T_shield[2] = pnp->twist.linear.z;
        imu_T_shield = imu_R_camera * camera_T_shield;

        T_norm = imu_T_shield.normalized();
    //    axis = x_axis.cross(T_norm).normalized();
    //    angle = acos( x_axis.dot(T_norm) );
        axis = T_norm.cross(x_axis).normalized();
        angle = acos( T_norm.dot(x_axis) );
        AngleAxisd R_now(angle, axis);
        Quaterniond q_now(R_now);

        Quaterniond dq = q_prev.conjugate() * q_now;
        // Quaterniond dq = q_now.conjugate() * q_prev;
        Vector3d d_theta = 2 * dq.vec();
        double dt_omg = pnp->header.stamp.toSec() - prev_pnp->header.stamp.toSec();
        Vector3d omg = d_theta / dt_omg;

        geometry_msgs::Vector3Stamped gyro_debug;
        gyro_debug.header = pnp->header;
        gyro_debug.vector.x = omg[0];
        gyro_debug.vector.y = omg[1];
        gyro_debug.vector.z = omg[2];
        propagate( gyro_debug );
        t_prev = pnp->header.stamp.toSec();
        debug_gyro_pub.publish(gyro_debug);

        visual_buf.pop();

        */


#include "controllers/sfm_aligner.h"

#include <ceres/cost_function.h>
#include <ceres/problem.h>
#include <ceres/rotation.h>
#include <ceres/solver.h>
#include <glog/logging.h>

#include <Eigen/Cholesky>
#include <Eigen/Geometry>
#include <Eigen/SparseCholesky>
#include <Eigen/SparseCore>
#include <limits>
#include <memory>
#include <unordered_map>
#include <vector>

#include "base/similarity_transform.h"
#include "base/track_selection.h"
#include "estimators/ransac_similarity.h"
#include "estimators/sim3.h"
#include "estimators/similarity_transform.h"
#include "math/util.h"
#include "optim/bundle_adjustment.h"
#include "sfm/incremental_triangulator.h"
#include "solver/l1_solver.h"
#include "util/misc.h"
#include "util/reconstruction_io.h"
#include "util/timer.h"

namespace DAGSfM {
namespace {

double CheckReprojError(const vector<Eigen::Vector3d>& src_observations,
                        const vector<Eigen::Vector3d>& dst_observations,
                        const double& scale, const Eigen::Matrix3d& R,
                        const Eigen::Vector3d& t) {
  double reproj_err = 0.0;
  const int size = src_observations.size();
  for (int i = 0; i < size; i++) {
    Eigen::Vector3d reproj_obv = scale * R * src_observations[i] + t;
    reproj_err += (reproj_obv - dst_observations[i]).norm();
  }

  LOG(INFO) << "Mean Reprojection Error: " << reproj_err / size << " ("
            << reproj_err << "/" << size << ")";
  return reproj_err / size;
}

void FindSimilarityTransform(const std::vector<Eigen::Vector3d>& observations1,
                             const std::vector<Eigen::Vector3d>& observations2,
                             const double threshold, const double p,
                             Eigen::Matrix3d& R, Eigen::Vector3d& t,
                             double& scale, double& msd) {
  std::vector<Eigen::Vector3d> inliers1, inliers2;

  if (observations1.size() > 5) {
    LOG(INFO) << "Finding Similarity by RANSAC";
    RansacSimilarity(observations1, observations2, inliers1, inliers2, R, t,
                     scale, threshold, p);
    VLOG(2) << "inliers size: " << inliers1.size();
    // Re-compute similarity by inliers
    Eigen::MatrixXd x1 = Eigen::MatrixXd::Zero(3, inliers1.size()),
                    x2 = Eigen::MatrixXd::Zero(3, inliers2.size());
    for (unsigned int i = 0; i < inliers1.size(); i++) {
      x1.col(i) = inliers1[i];
      x2.col(i) = inliers2[i];
    }
    DAGSfM::FindRTS(x1, x2, &scale, &t, &R);
    // Optional non-linear refinement of the found parameters
    DAGSfM::Refine_RTS(x1, x2, &scale, &t, &R);

    if (inliers1.size() < 4) {
      msd = numeric_limits<double>::max();
      return;
    }
    // else msd = CheckReprojError(inliers1, inliers2, scale, R, t);
  }

  if (observations1.size() <= 5 || inliers1.size() <= 5) {
    Eigen::MatrixXd x1 = Eigen::MatrixXd::Zero(3, observations1.size()),
                    x2 = Eigen::MatrixXd::Zero(3, observations2.size());
    for (unsigned int i = 0; i < observations1.size(); i++) {
      x1.col(i) = observations1[i];
      x2.col(i) = observations2[i];
    }
    DAGSfM::FindRTS(x1, x2, &scale, &t, &R);
    DAGSfM::Refine_RTS(x1, x2, &scale, &t, &R);

    // msd = CheckReprojError(observations1, observations2, scale, R, t);
  }

  msd = CheckReprojError(observations1, observations2, scale, R, t);
}

void FindCommon3DPoints(const std::vector<image_t>& common_reg_images,
                        const Reconstruction& recon1,
                        const Reconstruction& recon2,
                        std::vector<Eigen::Vector3d>& src_points,
                        std::vector<Eigen::Vector3d>& ref_points) {
  // std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>> common_3D_points;
  std::unordered_set<image_t> common_image_ids(common_reg_images.begin(),
                                               common_reg_images.end());
  LOG(INFO) << "Begin find common 3D points";
  for (const auto& point3D : recon2.Points3D()) {
    const Eigen::Vector3d point3D2 = point3D.second.XYZ();

    for (const auto& track_el : point3D.second.Track().Elements()) {
      if (common_image_ids.count(track_el.image_id) > 0) {
        const auto& point2D =
            recon1.Image(track_el.image_id).Point2D(track_el.point2D_idx);
        if (point2D.HasPoint3D()) {
          const Eigen::Vector3d point3D1 =
              recon1.Point3D(point2D.Point3DId()).XYZ();
          // common_3D_points.emplace_back(point3D1, point3D2);
          src_points.emplace_back(point3D1);
          ref_points.emplace_back(point3D2);
        }
      }
    }
  }
  LOG(INFO) << "Find " << ref_points.size() << " common 3D points.";
}

}  // namespace

SfMAligner::SfMAligner(const std::vector<Reconstruction*>& reconstructions,
                       const AlignOptions& options)
    : options_(options), reconstructions_(reconstructions) {
  // some logic or parameters check
  CHECK_GT(reconstructions.size(), 0);
  CHECK_GT(reconstructions_.size(), 0);

  for (unsigned int i = 0; i < reconstructions_.size(); i++) {
    LOG(INFO) << "Node id: " << i;
    CHECK_NOTNULL(reconstructions_[i]);
    LOG(INFO) << "Total images number: " << reconstructions_[i]->NumImages();
  }
}

const std::vector<BitmapColor<float>> SfMAligner::ColorContainers = {
    BitmapColor<float>(255, 25.5, 0), BitmapColor<float>(0, 255, 255),
    BitmapColor<float>(255, 102, 0),  BitmapColor<float>(153, 51, 204),
    BitmapColor<float>(0, 255, 51),   BitmapColor<float>(255, 0, 204),
    BitmapColor<float>(255, 255, 0),  BitmapColor<float>(255, 153, 255),
    BitmapColor<float>(255, 51, 0),   BitmapColor<float>(0, 204, 255),
    BitmapColor<float>(255, 204, 255)};

bool SfMAligner::Align() {
  Timer timer;

  // 1. Constructing a graph from reconstructions,
  // each node is a reconstruction, edges represent the connections between
  // reconstructions (by the means of common images or common 3D points), the
  // weight of edge represents the mean reprojection error.
  LOG(INFO) << "Constructing Reconstructions Graph...";
  timer.Start();
  ConstructReconsGraph();
  timer.Pause();
  summary_.construct_recon_graph_time = timer.ElapsedSeconds();
  recons_graph_.ShowInfo();
  CHECK_EQ(recons_graph_.GetNodesNum(), reconstructions_.size());

  // The reconstruction graph should be at least a spanning tree,
  // or we couldn't stitch all reconstructions together due to
  // too large alignment error or disconnected components.
  if (recons_graph_.GetEdgesNum() < recons_graph_.GetNodesNum() - 1) {
    LOG(WARNING) << "Can't align all reconstructions together due to "
                 << "too large alignment error or disconnected components."
                 << "We would just merge local maps in the largest connected "
                    "components.";
  }
  const Graph<Node, Edge> largest_cc = recons_graph_.ExtractLargestCC();
  std::vector<size_t> vec_largest_cc_nodes;
  vec_largest_cc_nodes.reserve(largest_cc.GetNodes().size());
  for (auto node_it : largest_cc.GetNodes()) {
    vec_largest_cc_nodes.push_back(node_it.first);
  }

  // 2. Constructing a minimum spanning tree, thus we can select the
  // most accurate n - 1 edges for accurate alignment.
  LOG(INFO) << "Finding Minimum Spanning Tree...";
  timer.Start();
  std::vector<Edge> mst_edges = largest_cc.Kruskal();

  Graph<Node, Edge> mst;
  for (const auto edge : mst_edges) {
    mst.AddEdge(edge);
  }

  if (mst_edges.size() < largest_cc.GetNodesNum() - 1) {
    LOG(WARNING) << "Invalid MST";
    mst.ShowInfo();
    return false;
  }
  mst.ShowInfo();
  timer.Pause();
  summary_.construct_mst_time = timer.ElapsedSeconds();

  // 3. Finding an anchor node, an anchor node is a reference reconstruction
  // that all other reconstructions should be aligned to.
  LOG(INFO) << "Finding Anchor Node...";
  timer.Start();
  FindAnchorNode(&mst);
  timer.Pause();
  summary_.find_anchor_node_time = timer.ElapsedSeconds();

  // 4. Compute the final transformation to anchor node for each cluster
  LOG(INFO) << "Computing Final Similarity Transformations...";
  timer.Start();
  for (auto i : vec_largest_cc_nodes) {
    if (static_cast<int>(i) != anchor_node_.id) {
      this->ComputePath(i, anchor_node_.id);
    }
  }
  sim3_to_anchor_[anchor_node_.id] = Sim3();
  timer.Pause();
  summary_.compute_final_transformation_time = timer.ElapsedSeconds();

  // 5. Merging all other reconstructions to anchor node
  LOG(INFO) << "Merging Reconstructions...";
  timer.Start();
  this->MergeReconstructions(vec_largest_cc_nodes);
  timer.Pause();
  summary_.merging_time = timer.ElapsedSeconds();

  return true;
}

Node SfMAligner::GetAnchorNode() const { return anchor_node_; }

std::vector<Sim3> SfMAligner::GetSim3ToAnchor() const {
  return sim3_to_anchor_;
}

const std::unordered_set<image_t>& SfMAligner::GetSeparators() const {
  return separators_;
}

void SfMAligner::ConstructReconsGraph() {
  // 1. Add nodes
  for (size_t i = 0; i < reconstructions_.size(); i++) {
    Node node(i);
    // node.recon = reconstructions_[i];
    recons_graph_.AddNode(node);
  }

  // 2. Add edges
  for (unsigned int i = 0; i < reconstructions_.size(); i++) {
    for (unsigned int j = i + 1; j < reconstructions_.size(); j++) {
      const double weight = ComputeEdgeWeight(i, j);
      LOG(INFO) << "weight: " << weight;
      if (weight != std::numeric_limits<double>::max()) {
        recons_graph_.AddEdge(Edge(i, j, (float)weight));
      }
    }
  }
}

double SfMAligner::ComputeEdgeWeight(const unsigned int i, const unsigned int j) {
  const Reconstruction& recon1 = *reconstructions_[i];
  const Reconstruction& recon2 = *reconstructions_[j];
  double weight = std::numeric_limits<double>::max();

  Eigen::Matrix3d R1 = Eigen::Matrix3d::Identity(3, 3),
                  R2 = Eigen::Matrix3d::Identity(3, 3);
  Eigen::Vector3d t1 = Eigen::Vector3d::Zero(), t2 = Eigen::Vector3d::Zero();
  double s1 = 1.0, s2 = 1.0;

  // Find common registered images
  std::vector<image_t> common_reg_images = recon1.FindCommonRegImageIds(recon2);
  for (auto image_id : common_reg_images) {
    separators_.insert(image_id);
  }
  std::vector<Eigen::Vector3d> src_points, ref_points;
  // for (const auto common_id : common_reg_images) {
  //     src_points.push_back(recon1.Image(common_id).ProjectionCenter());
  //     ref_points.push_back(recon2.Image(common_id).ProjectionCenter());
  // }
  FindCommon3DPoints(common_reg_images, recon1, recon2, src_points, ref_points);
  LOG(INFO) << "Common registerd images number: " << common_reg_images.size();

  if (common_reg_images.size() < 2) {
    LOG(WARNING) << "Not found enough common registered images.";
    return std::numeric_limits<double>::max();
  } else {
    double msd1 = 0.0, msd2 = 0.0;
    FindSimilarityTransform(src_points, ref_points, options_.threshold,
                            options_.confidence, R1, t1, s1, msd1);
    FindSimilarityTransform(ref_points, src_points, options_.threshold,
                            options_.confidence, R2, t2, s2, msd2);

    weight = std::max(msd1, msd2);

    if (weight != numeric_limits<double>::max()) {
      sim3_graph_[i][j] = Sim3(R1, t1, s1);
      sim3_graph_[j][i] = Sim3(R2, t2, s2);
    }
  }
  // else if (common_reg_images.size() < 3) {
  //     std::vector<Eigen::Matrix3d> src_rotations, ref_rotations;
  //     for (const auto common_id : common_reg_images) {
  //         src_rotations.push_back(recon1.Image(common_id).RotationMatrix());
  //         ref_rotations.push_back(recon2.Image(common_id).RotationMatrix());
  //     }
  //     ComputeSimilarityByCameraMotions(src_points, ref_points,
  //                                      src_rotations, ref_rotations, R1, t1,
  //                                      s1);
  //     ComputeSimilarityByCameraMotions(ref_points, src_points,
  //                                      ref_rotations, src_rotations, R2, t2,
  //                                      s2);

  //     double msd1 = CheckReprojError(src_points, ref_points, s1, R1, t1),
  //            msd2 = CheckReprojError(ref_points, src_points, s2, R2, t2);

  //     weight = std::max(msd1, msd2);

  //     if (weight != numeric_limits<double>::max()) {
  //         sim3_graph_[i][j] = Sim3(R1, t1, s1);
  //         sim3_graph_[j][i] = Sim3(R2, t2, s2);
  //     }
  // }

  return (weight > options_.max_reprojection_error)
             ? std::numeric_limits<double>::max()
             : weight;
}

void SfMAligner::FindAnchorNode(Graph<Node, Edge>* graph) {
  paths_.resize(recons_graph_.GetNodesNum());
  sim3_to_anchor_.resize(recons_graph_.GetNodesNum());

  // The anchor is found by merging all leaf nodes to their adjacent nodes,
  // until one node or two nodes left. If two nodes left, we choose the
  // reconstruction that has the largest size as the anchor.
  int layer = 1;
  unsigned int anchor_index = 0;

  while (graph->GetNodesNum() > 1) {
    LOG(INFO) << "Merging the " << layer++ << "-th layer leaf nodes";

    graph->CountOutDegrees();
    graph->CountInDegrees();
    graph->CountDegrees();
    std::unordered_map<size_t, size_t> degrees = graph->GetDegrees();

    // Finding all leaf nodes. Leaf node in graph has degree equals to 1.
    std::vector<size_t> indexes;
    if (graph->GetNodesNum() == 2) {
      indexes.push_back(degrees.begin()->first);
    } else {
      for (auto it = degrees.begin(); it != degrees.end(); ++it) {
        LOG(INFO) << "node: " << it->first << ", "
                  << "degree: " << it->second;
        if (it->second == 1) indexes.push_back(it->first);
      }
    }
    if (indexes.empty()) break;

    for (auto idx : indexes) {
      // if (idx == -1) break;
      const Edge& edge = graph->FindConnectedEdge(idx);

      LOG(INFO) << "Find node [degree = 1]: " << idx;
      LOG(INFO) << edge.src << "->" << edge.dst << ": " << edge.weight;

      // src is the node with degree = 1
      unsigned int src = (idx == edge.src) ? edge.src : edge.dst;
      unsigned int dst = (idx == edge.src) ? edge.dst : edge.src;

      LOG(INFO) << "Merge Clusters: " << src << "->" << dst << ": "
                << edge.weight;
      anchor_index = dst;
      const Sim3 sim = sim3_graph_[src][dst];
      paths_[src].insert(std::make_pair(dst, sim));

      graph->DeleteNode(src);
      graph->DeleteEdge(src, dst);
      graph->DeleteEdge(dst, src);
      graph->ShowInfo();
    }
  }

  anchor_node_.id = anchor_index;
}

void SfMAligner::ComputePath(int src, int dst) {
  LOG(INFO) << "Computing Path: " << src << "->" << dst;
  std::queue<int> qu;
  qu.push(src);

  Eigen::Matrix3d r = Eigen::Matrix3d::Identity();
  Eigen::Vector3d t = Eigen::Vector3d::Zero();
  double s = 1.0;

  Sim3 sim(Eigen::Matrix3d::Identity(), Eigen::Vector3d::Identity(), 1.0);
  LOG(INFO) << "v: " << src;
  while (!qu.empty()) {
    int u = qu.front();
    qu.pop();
    auto it = paths_[u].begin();
    int v = it->first;
    LOG(INFO) << "v: " << v;
    s = it->second.s * s;
    r = it->second.R * r.eval();
    t = it->second.s * it->second.R * t.eval() + it->second.t;
    if (v == dst) {
      sim.s = s;
      sim.R = r;
      sim.t = t;
      sim3_to_anchor_[src] = sim;
      return;
    } else
      qu.push(v);
  }
  LOG(INFO) << "\n";
}

void SfMAligner::MergeReconstructions(std::vector<size_t>& node_ids) {
  if (options_.assign_color_for_clusters) {
    for (size_t i = 0; i < reconstructions_.size(); i++) {
      const int color_id = i % SfMAligner::ColorContainers.size();
      reconstructions_[i]->AssignColorsForAllPoints(
          SfMAligner::ColorContainers[color_id]);
      // // Assign cluster id for each image.
      // const std::vector<image_t> reg_image_ids =
      // reconstructions_[i]->RegImageIds(); for (auto image_id : reg_image_ids)
      // {
      //     Image& image = reconstructions_[i]->Image(image_id);
      //     image.SetClusterId(i);
      // }
    }
  }

  for (auto id : node_ids) {
    if (static_cast<int>(id) == anchor_node_.id) {
      continue;
    }

    Sim3 sim3 = sim3_to_anchor_[id];
    Eigen::Matrix3x4d alignment;
    alignment.block(0, 0, 3, 3) = sim3.s * sim3.R;
    alignment.block(0, 3, 3, 1) = sim3.t;

    reconstructions_[anchor_node_.id]->Merge(*reconstructions_[id], alignment);
  }
}

bool ComputeSimilarityByCameraMotions(
    std::vector<Eigen::Vector3d>& camera_centers1,
    std::vector<Eigen::Vector3d>& camera_centers2,
    std::vector<Eigen::Matrix3d>& camera_rotations1,
    std::vector<Eigen::Matrix3d>& camera_rotations2,
    Eigen::Matrix3d& relative_r, Eigen::Vector3d& relative_t, double& scale) {
  // my hybrid approach by combining "Divide and Conquer: Efficient Large-Scale
  // Structure from Motion Using Graph Partitioning" and RANSAC

  const unsigned int n = camera_centers1.size();
  std::vector<Eigen::Vector3d> ts1(n);
  std::vector<Eigen::Vector3d> ts2(n);

  for (unsigned int i = 0; i < n; i++) {
    ts1[i] = -camera_rotations1[i] * camera_centers1[i];
    ts2[i] = -camera_rotations2[i] * camera_centers2[i];
  }

  // compute relative scale from a->b
  std::vector<double> scales;
  for (unsigned int i = 0; i < n; i++) {
    Eigen::Vector3d center_a1 = camera_centers1[i];
    Eigen::Vector3d center_b1 = camera_centers2[i];
    for (unsigned int j = i + 1; j < n; j++) {
      Eigen::Vector3d center_a2 = camera_centers1[j];
      Eigen::Vector3d center_b2 = camera_centers2[j];
      double scale_ab =
          (center_b1 - center_b2).norm() / (center_a1 - center_a2).norm();
      scales.push_back(scale_ab);
    }
  }
  // retrieve the median of scales, according to
  // the equation (5) of the paper "Divide and Conquer: Efficient Large-Scale
  // Structure from Motion Using Graph Partitioning"
  std::sort(scales.begin(), scales.end());
  scale = scales[scales.size() / 2];

  // compute relative rotation & relative translation from a->b
  std::vector<Correspondence3D> corres3d;
  std::vector<CorrespondenceEuc> input_datas;
  for (unsigned int i = 0; i < camera_centers1.size(); i++) {
    corres3d.emplace_back(camera_centers1[i], camera_centers2[i]);
    input_datas.push_back(make_pair(Euclidean3D(camera_rotations1[i], ts1[i]),
                                    Euclidean3D(camera_rotations2[i], ts2[i])));
  }
  EuclideanEstimator euc_estimator(scale, corres3d);

  Euclidean3D euc3d;
  RansacParameters params;
  params.rng =
      std::make_shared<RandomNumberGenerator>((unsigned int)time(NULL));
  params.error_thresh = 0.002;
  params.max_iterations = 1000;

  Prosac<EuclideanEstimator> prosac_euc3(params, euc_estimator);
  prosac_euc3.Initialize();
  RansacSummary summary;
  prosac_euc3.Estimate(input_datas, &euc3d, &summary);

  relative_r = euc3d.R;
  relative_t = euc3d.t;

  return true;
}

}  // namespace DAGSfM


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2018 John Maddock
//  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)

#ifndef BOOST_HYPERGEOMETRIC_1F1_LARGE_ABZ_HPP_
#define BOOST_HYPERGEOMETRIC_1F1_LARGE_ABZ_HPP_

#include <boost/math/special_functions/detail/hypergeometric_1F1_bessel.hpp>
#include <boost/math/special_functions/detail/hypergeometric_series.hpp>
#include <boost/math/special_functions/gamma.hpp>

  namespace boost { namespace math { namespace detail {

     template <class T>
     inline bool is_negative_integer(const T& x)
     {
        using std::floor;
        return (x <= 0) && (floor(x) == x);
     }

     template <class T, class Policy>
     struct hypergeometric_1F1_igamma_series
     {
        enum{ cache_size = 64 };

        typedef T result_type;
        hypergeometric_1F1_igamma_series(const T& alpha, const T& delta, const T& x, const Policy& pol)
           : delta_poch(-delta), alpha_poch(alpha), x(x), k(0), cache_offset(0), pol(pol)
        {
           BOOST_MATH_STD_USING
           T log_term = log(x) * -alpha;
           log_scaling = itrunc(log_term - 3 - boost::math::tools::log_min_value<T>() / 50);
           term = exp(log_term - log_scaling);
           refill_cache();
        }
        T operator()()
        {
           if (k - cache_offset >= cache_size)
           {
              cache_offset += cache_size;
              refill_cache();
           }
           T result = term * gamma_cache[k - cache_offset];
           term *= delta_poch * alpha_poch / (++k * x);
           delta_poch += 1;
           alpha_poch += 1;
           return result;
        }
        void refill_cache()
        {
           typedef typename lanczos::lanczos<T, Policy>::type lanczos_type;

           gamma_cache[cache_size - 1] = boost::math::gamma_p(alpha_poch + ((int)cache_size - 1), x, pol);
           for (int i = cache_size - 1; i > 0; --i)
           {
              gamma_cache[i - 1] = gamma_cache[i] >= 1 ? T(1) : T(gamma_cache[i] + regularised_gamma_prefix(T(alpha_poch + (i - 1)), x, pol, lanczos_type()) / (alpha_poch + (i - 1)));
           }
        }
        T delta_poch, alpha_poch, x, term;
        T gamma_cache[cache_size];
        int k;
        int log_scaling;
        int cache_offset;
        Policy pol;
     };

     template <class T, class Policy>
     T hypergeometric_1F1_igamma(const T& a, const T& b, const T& x, const T& b_minus_a, const Policy& pol, int& log_scaling)
     {
        BOOST_MATH_STD_USING
        if (b_minus_a == 0)
        {
           // special case: M(a,a,z) == exp(z)
           int scale = itrunc(x, pol);
           log_scaling += scale;
           return exp(x - scale);
        }
        hypergeometric_1F1_igamma_series<T, Policy> s(b_minus_a, a - 1, x, pol);
        log_scaling += s.log_scaling;
        boost::uintmax_t max_iter = boost::math::policies::get_max_series_iterations<Policy>();
        T result = boost::math::tools::sum_series(s, boost::math::policies::get_epsilon<T, Policy>(), max_iter);
        boost::math::policies::check_series_iterations<T>("boost::math::tgamma<%1%>(%1%,%1%)", max_iter, pol);
        T log_prefix = x + boost::math::lgamma(b, pol) - boost::math::lgamma(a, pol);
        int scale = itrunc(log_prefix);
        log_scaling += scale;
        return result * exp(log_prefix - scale);
     }

     template <class T, class Policy>
     T hypergeometric_1F1_shift_on_a(T h, const T& a_local, const T& b_local, const T& x, int a_shift, const Policy& pol, int& log_scaling)
     {
        BOOST_MATH_STD_USING
        T a = a_local + a_shift;
        if (a_shift == 0)
           return h;
        else if (a_shift > 0)
        {
           //
           // Forward recursion on a is stable as long as 2a-b+z > 0.
           // If 2a-b+z < 0 then backwards recursion is stable even though
           // the function may be strictly increasing with a.  Potentially
           // we may need to split the recurrence in 2 sections - one using
           // forward recursion, and one backwards.
           //
           // We will get the next seed value from the ratio
           // on b as that's stable and quick to compute.
           //

           T crossover_a = (b_local - x) / 2;
           int crossover_shift = itrunc(crossover_a - a_local);

           if (crossover_shift > 1)
           {
              //
              // Forwards recursion will start off unstable, but may switch to the stable direction later.
              // Start in the middle and go in both directions:
              //
              if (crossover_shift > a_shift)
                 crossover_shift = a_shift;
              crossover_a = a_local + crossover_shift;
              boost::math::detail::hypergeometric_1F1_recurrence_b_coefficients<T> b_coef(crossover_a, b_local, x);
              boost::uintmax_t max_iter = boost::math::policies::get_max_series_iterations<Policy>();
              T b_ratio = boost::math::tools::function_ratio_from_backwards_recurrence(b_coef, boost::math::policies::get_epsilon<T, Policy>(), max_iter);
              boost::math::policies::check_series_iterations<T>("boost::math::hypergeometric_1F1_large_abz<%1%>(%1%,%1%,%1%)", max_iter, pol);
              //
              // Convert to a ratio:
              //         (1+a-b)M(a, b, z) - aM(a+1, b, z) + (b-1)M(a, b-1, z) = 0
              //
              //  hence: M(a+1,b,z) = ((1+a-b) / a) M(a,b,z) + ((b-1) / a) M(a,b,z)/b_ratio
              //
              T first = 1;
              T second = ((1 + crossover_a - b_local) / crossover_a) + ((b_local - 1) / crossover_a) / b_ratio;
              //
              // Recurse down to a_local, compare values and re-normalise first and second:
              //
              boost::math::detail::hypergeometric_1F1_recurrence_a_coefficients<T> a_coef(crossover_a, b_local, x);
              int backwards_scale = 0;
              T comparitor = boost::math::tools::apply_recurrence_relation_backward(a_coef, crossover_shift, second, first, &backwards_scale);
              log_scaling -= backwards_scale;
              if ((h < 1) && (tools::max_value<T>() * h > comparitor))
              {
                 // Need to rescale!
                 int scale = itrunc(log(h), pol) + 1;
                 h *= exp(T(-scale));
                 log_scaling += scale;
              }
              comparitor /= h;
              first /= comparitor;
              second /= comparitor;
              //
              // Now we can recurse forwards for the rest of the range:
              //
              if (crossover_shift < a_shift)
              {
                 boost::math::detail::hypergeometric_1F1_recurrence_a_coefficients<T> a_coef_2(crossover_a + 1, b_local, x);
                 h = boost::math::tools::apply_recurrence_relation_forward(a_coef_2, a_shift - crossover_shift - 1, first, second, &log_scaling);
              }
              else
                 h = first;
           }
           else
           {
              //
              // Regular case where forwards iteration is stable right from the start:
              //
              boost::math::detail::hypergeometric_1F1_recurrence_b_coefficients<T> b_coef(a_local, b_local, x);
              boost::uintmax_t max_iter = boost::math::policies::get_max_series_iterations<Policy>();
              T b_ratio = boost::math::tools::function_ratio_from_backwards_recurrence(b_coef, boost::math::policies::get_epsilon<T, Policy>(), max_iter);
              boost::math::policies::check_series_iterations<T>("boost::math::hypergeometric_1F1_large_abz<%1%>(%1%,%1%,%1%)", max_iter, pol);
              //
              // Convert to a ratio:
              //         (1+a-b)M(a, b, z) - aM(a+1, b, z) + (b-1)M(a, b-1, z) = 0
              //
              //  hence: M(a+1,b,z) = ((1+a-b) / a) M(a,b,z) + ((b-1) / a) M(a,b,z)/b_ratio
              //
              T second = ((1 + a_local - b_local) / a_local) * h + ((b_local - 1) / a_local) * h / b_ratio;
              boost::math::detail::hypergeometric_1F1_recurrence_a_coefficients<T> a_coef(a_local + 1, b_local, x);
              h = boost::math::tools::apply_recurrence_relation_forward(a_coef, --a_shift, h, second, &log_scaling);
           }
        }
        else
        {
           //
           // We've calculated h for a larger value of a than we want, and need to recurse down.
           // However, only forward iteration is stable, so calculate the ratio, compare values,
           // and normalise.  Note that we calculate the ratio on b and convert to a since the
           // direction is the minimal solution for N->+INF.
           //
           // IMPORTANT: this is only currently called for a > b and therefore forwards iteration
           // is the only stable direction as we will only iterate down until a ~ b, but we
           // will check this with an assert:
           //
           BOOST_ASSERT(2 * a - b_local + x > 0);
           boost::math::detail::hypergeometric_1F1_recurrence_b_coefficients<T> b_coef(a, b_local, x);
           boost::uintmax_t max_iter = boost::math::policies::get_max_series_iterations<Policy>();
           T b_ratio = boost::math::tools::function_ratio_from_backwards_recurrence(b_coef, boost::math::policies::get_epsilon<T, Policy>(), max_iter);
           boost::math::policies::check_series_iterations<T>("boost::math::hypergeometric_1F1_large_abz<%1%>(%1%,%1%,%1%)", max_iter, pol);
           //
           // Convert to a ratio:
           //         (1+a-b)M(a, b, z) - aM(a+1, b, z) + (b-1)M(a, b-1, z) = 0
           //
           //  hence: M(a+1,b,z) = (1+a-b) / a M(a,b,z) + (b-1) / a M(a,b,z)/ (M(a,b,z)/M(a,b-1,z))
           //
           T first = 1;  // arbitrary value;
           T second = ((1 + a - b_local) / a) + ((b_local - 1) / a) * (1 / b_ratio);

           if (a_shift == -1)
              h = h / second;
           else
           {
              boost::math::detail::hypergeometric_1F1_recurrence_a_coefficients<T> a_coef(a + 1, b_local, x);
              T comparitor = boost::math::tools::apply_recurrence_relation_forward(a_coef, -(a_shift + 1), first, second);
              if (boost::math::tools::min_value<T>() * comparitor > h)
              {
                 // Ooops, need to rescale h:
                 int rescale = itrunc(log(fabs(h)));
                 T scale = exp(T(-rescale));
                 h *= scale;
                 log_scaling += rescale;
              }
              h = h / comparitor;
           }
        }
        return h;
     }

     template <class T, class Policy>
     T hypergeometric_1F1_shift_on_b(T h, const T& a, const T& b_local, const T& x, int b_shift, const Policy& pol, int& log_scaling)
     {
        BOOST_MATH_STD_USING

        T b = b_local + b_shift;
        if (b_shift == 0)
           return h;
        else if (b_shift > 0)
        {
           //
           // We get here for b_shift > 0 when b > z.  We can't use forward recursion on b - it's unstable,
           // so grab the ratio and work backwards to b - b_shift and normalise.
           //
           boost::math::detail::hypergeometric_1F1_recurrence_b_coefficients<T> b_coef(a, b, x);
           boost::uintmax_t max_iter = boost::math::policies::get_max_series_iterations<Policy>();

           T first = 1;  // arbitrary value;
           T second = 1 / boost::math::tools::function_ratio_from_backwards_recurrence(b_coef, boost::math::policies::get_epsilon<T, Policy>(), max_iter);
           boost::math::policies::check_series_iterations<T>("boost::math::hypergeometric_1F1_large_abz<%1%>(%1%,%1%,%1%)", max_iter, pol);
           if (b_shift == 1)
              h = h / second;
           else
           {
              //
              // Reset coefficients and recurse:
              //
              boost::math::detail::hypergeometric_1F1_recurrence_b_coefficients<T> b_coef_2(a, b - 1, x);
              int local_scale = 0;
              T comparitor = boost::math::tools::apply_recurrence_relation_backward(b_coef_2, --b_shift, first, second, &local_scale);
              log_scaling -= local_scale;
              if (boost::math::tools::min_value<T>() * comparitor > h)
              {
                 // Ooops, need to rescale h:
                 int rescale = itrunc(log(fabs(h)));
                 T scale = exp(T(-rescale));
                 h *= scale;
                 log_scaling += rescale;
              }
              h = h / comparitor;
           }
        }
        else
        {
           T second;
           if (a == b_local)
           {
               // recurrence is trivial for a == b and method of ratios fails as the c-term goes to zero:
              second = -b_local * (1 - b_local - x) * h / (b_local * (b_local - 1));
           }
           else
           {
              BOOST_ASSERT(!is_negative_integer(b - a));
              boost::math::detail::hypergeometric_1F1_recurrence_b_coefficients<T> b_coef(a, b_local, x);
              boost::uintmax_t max_iter = boost::math::policies::get_max_series_iterations<Policy>();
              second = h / boost::math::tools::function_ratio_from_backwards_recurrence(b_coef, boost::math::policies::get_epsilon<T, Policy>(), max_iter);
              boost::math::policies::check_series_iterations<T>("boost::math::hypergeometric_1F1_large_abz<%1%>(%1%,%1%,%1%)", max_iter, pol);
           }
           if (b_shift == -1)
              h = second;
           else
           {
              boost::math::detail::hypergeometric_1F1_recurrence_b_coefficients<T> b_coef_2(a, b_local - 1, x);
              h = boost::math::tools::apply_recurrence_relation_backward(b_coef_2, -(++b_shift), h, second, &log_scaling);
           }
        }
        return h;
     }

     template <class T, class Policy>
     T hypergeometric_1F1_large_igamma(const T& a, const T& b, const T& x, const T& b_minus_a, const Policy& pol, int& log_scaling)
     {
        BOOST_MATH_STD_USING
        //
        // We need a < b < z in order to ensure there's at least a chance of convergence,
        // we can use recurrence relations to shift forwards on a+b or just a to achieve this,
        // for decent accuracy, try to keep 2b - 1 < a < 2b < z
        //
        int b_shift = b * 2 < x ? 0 : itrunc(b - x / 2);
        int a_shift = a > b - b_shift ? -itrunc(b - b_shift - a - 1) : -itrunc(b - b_shift - a);

        if (a_shift < 0)
        {
           // Might as well do all the shifting on b as scale a downwards:
           b_shift -= a_shift;
           a_shift = 0;
        }

        T a_local = a - a_shift;
        T b_local = b - b_shift;
        T b_minus_a_local = (a_shift == 0) && (b_shift == 0) ? b_minus_a : b_local - a_local;
        int local_scaling = 0;
        T h = hypergeometric_1F1_igamma(a_local, b_local, x, b_minus_a_local, pol, local_scaling);
        log_scaling += local_scaling;

        //
        // Apply shifts on a and b as required:
        //
        h = hypergeometric_1F1_shift_on_a(h, a_local, b_local, x, a_shift, pol, log_scaling);
        h = hypergeometric_1F1_shift_on_b(h, a, b_local, x, b_shift, pol, log_scaling);

        return h;
     }

     template <class T, class Policy>
     T hypergeometric_1F1_large_series(const T& a, const T& b, const T& z, const Policy& pol, int& log_scaling)
     {
        BOOST_MATH_STD_USING
        //
        // We make a small, and b > z:
        //
        int a_shift(0), b_shift(0);
        if (a * z > b)
        {
           a_shift = itrunc(a) - 5;
           b_shift = b < z ? itrunc(b - z - 1) : 0;
        }
        //
        // If a_shift is trivially small, there's really not much point in losing
        // accuracy to save a couple of iterations:
        //
        if (a_shift < 5)
           a_shift = 0;
        T a_local = a - a_shift;
        T b_local = b - b_shift;
        T h = boost::math::detail::hypergeometric_1F1_generic_series(a_local, b_local, z, pol, log_scaling, "hypergeometric_1F1_large_series<%1%>(a,b,z)");
        //
        // Apply shifts on a and b as required:
        //
        if (a_shift && (a_local == 0))
        {
           //
           // Shifting on a via method of ratios in hypergeometric_1F1_shift_on_a fails when
           // a_local == 0.  However, the value of h calculated was trivial (unity), so
           // calculate a second 1F1 for a == 1 and recurse as normal:
           //
           int scale = 0;
           T h2 = boost::math::detail::hypergeometric_1F1_generic_series(T(a_local + 1), b_local, z, pol, scale, "hypergeometric_1F1_large_series<%1%>(a,b,z)");
           if (scale != log_scaling)
           {
              h2 *= exp(T(scale - log_scaling));
           }
           boost::math::detail::hypergeometric_1F1_recurrence_a_coefficients<T> coef(a_local + 1, b_local, z);
           h = boost::math::tools::apply_recurrence_relation_forward(coef, a_shift - 1, h, h2, &log_scaling);
           h = hypergeometric_1F1_shift_on_b(h, a, b_local, z, b_shift, pol, log_scaling);
        }
        else
        {
           h = hypergeometric_1F1_shift_on_a(h, a_local, b_local, z, a_shift, pol, log_scaling);
           h = hypergeometric_1F1_shift_on_b(h, a, b_local, z, b_shift, pol, log_scaling);
        }
        return h;
     }

     template <class T, class Policy>
     T hypergeometric_1F1_large_13_3_6_series(const T& a, const T& b, const T& z, const Policy& pol, int& log_scaling)
     {
        BOOST_MATH_STD_USING
        //
        // A&S 13.3.6 is good only when a ~ b, but isn't too fussy on the size of z.
        // So shift b to match a (b shifting seems to be more stable via method of ratios).
        //
        int b_shift = itrunc(b - a);
        T b_local = b - b_shift;
        T h = boost::math::detail::hypergeometric_1F1_AS_13_3_6(a, b_local, z, T(b_local - a), pol, log_scaling);
        return hypergeometric_1F1_shift_on_b(h, a, b_local, z, b_shift, pol, log_scaling);
     }

     template <class T, class Policy>
     T hypergeometric_1F1_large_abz(const T& a, const T& b, const T& z, const Policy& pol, int& log_scaling)
     {
        BOOST_MATH_STD_USING
        //
        // This is the selection logic to pick the "best" method.
        // We have a,b,z >> 0 and need to compute the approximate cost of each method
        // and then select whichever wins out.
        //
        enum method
        {
           method_series = 0,
           method_shifted_series,
           method_gamma,
           method_bessel
        };
        //
        // Cost of direct series, is the approx number of terms required until we hit convergence:
        //
        T current_cost = (sqrt(16 * z * (3 * a + z) + 9 * b * b - 24 * b * z) - 3 * b + 4 * z) / 6;
        method current_method = method_series;
        //
        // Cost of shifted series, is the number of recurrences required to move to a zone where
        // the series is convergent right from the start.
        // Note that recurrence relations fail for very small b, and too many recurrences on a
        // will completely destroy all our digits.
        // Also note that the method fails when b-a is a negative integer unless b is already
        // larger than z and thus does not need shifting.
        //
        T cost = a + ((b < z) ? T(z - b) : T(0));
        if((b > 1) && (cost < current_cost) && ((b > z) || !is_negative_integer(b-a)))
        {
           current_method = method_shifted_series;
           current_cost = cost;
        }
        //
        // Cost for gamma function method is the number of recurrences required to move it
        // into a convergent zone, note that recurrence relations fail for very small b.
        // Also add on a fudge factor to account for the fact that this method is both
        // more expensive to compute (requires gamma functions), and less accurate than the
        // methods above:
        //
        T b_shift = fabs(b * 2 < z ? T(0) : T(b - z / 2));
        T a_shift = fabs(a > b - b_shift ? T(-(b - b_shift - a - 1)) : T(-(b - b_shift - a)));
        cost = 1000 + b_shift + a_shift;
        if((b > 1) && (cost <= current_cost))
        {
           current_method = method_gamma;
           current_cost = cost;
        }
        //
        // Cost for bessel approximation is the number of recurrences required to make a ~ b,
        // Note that recurrence relations fail for very small b.  We also have issue with large
        // z: either overflow/numeric instability or else the series goes divergent.  We seem to be
        // OK for z smaller than log_max_value<Quad> though, maybe we can stretch a little further
        // but that's not clear...
        // Also need to add on a fudge factor to the cost to account for the fact that we need
        // to calculate the Bessel functions, this is not quite as high as the gamma function
        // method above as this is generally more accurate and so preferred if the methods are close:
        //
        cost = 50 + fabs(b - a);
        if((b > 1) && (cost <= current_cost) && (z < tools::log_max_value<T>()) && (z < 11356) && (b - a != 0.5f))
        {
           current_method = method_bessel;
           current_cost = cost;
        }

        switch (current_method)
        {
        case method_series:
           return detail::hypergeometric_1F1_generic_series(a, b, z, pol, log_scaling, "hypergeometric_1f1_large_abz<%1%>(a,b,z)");
        case method_shifted_series:
           return detail::hypergeometric_1F1_large_series(a, b, z, pol, log_scaling);
        case method_gamma:
           return detail::hypergeometric_1F1_large_igamma(a, b, z, T(b - a), pol, log_scaling);
        case method_bessel:
           return detail::hypergeometric_1F1_large_13_3_6_series(a, b, z, pol, log_scaling);
        }
        return 0; // We don't get here!
     }

  } } } // namespaces

#endif // BOOST_HYPERGEOMETRIC_1F1_LARGE_ABZ_HPP_