// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2015 Alec Jacobson <alecjacobson@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 "order_facets_around_edges.h"
#include "order_facets_around_edge.h"
#include "../../sort_angles.h"
#include <Eigen/Geometry>
#include <type_traits>
#include <CGAL/Exact_predicates_exact_constructions_kernel.h>

template<
    typename DerivedV,
    typename DerivedF,
    typename DerivedN,
    typename DeriveduE,
    typename uE2EType,
    typename uE2oEType,
    typename uE2CType >
IGL_INLINE
typename std::enable_if<!std::is_same<typename DerivedV::Scalar,
typename CGAL::Exact_predicates_exact_constructions_kernel::FT>::value, void>::type
igl::copyleft::cgal::order_facets_around_edges(
        const Eigen::PlainObjectBase<DerivedV>& V,
        const Eigen::PlainObjectBase<DerivedF>& F,
        const Eigen::PlainObjectBase<DerivedN>& N,
        const Eigen::PlainObjectBase<DeriveduE>& uE,
        const std::vector<std::vector<uE2EType> >& uE2E,
        std::vector<std::vector<uE2oEType> >& uE2oE,
        std::vector<std::vector<uE2CType > >& uE2C ) {

    typedef Eigen::Matrix<typename DerivedN::Scalar, 3, 1> Vector3F;
    const typename DerivedV::Scalar EPS = 1e-12;
    const size_t num_faces = F.rows();
    const size_t num_undirected_edges = uE.rows();

    auto edge_index_to_face_index = [&](size_t ei) { return ei % num_faces; };
    auto edge_index_to_corner_index = [&](size_t ei) { return ei / num_faces; };

    uE2oE.resize(num_undirected_edges);
    uE2C.resize(num_undirected_edges);

    for(size_t ui = 0;ui<num_undirected_edges;ui++)
    {
        const auto& adj_edges = uE2E[ui];
        const size_t edge_valance = adj_edges.size();
        assert(edge_valance > 0);

        const auto ref_edge = adj_edges[0];
        const auto ref_face = edge_index_to_face_index(ref_edge);
        Vector3F ref_normal = N.row(ref_face);

        const auto ref_corner_o = edge_index_to_corner_index(ref_edge);
        const auto ref_corner_s = (ref_corner_o+1)%3;
        const auto ref_corner_d = (ref_corner_o+2)%3;

        const typename DerivedF::Scalar o = F(ref_face, ref_corner_o);
        const typename DerivedF::Scalar s = F(ref_face, ref_corner_s);
        const typename DerivedF::Scalar d = F(ref_face, ref_corner_d);

        Vector3F edge = V.row(d) - V.row(s);
        auto edge_len = edge.norm();
        bool degenerated = edge_len < EPS;
        if (degenerated) {
            if (edge_valance <= 2) {
                // There is only one way to order 2 or less faces.
                edge.setZero();
            } else {
                edge.setZero();
                Eigen::Matrix<typename DerivedN::Scalar, Eigen::Dynamic, 3>
                    normals(edge_valance, 3);
                for (size_t fei=0; fei<edge_valance; fei++) {
                    const auto fe = adj_edges[fei];
                    const auto f = edge_index_to_face_index(fe);
                    normals.row(fei) = N.row(f);
                }
                for (size_t i=0; i<edge_valance; i++) {
                    size_t j = (i+1) % edge_valance;
                    Vector3F ni = normals.row(i);
                    Vector3F nj = normals.row(j);
                    edge = ni.cross(nj);
                    edge_len = edge.norm();
                    if (edge_len >= EPS) {
                        edge.normalize();
                        break;
                    }
                }

                // Ensure edge direction are consistent with reference face.
                Vector3F in_face_vec = V.row(o) - V.row(s);
                if (edge.cross(in_face_vec).dot(ref_normal) < 0) {
                    edge *= -1;
                }

                if (edge.norm() < EPS) {
                    std::cerr << "=====================================" << std::endl;
                    std::cerr << "  ui: " << ui << std::endl;
                    std::cerr << "edge: " << ref_edge << std::endl;
                    std::cerr << "face: " << ref_face << std::endl;
                    std::cerr << "  vs: " << V.row(s) << std::endl;
                    std::cerr << "  vd: " << V.row(d) << std::endl;
                    std::cerr << "adj face normals: " << std::endl;
                    std::cerr << normals << std::endl;
                    std::cerr << "Very degenerated case detected:" << std::endl;
                    std::cerr << "Near zero edge surrounded by "
                        << edge_valance << " neearly colinear faces" <<
                        std::endl;
                    std::cerr << "=====================================" << std::endl;
                }
            }
        } else {
            edge.normalize();
        }

        Eigen::MatrixXd angle_data(edge_valance, 3);
        std::vector<bool> cons(edge_valance);

        for (size_t fei=0; fei<edge_valance; fei++) {
            const auto fe = adj_edges[fei];
            const auto f = edge_index_to_face_index(fe);
            const auto c = edge_index_to_corner_index(fe);
            cons[fei] = (d == F(f, (c+1)%3));
            assert( cons[fei] ||  (d == F(f,(c+2)%3)));
            assert(!cons[fei] || (s == F(f,(c+2)%3)));
            assert(!cons[fei] || (d == F(f,(c+1)%3)));
            Vector3F n = N.row(f);
            angle_data(fei, 0) = ref_normal.cross(n).dot(edge);
            angle_data(fei, 1) = ref_normal.dot(n);
            if (cons[fei]) {
                angle_data(fei, 0) *= -1;
                angle_data(fei, 1) *= -1;
            }
            angle_data(fei, 0) *= -1; // Sort clockwise.
            angle_data(fei, 2) = (cons[fei]?1.:-1.)*(f+1);
        }

        Eigen::VectorXi order;
        igl::sort_angles(angle_data, order);

        auto& ordered_edges = uE2oE[ui];
        auto& consistency = uE2C[ui];

        ordered_edges.resize(edge_valance);
        consistency.resize(edge_valance);
        for (size_t fei=0; fei<edge_valance; fei++) {
            ordered_edges[fei] = adj_edges[order[fei]];
            consistency[fei] = cons[order[fei]];
        }
    }
}

template<
    typename DerivedV,
    typename DerivedF,
    typename DerivedN,
    typename DeriveduE,
    typename uE2EType,
    typename uE2oEType,
    typename uE2CType >
IGL_INLINE
typename std::enable_if<std::is_same<typename DerivedV::Scalar,
typename CGAL::Exact_predicates_exact_constructions_kernel::FT>::value, void>::type
igl::copyleft::cgal::order_facets_around_edges(
        const Eigen::PlainObjectBase<DerivedV>& V,
        const Eigen::PlainObjectBase<DerivedF>& F,
        const Eigen::PlainObjectBase<DerivedN>& N,
        const Eigen::PlainObjectBase<DeriveduE>& uE,
        const std::vector<std::vector<uE2EType> >& uE2E,
        std::vector<std::vector<uE2oEType> >& uE2oE,
        std::vector<std::vector<uE2CType > >& uE2C ) {

    typedef Eigen::Matrix<typename DerivedN::Scalar, 3, 1> Vector3F;
    typedef Eigen::Matrix<typename DerivedV::Scalar, 3, 1> Vector3E;
    const typename DerivedV::Scalar EPS = 1e-12;
    const size_t num_faces = F.rows();
    const size_t num_undirected_edges = uE.rows();

    auto edge_index_to_face_index = [&](size_t ei) { return ei % num_faces; };
    auto edge_index_to_corner_index = [&](size_t ei) { return ei / num_faces; };

    uE2oE.resize(num_undirected_edges);
    uE2C.resize(num_undirected_edges);

    for(size_t ui = 0;ui<num_undirected_edges;ui++)
    {
        const auto& adj_edges = uE2E[ui];
        const size_t edge_valance = adj_edges.size();
        assert(edge_valance > 0);

        const auto ref_edge = adj_edges[0];
        const auto ref_face = edge_index_to_face_index(ref_edge);
        Vector3F ref_normal = N.row(ref_face);

        const auto ref_corner_o = edge_index_to_corner_index(ref_edge);
        const auto ref_corner_s = (ref_corner_o+1)%3;
        const auto ref_corner_d = (ref_corner_o+2)%3;

        const typename DerivedF::Scalar o = F(ref_face, ref_corner_o);
        const typename DerivedF::Scalar s = F(ref_face, ref_corner_s);
        const typename DerivedF::Scalar d = F(ref_face, ref_corner_d);

        Vector3E exact_edge = V.row(d) - V.row(s);
        exact_edge.array() /= exact_edge.squaredNorm();
        Vector3F edge(
                CGAL::to_double(exact_edge[0]),
                CGAL::to_double(exact_edge[1]),
                CGAL::to_double(exact_edge[2]));
        edge.normalize();

        Eigen::MatrixXd angle_data(edge_valance, 3);
        std::vector<bool> cons(edge_valance);

        for (size_t fei=0; fei<edge_valance; fei++) {
            const auto fe = adj_edges[fei];
            const auto f = edge_index_to_face_index(fe);
            const auto c = edge_index_to_corner_index(fe);
            cons[fei] = (d == F(f, (c+1)%3));
            assert( cons[fei] ||  (d == F(f,(c+2)%3)));
            assert(!cons[fei] || (s == F(f,(c+2)%3)));
            assert(!cons[fei] || (d == F(f,(c+1)%3)));
            Vector3F n = N.row(f);
            angle_data(fei, 0) = ref_normal.cross(n).dot(edge);
            angle_data(fei, 1) = ref_normal.dot(n);
            if (cons[fei]) {
                angle_data(fei, 0) *= -1;
                angle_data(fei, 1) *= -1;
            }
            angle_data(fei, 0) *= -1; // Sort clockwise.
            angle_data(fei, 2) = (cons[fei]?1.:-1.)*(f+1);
        }

        Eigen::VectorXi order;
        igl::sort_angles(angle_data, order);

        auto& ordered_edges = uE2oE[ui];
        auto& consistency = uE2C[ui];

        ordered_edges.resize(edge_valance);
        consistency.resize(edge_valance);
        for (size_t fei=0; fei<edge_valance; fei++) {
            ordered_edges[fei] = adj_edges[order[fei]];
            consistency[fei] = cons[order[fei]];
        }
    }
}

template<
    typename DerivedV,
    typename DerivedF,
    typename DeriveduE,
    typename uE2EType,
    typename uE2oEType,
    typename uE2CType >
IGL_INLINE void igl::copyleft::cgal::order_facets_around_edges(
        const Eigen::PlainObjectBase<DerivedV>& V,
        const Eigen::PlainObjectBase<DerivedF>& F,
        const Eigen::PlainObjectBase<DeriveduE>& uE,
        const std::vector<std::vector<uE2EType> >& uE2E,
        std::vector<std::vector<uE2oEType> >& uE2oE,
        std::vector<std::vector<uE2CType > >& uE2C ) {

    //typedef Eigen::Matrix<typename DerivedV::Scalar, 3, 1> Vector3E;
    const size_t num_faces = F.rows();
    const size_t num_undirected_edges = uE.rows();

    auto edge_index_to_face_index = [&](size_t ei) { return ei % num_faces; };
    auto edge_index_to_corner_index = [&](size_t ei) { return ei / num_faces; };

    uE2oE.resize(num_undirected_edges);
    uE2C.resize(num_undirected_edges);

    for(size_t ui = 0;ui<num_undirected_edges;ui++)
    {
        const auto& adj_edges = uE2E[ui];
        const size_t edge_valance = adj_edges.size();
        assert(edge_valance > 0);

        const auto ref_edge = adj_edges[0];
        const auto ref_face = edge_index_to_face_index(ref_edge);

        const auto ref_corner_o = edge_index_to_corner_index(ref_edge);
        const auto ref_corner_s = (ref_corner_o+1)%3;
        const auto ref_corner_d = (ref_corner_o+2)%3;

        //const typename DerivedF::Scalar o = F(ref_face, ref_corner_o);
        const typename DerivedF::Scalar s = F(ref_face, ref_corner_s);
        const typename DerivedF::Scalar d = F(ref_face, ref_corner_d);

        std::vector<bool> cons(edge_valance);
        std::vector<int> adj_faces(edge_valance);
        for (size_t fei=0; fei<edge_valance; fei++) {
            const auto fe = adj_edges[fei];
            const auto f = edge_index_to_face_index(fe);
            const auto c = edge_index_to_corner_index(fe);
            cons[fei] = (d == F(f, (c+1)%3));
            adj_faces[fei] = (f+1) * (cons[fei] ? 1:-1);

            assert( cons[fei] ||  (d == F(f,(c+2)%3)));
            assert(!cons[fei] || (s == F(f,(c+2)%3)));
            assert(!cons[fei] || (d == F(f,(c+1)%3)));
        }

        Eigen::VectorXi order;
        order_facets_around_edge(V, F, s, d, adj_faces, order);
        assert((size_t)order.size() == edge_valance);

        auto& ordered_edges = uE2oE[ui];
        auto& consistency = uE2C[ui];

        ordered_edges.resize(edge_valance);
        consistency.resize(edge_valance);
        for (size_t fei=0; fei<edge_valance; fei++) {
            ordered_edges[fei] = adj_edges[order[fei]];
            consistency[fei] = cons[order[fei]];
        }
    }
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
// generated by autoexplicit.sh
template void igl::copyleft::cgal::order_facets_around_edges<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, int, int, bool>(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, std::vector<std::vector<int, std::allocator<int> >, std::allocator<std::vector<int, std::allocator<int> > > > const&, std::vector<std::vector<int, std::allocator<int> >, std::allocator<std::vector<int, std::allocator<int> > > >&, std::vector<std::vector<bool, std::allocator<bool> >, std::allocator<std::vector<bool, std::allocator<bool> > > >&);
// generated by autoexplicit.sh
template std::enable_if<!(std::is_same<Eigen::Matrix<double, -1, -1, 0, -1, -1>::Scalar, CGAL::Lazy_exact_nt<CGAL::Gmpq> >::value), void>::type igl::copyleft::cgal::order_facets_around_edges<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, int, int, bool>(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, std::vector<std::vector<int, std::allocator<int> >, std::allocator<std::vector<int, std::allocator<int> > > > const&, std::vector<std::vector<int, std::allocator<int> >, std::allocator<std::vector<int, std::allocator<int> > > >&, std::vector<std::vector<bool, std::allocator<bool> >, std::allocator<std::vector<bool, std::allocator<bool> > > >&);
template void igl::copyleft::cgal::order_facets_around_edges<Eigen::Matrix<CGAL::Lazy_exact_nt<CGAL::Gmpq>, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 2, 0, -1, 2>, long, long, bool>(Eigen::PlainObjectBase<Eigen::Matrix<CGAL::Lazy_exact_nt<CGAL::Gmpq>, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 2, 0, -1, 2> > const&, std::vector<std::vector<long, std::allocator<long> >, std::allocator<std::vector<long, std::allocator<long> > > > const&, std::vector<std::vector<long, std::allocator<long> >, std::allocator<std::vector<long, std::allocator<long> > > >&, std::vector<std::vector<bool, std::allocator<bool> >, std::allocator<std::vector<bool, std::allocator<bool> > > >&);
template void igl::copyleft::cgal::order_facets_around_edges<Eigen::Matrix<double, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 2, 0, -1, 2>, long, long, bool>(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 2, 0, -1, 2> > const&, std::vector<std::vector<long, std::allocator<long> >, std::allocator<std::vector<long, std::allocator<long> > > > const&, std::vector<std::vector<long, std::allocator<long> >, std::allocator<std::vector<long, std::allocator<long> > > >&, std::vector<std::vector<bool, std::allocator<bool> >, std::allocator<std::vector<bool, std::allocator<bool> > > >&);
template void igl::copyleft::cgal::order_facets_around_edges<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, 2, 0, -1, 2>, long, long, bool>(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 2, 0, -1, 2> > const&, std::vector<std::vector<long, std::allocator<long> >, std::allocator<std::vector<long, std::allocator<long> > > > const&, std::vector<std::vector<long, std::allocator<long> >, std::allocator<std::vector<long, std::allocator<long> > > >&, std::vector<std::vector<bool, std::allocator<bool> >, std::allocator<std::vector<bool, std::allocator<bool> > > >&);
template void igl::copyleft::cgal::order_facets_around_edges<Eigen::Matrix<CGAL::Lazy_exact_nt<CGAL::Gmpq>, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, 2, 0, -1, 2>, long, long, bool>(Eigen::PlainObjectBase<Eigen::Matrix<CGAL::Lazy_exact_nt<CGAL::Gmpq>, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 2, 0, -1, 2> > const&, std::vector<std::vector<long, std::allocator<long> >, std::allocator<std::vector<long, std::allocator<long> > > > const&, std::vector<std::vector<long, std::allocator<long> >, std::allocator<std::vector<long, std::allocator<long> > > >&, std::vector<std::vector<bool, std::allocator<bool> >, std::allocator<std::vector<bool, std::allocator<bool> > > >&);
#ifdef WIN32
template void igl::copyleft::cgal::order_facets_around_edges<class Eigen::Matrix<class CGAL::Lazy_exact_nt<class CGAL::Gmpq>,-1,-1,0,-1,-1>,class Eigen::Matrix<int,-1,-1,0,-1,-1>,class Eigen::Matrix<int,-1,2,0,-1,2>,__int64,__int64,bool>(class Eigen::PlainObjectBase<class Eigen::Matrix<class CGAL::Lazy_exact_nt<class CGAL::Gmpq>,-1,-1,0,-1,-1> > const &,class Eigen::PlainObjectBase<class Eigen::Matrix<int,-1,-1,0,-1,-1> > const &,class Eigen::PlainObjectBase<class Eigen::Matrix<int,-1,2,0,-1,2> > const &,class std::vector<class std::vector<__int64,class std::allocator<__int64> >,class std::allocator<class std::vector<__int64,class std::allocator<__int64> > > > const &,class std::vector<class std::vector<__int64,class std::allocator<__int64> >,class std::allocator<class std::vector<__int64,class std::allocator<__int64> > > > &,class std::vector<class std::vector<bool,class std::allocator<bool> >,class std::allocator<class std::vector<bool,class std::allocator<bool> > > > &);
template void igl::copyleft::cgal::order_facets_around_edges<class Eigen::Matrix<double,-1,-1,0,-1,-1>,class Eigen::Matrix<int,-1,-1,0,-1,-1>,class Eigen::Matrix<int,-1,2,0,-1,2>,__int64,__int64,bool>(class Eigen::PlainObjectBase<class Eigen::Matrix<double,-1,-1,0,-1,-1> > const &,class Eigen::PlainObjectBase<class Eigen::Matrix<int,-1,-1,0,-1,-1> > const &,class Eigen::PlainObjectBase<class Eigen::Matrix<int,-1,2,0,-1,2> > const &,class std::vector<class std::vector<__int64,class std::allocator<__int64> >,class std::allocator<class std::vector<__int64,class std::allocator<__int64> > > > const &,class std::vector<class std::vector<__int64,class std::allocator<__int64> >,class std::allocator<class std::vector<__int64,class std::allocator<__int64> > > > &,class std::vector<class std::vector<bool,class std::allocator<bool> >,class std::allocator<class std::vector<bool,class std::allocator<bool> > > > &);
template void igl::copyleft::cgal::order_facets_around_edges<class Eigen::Matrix<double,-1,3,0,-1,3>,class Eigen::Matrix<int,-1,3,0,-1,3>,class Eigen::Matrix<int,-1,2,0,-1,2>,__int64,__int64,bool>(class Eigen::PlainObjectBase<class Eigen::Matrix<double,-1,3,0,-1,3> > const &,class Eigen::PlainObjectBase<class Eigen::Matrix<int,-1,3,0,-1,3> > const &,class Eigen::PlainObjectBase<class Eigen::Matrix<int,-1,2,0,-1,2> > const &,class std::vector<class std::vector<__int64,class std::allocator<__int64> >,class std::allocator<class std::vector<__int64,class std::allocator<__int64> > > > const &,class std::vector<class std::vector<__int64,class std::allocator<__int64> >,class std::allocator<class std::vector<__int64,class std::allocator<__int64> > > > &,class std::vector<class std::vector<bool,class std::allocator<bool> >,class std::allocator<class std::vector<bool,class std::allocator<bool> > > > &);
#endif
#endif


//          Copyright Gavin Band 2008 - 2012.
// 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 <string>
#include <iostream>
#include <limits>
#include <typeinfo>
#include <fstream>
#include <unordered_map>

#include <boost/filesystem.hpp>
#include <boost/format.hpp>
#include <boost/math/distributions/normal.hpp>
#include <boost/math/distributions/chi_squared.hpp>
#include <boost/timer/timer.hpp>
#include <boost/ptr_container/ptr_vector.hpp>
#include <boost/bind.hpp>
#include <boost/foreach.hpp>
#include <boost/iterator/counting_iterator.hpp>

#include "config/package_revision_autogenerated.hpp"

#include "appcontext/CmdLineOptionProcessor.hpp"
#include "appcontext/ApplicationContext.hpp"

#include "genfile/SNPDataSource.hpp"
#include "genfile/SNPDataSourceChain.hpp"
#include "genfile/ThreshholdingSNPDataSource.hpp"
#include "genfile/ToGP.hpp"
#include "genfile/CommonSNPFilter.hpp"
#include "genfile/VariantIdentifyingDataFilteringSNPDataSource.hpp"
#include "genfile/CrossCohortCovariateValueMapping.hpp"
#include "genfile/SampleFilter.hpp"
#include "genfile/SampleFilteringSNPDataSource.hpp"
#include "genfile/SampleFilteringCohortIndividualSource.hpp"
#include "genfile/CompoundSampleFilter.hpp"
#include "genfile/SampleFilterNegation.hpp"
#include "genfile/VariableInSetSampleFilter.hpp"
#include "genfile/GPThresholdingGTSetter.hpp"
#include "genfile/db/Error.hpp"
#include "genfile/CartesianProductVisitor.hpp"

#include "metro/constants.hpp"
#include "metro/regression/Design.hpp"
#include "metro/SampleRange.hpp"
#include "metro/intersect_ranges.hpp"
#include "metro/count_range.hpp"
#include "metro/regression/BinomialLogistic.hpp"
#include "metro/regression/ThreadedLogLikelihood.hpp"
#include "metro/regression/IndependentNormalWeightedLogLikelihood.hpp"
#include "metro/regression/IndependentLogFWeightedLogLikelihood.hpp"
#include "metro/regression/LogPosteriorDensity.hpp"
#include "metro/ValueStabilisesStoppingCondition.hpp"
#include "metro/Snptest25StoppingCondition.hpp"
#include "metro/maximisation.hpp"
#include "metro/fit_model.hpp"
#include "metro/CholeskyStepper.hpp"
#include "metro/FishersExactTest.hpp"

#include "qcdb/MultiVariantStorage.hpp"
#include "qcdb/FlatTableDBOutputter.hpp"

#include <Eigen/Core>
#include <Eigen/QR>
#include "Eigen/Eigenvalues"

// #define DEBUG 1

namespace globals {
	std::string const program_name = "ldbird" ;
	std::string const program_version = package_version ;
	std::string const program_revision =  std::string( package_revision ).substr( 0, 7 ) ;
}

namespace {
	std::vector< std::string > collect_unique_ids( std::vector< std::string > const& ids_or_filenames ) {
		std::vector< std::string > result ;
		for( auto elt: ids_or_filenames ) {
			if( boost::filesystem::exists( elt )) {
				std::ifstream f( elt ) ;
				std::copy(
					std::istream_iterator< std::string >( f ),
					std::istream_iterator< std::string >(),
					std::back_inserter< std::vector< std::string > >( result )
				) ;
			} else {
				result.push_back( elt ) ;
			}
		}
		// now sort and uniqueify them...
		std::sort( result.begin(), result.end() ) ;
		std::vector< std::string >::const_iterator newBack = std::unique( result.begin(), result.end() ) ;
		result.resize( newBack - result.begin() ) ;
		return result ;
	}

}

struct CorrelatorOptions: public appcontext::CmdLineOptionProcessor
{
public:
	std::string get_program_name() const { return globals::program_name ; }

	void declare_options( appcontext::OptionProcessor& options ) {
		// Meta-options
		options.set_help_option( "-help" ) ;
		options.set_spec_option( "-spec" ) ;

		// File options
		options.declare_group( "Input file options" ) ;
		options[ "-g1" ]
			.set_description( 	"Path to first genotype file."
								"The given filename may contain the wildcard character '#', which expands to match a"
								"one- or two-character chromosome identifier." )
			.set_takes_values( 1 )
			.set_minimum_multiplicity( 1 )
			.set_maximum_multiplicity( 1 ) ;
		options[ "-g1-incl-rsids" ]
			.set_description( "Exclude all SNPs whose RSID is not in the given file(s) from the analysis.")
			.set_takes_values_until_next_option()
			.set_maximum_multiplicity( 100 ) ;
		options[ "-g1-incl-range" ]
			.set_description( "Specify a range of SNPs (or comma-separated list of ranges of SNPs) to operate on. "
				"Each range should be in the format CC:xxxx-yyyy where CC is the chromosome and xxxx and yyyy are the "
				"start and end coordinates, or just xxxx-yyyy which matches that range from all chromosomes. "
				"You can also omit either of xxxx or yyyy to get all SNPs from the start or to the end of a chromosome." )
			.set_takes_values_until_next_option() ;

		options.option_implies_option( "-g1-incl-range", "-g1" ) ;
		options.option_implies_option( "-g1-incl-rsids", "-g1" ) ;

		options[ "-g2" ]
			.set_description( 	"Path to second genotype file.  If not given the first genotype file will be used."
								"The given filename may contain the wildcard character '#', which expands to match a"
								"one- or two-character chromosome identifier." )
			.set_takes_values( 1 )
			.set_minimum_multiplicity( 0 )
			.set_maximum_multiplicity( 1 ) ;

		options[ "-g2-incl-rsids" ]
			.set_description( "Exclude all SNPs whose RSID is not in the given file(s) from the analysis.")
			.set_takes_values_until_next_option()
			.set_maximum_multiplicity( 100 ) ;
		options[ "-g2-incl-range" ]
			.set_description( "Specify a range of SNPs (or comma-separated list of ranges of SNPs) to operate on. "
				"Each range should be in the format CC:xxxx-yyyy where CC is the chromosome and xxxx and yyyy are the "
				"start and end coordinates, or just xxxx-yyyy which matches that range from all chromosomes. "
				"You can also omit either of xxxx or yyyy to get all SNPs from the start or to the end of a chromosome." )
			.set_takes_values_until_next_option() ;

		options.option_implies_option( "-g2-incl-range", "-g2" ) ;
		options.option_implies_option( "-g2-incl-rsids", "-g2" ) ;

		options[ "-s" ]
			.set_description( "Path of sample file" )
			.set_takes_values( 1 )
			.set_minimum_multiplicity( 1 )
			.set_maximum_multiplicity( 1 ) ;

		options.declare_group( "Output file options" ) ;
		options[ "-o" ]
			.set_description( "Output file" )
			.set_takes_values( 1 )
			.set_minimum_multiplicity( 1 )
			.set_maximum_multiplicity( 1 ) ;
		options[ "-details" ]
			.set_description( "Output counts and a Fisher's exact test P-value. "
				"Currently only supported if genotypes are haploid." ) ;

		options.declare_group( "Model options" ) ;
		options[ "-incl-samples"]
			.set_description( "Filter out samples whose sample ID does not lie in the given file(s).")
			.set_takes_values( 1 )
			.set_minimum_multiplicity( 0 )
			.set_maximum_multiplicity( 100 ) ;
		options[ "-excl-samples"]
			.set_description( "Filter out samples whose sample ID lies in the given file.")
			.set_takes_values( 1 )
			.set_minimum_multiplicity( 0 )
			.set_maximum_multiplicity( 100 ) ;
		options[ "-incl-samples-where"]
			.set_description( "Include samples by specifying conditions on the values of columns in the sample file.")
			.set_takes_values( 1 )
			.set_minimum_multiplicity( 0 )
			.set_maximum_multiplicity( 100 ) ;
		options[ "-excl-samples-where"]
			.set_description( "Exclude samples by specifying conditions on the values of columns in the sample file.")
			.set_takes_single_value()
			.set_minimum_multiplicity( 0 )
			.set_maximum_multiplicity( 100 ) ;
		options[ "-min-r2" ]
			.set_description( "Do not output results where r^2 is lower than this threshold. "
					" (These values are still computed and contribute to the histogram.)" )
			.set_takes_single_value()
			.set_default_value( 0.05 ) ;
		options[ "-min-maf" ]
			.set_description( "Do not compute correlation results where the maf of either variant is lower than this threshold." )
			.set_takes_single_value()
			.set_default_value( 0 ) ;
		options[ "-min-N" ]
			.set_description( "Do not compute results where the total number of pairwise non-missing genotypes is below this threshold." )
			.set_takes_single_value()
			.set_default_value( 0 ) ;
		options[ "-min-N-propn" ]
			.set_description( "Ignore (do not compute or output) results where the propn of pairwise non-missing genotypes is below this threshold." )
			.set_takes_single_value()
			.set_default_value( 0.0 ) ;
		options[ "-min-distance" ]
			.set_description( "Ignore (do not compute or output) results where the variants are on the same chromosome and closer than this physical distance." )
			.set_takes_single_value()
			.set_default_value( 0 ) ;
		options[ "-assume-haploid" ]
			.set_description( "Convert all data to haploid calls.  This converts homozygous calls"
				" to haploid calls, and treats any heterozygous calls as missing.")
		;
		options[ "-prior-weight" ]
			.set_description( "Specify a prior weight for samples.  "
				" This augments data with one of each of the four possible haploid genotype combinations "
				" for each pairwise comparison, with the total weight given."
				" For example, a value of 1 indicates each haplotype is given 1/4 weight."
			)
			.set_takes_single_value()
			.set_default_value( "0" )
		;
		options.declare_group( "Miscellaneous options" ) ;
		options[ "-analysis-name" ]
			.set_description( "Specify a name to label results from this analysis with." )
			.set_takes_single_value()
			.set_default_value( "ldbird analysis" ) ;
		options[ "-analysis-chunk" ]
			.set_description( "Specify a name denoting the current genomic region or chunk on which this is run.  This is intended for use in parallel environments." )
			.set_takes_single_value()
			.set_default_value( genfile::MissingValue() ) ;
		options[ "-analysis-id" ]
			.set_description( "Specify an integer ID for the current analysis." )
			.set_takes_single_value() ;
		options[ "-table-prefix" ]
			.set_description( "Specify a prefix to add to tables.  They will be called <prefix>Frequency and <prefix>R." )
			.set_takes_single_value()
			.set_default_value( "" ) ;
		options[ "-threshold" ]
			.set_description( "The threshold to apply to genotype probabilities, if necessary, to make calls." )
			.set_takes_single_value()
			.set_default_value( 0.9 ) ;
		options.declare_group( "Miscellaneous options" ) ;
		options[ "-debug" ]
			.set_description( "Output debugging information." ) ;
	}
} ;

namespace {
	struct ProbSetter: public genfile::VariantDataReader::PerSampleSetter {
		typedef std::vector< metro::SampleRange > SampleRanges ;

		ProbSetter(
			Eigen::MatrixXd* genotypes,
			Eigen::VectorXi* ploidy,
			SampleRanges* nonmissing_samples
		):
			m_genotypes( genotypes ),
			m_ploidy( ploidy ),
			m_nonmissing_samples( nonmissing_samples ),
			m_sample_i(0)
		{
			assert( genotypes != 0 && nonmissing_samples != 0 && ploidy != 0 ) ;
		}

		void initialise( std::size_t nSamples, std::size_t nAlleles ) {
			if( nAlleles != 2 ) {
				throw genfile::BadArgumentError(
					"ProbSetter::initialise()",
					"nAlleles=" + genfile::string_utils::to_string( nAlleles ),
					"I only support biallelic variants"
				) ;
			}
			m_genotypes->resize( nSamples, 3 ) ;
			m_genotypes->setZero() ;
			m_ploidy->resize( nSamples ) ;
			m_ploidy->setZero() ;

			m_nonmissing_samples->clear() ;
			m_last_nonmissing_sample_i = 0 ;
		}

		bool set_sample( std::size_t n ) {
			m_sample_i = n ;
			return true ;
		}

		void set_number_of_entries(
			uint32_t ploidy, std::size_t n,
			genfile::OrderType const order_type,
			genfile::ValueType const value_type
		) {
			if( value_type != genfile::eProbability ) {
				throw genfile::BadArgumentError(
					"Callsetter::set_number_of_entries()",
					"value_type=" + genfile::string_utils::to_string( value_type ),
					"Expected genotype call probabilities (e.g. GP field)."
				) ;
			}
			if( order_type != genfile::ePerUnorderedGenotype ) {
				throw genfile::BadArgumentError(
					"Callsetter::set_number_of_entries()",
					"order_type=" + genfile::string_utils::to_string( order_type ),
					"Expected genotype call probabilities (e.g. GP field)."
				) ;
			}
			if( ploidy != 2 ) {
				throw genfile::BadArgumentError(
					"Callsetter::set_number_of_entries()",
					"order_type=" + genfile::string_utils::to_string( order_type ),
					"Expected a diploid sample."
				) ;
			}
			(*m_ploidy)(m_sample_i) = ploidy ;
		}

		void set_value( std::size_t entry_i, genfile::MissingValue const value ) {
			// This sample has missing data, end our current sample range here.
			if( m_sample_i > m_last_nonmissing_sample_i ) {
				m_nonmissing_samples->push_back(
					metro::SampleRange( m_last_nonmissing_sample_i, m_sample_i )
				) ;
			}
			// Skip this sample for next range.
			m_last_nonmissing_sample_i = m_sample_i + 1 ;
		}

		void set_value( std::size_t entry_i, Integer const value ) {
			assert(0) ; // expected a probabilty
		}

		void set_value( std::size_t entry_i, double const value ) {
			if( m_sample_i >= m_last_nonmissing_sample_i ) {
				(*m_genotypes)(m_sample_i, entry_i) = value ;
			}
		}

		void finalise() {
			// Add the final sample range if needed.
			if( (m_sample_i+1) > m_last_nonmissing_sample_i ) {
				m_nonmissing_samples->push_back(
					metro::SampleRange( m_last_nonmissing_sample_i, m_sample_i+1 )
				) ;
			}

			// For probability data, we need to handle the case of missingness encoded as zero probabilities.
			// Do that now by inspecting the genotypes.
			{
				std::vector< metro::SampleRange > ranges ;
				std::size_t last_nonmissing_sample_i = 0 ;
				std::size_t i = 0 ;
				for( ; i < m_genotypes->rows(); ++i ) {
					if( m_genotypes->row(i).sum() == 0.0 ) {
						if( i > last_nonmissing_sample_i ) {
							ranges.push_back( metro::SampleRange( last_nonmissing_sample_i, i )) ;
						}
						last_nonmissing_sample_i = i+1 ;
					}
				}
				if( i > last_nonmissing_sample_i ) {
					ranges.push_back( metro::SampleRange( last_nonmissing_sample_i, i )) ;
				}

				*m_nonmissing_samples = metro::impl::intersect_ranges( *m_nonmissing_samples, ranges ) ;
			}
		}

	private:
		Eigen::MatrixXd* m_genotypes ;
		Eigen::VectorXi* m_ploidy ;
		SampleRanges* m_nonmissing_samples ;
		std::size_t m_last_nonmissing_sample_i ;
		std::size_t m_sample_i ;
	} ;
}

struct LDbirdApplication: public appcontext::ApplicationContext
{
public:

	typedef Eigen::MatrixXd Matrix ;
	typedef Eigen::VectorXd Vector ;
	typedef Eigen::VectorXi IntegerVector ;
	typedef Eigen::RowVectorXd RowVector ;

public:
	LDbirdApplication( int argc, char** argv ):
		appcontext::ApplicationContext(
			globals::program_name,
			globals::program_version + ", revision " + globals::program_revision,
			std::auto_ptr< appcontext::OptionProcessor >( new CorrelatorOptions ),
			argc,
			argv,
			"-log"
		)
	{
		process() ;
	}

private:
	void process() {
		try {
			unsafe_process() ;
		}
		catch( genfile::InputError const& e ) {
			get_ui_context().logger() << "!! Error (" << e.what() << "): " << e.format_message() << ".\n" ;
			throw appcontext::HaltProgramWithReturnCode( -1 ) ;
		}
		catch( genfile::FileNotFoundError const& e ) {
			get_ui_context().logger() << "\nError: No file matching \"" << e.filespec() << "\" could be found.\n" ;
			throw appcontext::HaltProgramWithReturnCode( -1 ) ;
		}
		catch( genfile::db::Error const& e ) {
			get_ui_context().logger() << "!! Error (" << e.what() << ") with the following statement: \""
				<< e.sql()
				<< "\".\n" ;
			throw appcontext::HaltProgramWithReturnCode( -1 ) ;
		}
	}

	void unsafe_process() {
		using genfile::string_utils::to_string ;

		genfile::SampleFilterConjunction::UniquePtr sample_filter = get_sample_filter() ;

		genfile::CohortIndividualSource::UniquePtr
			samples = genfile::CohortIndividualSource::create( options().get< std::string >( "-s" ) ) ;

		std::set< std::size_t > const excluded_samples = compute_excluded_samples( *sample_filter, *samples ) ;
		if( excluded_samples.size() > 0 ) {
			samples.reset(
				genfile::SampleFilteringCohortIndividualSource::create( samples, excluded_samples ).release()
			) ;
		}

		genfile::SNPDataSource::UniquePtr g1 = open_genotype_data_sources(
			options().get< std::string >( "-g1" ),
			get_variant_filter(
				"-g1-incl-range",
				"-g1-incl-rsids"
			),
			excluded_samples
		) ;

		genfile::SNPDataSource::UniquePtr g2 ;
		if( options().check( "-g2" )) {
			// Two files given, we'll do a full carThis mechanism chooses a full cartesian product or a lower triangle implementation.
			// TODO: make this more obvious / cleaner.
			g2 = open_genotype_data_sources(
				options().get< std::string >( "-g2" ),
				get_variant_filter(
					"-g2-incl-range",
					"-g2-incl-rsids"
				),
				excluded_samples
			) ;
		} else {
			g2 = open_genotype_data_sources(
				options().get< std::string >( "-g1" ),
				get_variant_filter(
					"-g1-incl-range",
					"-g1-incl-rsids"
				),
				excluded_samples
			) ;
		}

		std::size_t const N = samples->size() ;
		if( g1->number_of_samples() != N ) {
			throw genfile::BadArgumentError(
				"LDbirdApplication::unsafe_process()",
				"-g1 \"" + options().get< std::string >( "-g1" ) + "\"",
				"Wrong number of samples (" + to_string(g1->number_of_samples()) + ", expected " + to_string(N) + ")"
			) ;
		}
		if( g2->number_of_samples() != N ) {
			throw genfile::BadArgumentError(
				"LDbirdApplication::unsafe_process()",
				"-g2 \"" + options().get< std::string >( "-g2" ) + "\"",
				"Wrong number of samples (" + to_string(g2->number_of_samples()) + ", expected " + to_string(N) + ")"
			) ;
		}

		write_preamble( *g1, *g2, *samples ) ;

		std::vector< int64_t > histogram( 2049, 0 ) ;

		boost::optional< genfile::db::Connection::RowId > analysis_id ;
		if( options().check( "-analysis-id" ) ) {
			analysis_id = options().get< genfile::db::Connection::RowId >( "-analysis-id" ) ;
		}

		std::string const fileSpec = options().get< std::string > ( "-o" ) ;
		std::string const tablePrefix = options().get< std::string > ( "-table-prefix" ) ;

		{
			qcdb::Storage::UniquePtr frequencyStorage = qcdb::Storage::create(
				fileSpec + ":" + tablePrefix + "Frequency",
				options().get< std::string > ( "-analysis-name" ),
				options().get< std::string > ( "-analysis-chunk" ),
				get_application_metadata(),
				"position,alleles",
				analysis_id
			) ;
			analysis_id = frequencyStorage->analysis_id() ;

			frequencyStorage->add_variable( "number_of_haplotypes" ) ;
			frequencyStorage->add_variable( "frequency" ) ;
			//frequencyStorage->set_variant_names( std::vector< std::string >({ "g1", "g2" })) ;

			qcdb::MultiVariantStorage::UniquePtr correlationStorage = qcdb::MultiVariantStorage::create(
				fileSpec + ":" + tablePrefix + "R+without rowid",
				2,
				options().get< std::string > ( "-analysis-name" ),
				options().get< std::string > ( "-analysis-chunk" ),
				get_application_metadata(),
				"position,alleles",
				analysis_id
			) ;

			correlationStorage->set_variant_names( std::vector< std::string >({ "g1", "g2" })) ;
			correlationStorage->add_variable( "N" ) ;
			correlationStorage->add_variable( "encoded_r" ) ;

			if( options().check( "-details" )) {
				for( int i = 0; i < 2; ++i ) {
					for( int j = 0; j < 2; ++j ) {
						correlationStorage->add_variable( "n_" + to_string(i) + to_string(j) ) ;
					}
				}

				correlationStorage->add_variable( "fet_pvalue" ) ;
			}
			// This mechanism chooses a full cartesian product or a lower triangle implementation
			// TODO: make this more obvious / cleaner.
			genfile::CartesianProductVisitor visitor( !options().check( "-g2" ) ) ;
			visitor.add_source( "g1", g1.get() ) ;
			visitor.add_source( "g2", g2.get() ) ;

			if( options().check( "-min-distance" )) {
				visitor.set_min_distance( options().get< int64_t >( "-min-distance" )) ;
			}

			run( visitor, *samples, *frequencyStorage, *correlationStorage, &histogram ) ;

			frequencyStorage->finalise() ;
			correlationStorage->finalise() ;
		}

		{
			qcdb::MultiVariantStorage::UniquePtr histogramStorage = qcdb::MultiVariantStorage::create(
				fileSpec + ":" + tablePrefix + "Histogram",
				0,
				options().get< std::string > ( "-analysis-name" ),
				options().get< std::string > ( "-analysis-chunk" ),
				get_application_metadata(),
				"position,alleles",
				analysis_id
			) ;
			histogramStorage->add_variable( "encoded_r" ) ;
			histogramStorage->add_variable( "count" ) ;
			qcdb::MultiVariantStorage::Key const key ;
			for( std::size_t i = 0; i < histogram.size(); ++i ) {
				histogramStorage->create_new_key( key ) ;
				histogramStorage->store_data_for_key( key, "encoded_r", int64_t(i) ) ;
				histogramStorage->store_data_for_key( key, "count", histogram[i] ) ;
			}
			histogramStorage->finalise() ;
		}
	}

	genfile::CommonSNPFilter::UniquePtr get_variant_filter(
		std::string const& incl_range_option,
		std::string const& incl_ids_option
	) const {
		genfile::CommonSNPFilter::UniquePtr result ;
		if( options().check( incl_range_option ) || options().check( incl_ids_option ) ) {
			result.reset( new genfile::CommonSNPFilter() ) ;
			if( options().check( incl_range_option )) {
				std::vector< std::string > specs = collect_unique_ids( options().get_values< std::string >( incl_range_option ) ) ;
				for ( std::size_t i = 0; i < specs.size(); ++i ) {
					result->include_snps_in_range(
						genfile::GenomePositionRange::parse( specs[i] )
					) ;
				}
			}
			if( options().check( incl_ids_option )) {
				std::vector< std::string > files = collect_unique_ids( options().get_values< std::string > ( incl_ids_option ) ) ;
				result->include_snps_in_set(
					std::set< std::string >( files.begin(), files.end() ),
					genfile::CommonSNPFilter::RSIDs
				) ;
			}
		}
		return result ;
	}

	genfile::SampleFilterConjunction::UniquePtr get_sample_filter() const {
		genfile::SampleFilterConjunction::UniquePtr filter( new genfile::SampleFilterConjunction() ) ;
		genfile::SampleFilterDisjunction::UniquePtr sample_inclusion_filter( new genfile::SampleFilterDisjunction() ) ;

		if( options().check_if_option_was_supplied( "-incl-samples" ) ) {
			genfile::SampleFilter::UniquePtr id1_filter = get_sample_id_filter(
				options().get_values< std::string >( "-incl-samples" )
			) ;
			sample_inclusion_filter->add_clause( genfile::SampleFilter::UniquePtr( id1_filter.release() ) ) ;
		}

		if( options().check_if_option_was_supplied( "-excl-samples" ) ) {
			genfile::SampleFilter::UniquePtr id1_filter = get_sample_id_filter(
				options().get_values< std::string >( "-excl-samples" )
			) ;
			filter->add_clause(
				genfile::SampleFilter::UniquePtr(
					new genfile::SampleFilterNegation(
						id1_filter
					)
				)
			) ;
		}

		if( options().check_if_option_was_supplied( "-incl-samples-where" ) ) {
			std::vector< std::string > conditions = options().get_values< std::string >( "-incl-samples-where" ) ;
			// we OR together different WHERE clauses.
			for( std::size_t i = 0; i < conditions.size(); ++i ) {
				sample_inclusion_filter->add_clause( genfile::SampleFilter::create( conditions[i] )) ;
			}
		}

		if( options().check_if_option_was_supplied( "-excl-samples-where" ) ) {
			std::vector< std::string > conditions = options().get_values< std::string >( "-excl-samples-where" ) ;
			// we AND together different WHERE clauses.
			genfile::SampleFilterDisjunction::UniquePtr where( new genfile::SampleFilterDisjunction() ) ;
			for( std::size_t i = 0; i < conditions.size(); ++i ) {
				where->add_clause( genfile::SampleFilter::create( conditions[i] )) ;
			}
			filter->add_clause(
				genfile::SampleFilter::UniquePtr(
					new genfile::SampleFilterNegation(
						genfile::SampleFilter::UniquePtr( where.release() )
					)
				)
			) ;
		}
		if( sample_inclusion_filter->number_of_clauses() > 0 ) {
			filter->add_clause( genfile::SampleFilter::UniquePtr( sample_inclusion_filter.release() ) ) ;
		}

		return filter ;
	}

	std::set< std::size_t > compute_excluded_samples( genfile::SampleFilter const& filter, genfile::CohortIndividualSource const& samples ) const {
		std::vector< std::size_t > included_samples ;
		void(std::vector<std::size_t>::*push_back)(std::size_t const&) = &std::vector<std::size_t>::push_back ;
		filter.test(
			samples,
			boost::bind(
				push_back,
				&included_samples,
				_1
			)
		) ;
		std::set< std::size_t > excluded_samples(
			boost::counting_iterator< std::size_t >(0),
			boost::counting_iterator< std::size_t >( samples.get_number_of_individuals() )
		) ;
		for( std::size_t i = 0; i < included_samples.size(); ++i ) {
			excluded_samples.erase( included_samples[i] ) ;
		}
		return excluded_samples ;
	}

	genfile::SampleFilter::UniquePtr get_sample_id_filter( std::vector< std::string > const& filenames ) const {
		genfile::VariableInSetSampleFilter::UniquePtr result( new genfile::VariableInSetSampleFilter( "ID_1" )) ;
		for( std::size_t i = 0; i < filenames.size(); ++i ) {
			std::string elt ;
			std::ifstream is( filenames[i].c_str() ) ;
			while( is >> elt ) {
				result->add_level( elt ) ;
			}
		}
		return genfile::SampleFilter::UniquePtr( result.release() ) ;
	}

	genfile::SNPDataSource::UniquePtr open_genotype_data_sources(
		std::string const& filename,
		genfile::CommonSNPFilter::UniquePtr filter,
		std::set< std::size_t > const& excluded_samples,
		bool threshold = false
	) {
		std::vector< genfile::wildcard::FilenameMatch > filenames
			= genfile::wildcard::find_files_by_chromosome(
				filename,
				genfile::wildcard::eALL_CHROMOSOMES
			) ;

		genfile::SNPDataSource::UniquePtr source(
			genfile::SNPDataSourceChain::create(
				filenames
			).release()
		) ;
		if( filter.get() ) {
			source.reset(
				genfile::VariantIdentifyingDataFilteringSNPDataSource::create(
					source,
					genfile::VariantIdentifyingDataTest::UniquePtr( filter.release() )
				).release()
			) ;
		}
		if( threshold ) {
			source.reset(
				new genfile::ThreshholdingSNPDataSource( source, 0.9 )
			) ;
		}
		if( excluded_samples.size() > 0 ) {
			source.reset(
				new genfile::SampleFilteringSNPDataSource( source, excluded_samples )
			) ;
		}
		return source ;
	}

	void write_preamble(
		genfile::SNPDataSource const& host,
		genfile::SNPDataSource const& para,
		genfile::CohortIndividualSource const& samples
	) {
		get_ui_context().logger() << "Loaded data for " << samples.size() << " samples.\n" ;
		get_ui_context().logger() << "  First dataset:\n" << host.get_summary() << "\n" ;
		get_ui_context().logger() << " Second dataset:\n" << para.get_summary() << "\n" ;
	}

	void run(
		genfile::MultiSourceVisitor& visitor,
		genfile::CohortIndividualSource const& samples,
		qcdb::Storage& frequencyOutput,
		qcdb::MultiVariantStorage& correlationOutput,
		std::vector< int64_t >* histogram
	) {
		appcontext::UIContext::ProgressContext progress_context = get_ui_context().get_progress_context( "Testing" ) ;
		histogram->resize( 2049 ) ;
		std::fill( histogram->begin(), histogram->end(), 0 ) ;
		for(
			std::size_t count = 0;
			visitor.step(
				[this,&frequencyOutput,&correlationOutput,&histogram](
					std::vector< int > const& changed,
					std::vector< genfile::VariantIdentifyingData > const& variants,
					std::vector< genfile::VariantDataReader::SharedPtr > const& readers
				) {
					this->process_one(
						changed,
						variants,
						readers,
						frequencyOutput,
						correlationOutput,
						histogram
					) ;
				}
			) ;
			++count
		) {
			progress_context( count, visitor.count() ) ;
		} ;
		progress_context.finish() ;
	}

	void process_one(
		std::vector< int > const& changed,
		std::vector< genfile::VariantIdentifyingData > const& variants,
		std::vector< genfile::VariantDataReader::SharedPtr > const& readers,
		qcdb::Storage& frequencyOutput,
		qcdb::MultiVariantStorage& correlationOutput,
		std::vector< int64_t >* histogram
	) {
		using genfile::string_utils::to_string ;

		if( m_dosages.size() != changed.size() ) {
			m_dosages.resize( changed.size() ) ;
			m_ploidy.resize( changed.size() ) ;
			m_nonmissingness.resize( changed.size() ) ;
		}
		double const prior_weight = options().get< double >( "-prior-weight" ) ;
		double min_maf = 0.5 ;
		for( std::size_t i = 0; i < changed.size(); ++i ) {
			if( changed[i] == 1 ) {
				DosageSetter setter( &(m_dosages[i]), &(m_ploidy[i]), &(m_nonmissingness[i]), options().check( "-assume-haploid" )) ;
				readers[i]->get( ":genotypes:", genfile::to_GP_unphased( setter )) ;
			}

#if DEBUG
			std::cerr << globals::program_name + ":process_one(): variant " << i << ": " << variants[i] << ":\n" ;
			std::cerr << "Loaded data with nonmissingness: " << m_nonmissingness[i] << ".\n" ;
#endif

			FrequencyStore::const_iterator where = m_frequencies.find( variants[i] ) ;
			double frequency = 0.0 ;
			if( where != m_frequencies.end() ) {
				frequency = where->second ;
			} else {
				std::pair< double, int64_t > computedFrequency = compute_regularised_frequency( m_dosages[i], m_ploidy[i], m_nonmissingness[i], prior_weight ) ;
				frequency = computedFrequency.first ;

				// not yet computed, so store it
				m_frequencies[ variants[i] ] = frequency ;
				// Output the sample count and the frequency
				frequencyOutput.store_per_variant_data(
					variants[i],
					"number_of_haplotypes",
					computedFrequency.second
				) ;
				frequencyOutput.store_per_variant_data(
					variants[i],
					"frequency",
					frequency
				) ;
			}
			min_maf = std::min( min_maf, std::min( frequency, 1.0 - frequency )) ;
		}

		// Bail out if any of the variants are too rare
		if( min_maf < options().get< double >( "-min-maf" )) {
			return ;
		}

		int const total_number_of_samples = m_dosages[0].size() ;
		std::vector< metro::SampleRange > included_samples( 1, metro::SampleRange( 0, total_number_of_samples )) ;
		for( std::size_t i = 0; i < changed.size(); ++i ) {
			included_samples = metro::impl::intersect_ranges( included_samples, m_nonmissingness[i] ) ;
		}

		// Bail out if there aren't enough samples in the pairwise comparison
		{
			std::size_t count = metro::impl::count_range( included_samples ) ;
			if(
				(count < options().get< double >( "-min-N" ))
			 	   || (( double(count)/total_number_of_samples ) < options().get< double >( "-min-N-propn" ))
			) {
				return ;
			}
		}

		double covariance = 0.0, correlation = 0.0, N = 0.0 ;

		compute_regularised_correlation(
			m_dosages, m_ploidy, included_samples,
			&covariance, &correlation, &N,
			prior_weight
		) ;

		// ints in sqlite use only 2 bytes if up to +ve integer 2287.
		// We map -1...1 to 0...2048, such that the transformation
		//
		// correlation = (stored_value / 1024) - 1.0
		// or
		// correlation = (stored_value - 1024.0) / 1024.0
		// maps back to correlation space.
		//
		int64_t const encoded_r = ( std::round((correlation + 1.0) * 1024 )) ;
		++((*histogram)[ encoded_r ]) ;

		if( ( correlation * correlation ) >= options().get< double >( "-min-r2" ) ) {
			correlationOutput.store_data_for_key(
				variants,
				"N",
				int64_t(N)
			) ;
			// We store correlations as integers on the scale -16384 to 16384.
			// This is because sqlite has compression for integer storage, such that
			// reals take 8 bytes but integers take
			correlationOutput.store_data_for_key(
				variants,
				"encoded_r",
				encoded_r
			) ;

			if( options().check( "-details" )) {
				Eigen::MatrixXd const table = tabulate( m_dosages, m_ploidy, included_samples ) ;
				for( int i = 0; i < table.rows(); ++i ) {
					for( int j = 0; j < table.cols(); ++j ) {
						correlationOutput.store_data_for_key(
							variants,
							"n_" + to_string(i) + to_string(j),
							table(i,j)
						) ;
					}
				}

				if( m_ploidy[0].maxCoeff() == 1 ) {
					metro::FishersExactTest test( table ) ;
					double pvalue = test.get_pvalue( metro::FishersExactTest::eTwoSided ) ;
					correlationOutput.store_data_for_key(
						variants,
						"fet_pvalue",
						pvalue
					) ;
				}
			}
		}
#if DEBUG
		std::cerr << globals::program_name + ":process_one():\n" ;
		std::cerr << "dosage1: " << m_dosages[0].head( 15 ).transpose() << "...\n" ;
		std::cerr << "dosage2: " << m_dosages[1].head( 15 ).transpose() << "...\n" ;
		std::cerr << "N: " << N << ".\n" ;
		std::cerr << "covariance: " << covariance << ".\n" ;
		std::cerr << "correlation: " << correlation << ".\n" ;
		std::cerr << "included: " << included_samples << ".\n" ;
#endif

	}

private:
	std::vector< Eigen::VectorXd > m_dosages ;
	std::vector< Eigen::VectorXd > m_ploidy ;
	std::vector< std::vector< metro::SampleRange > > m_nonmissingness ;
	typedef std::map< genfile::VariantIdentifyingData, double > FrequencyStore ;
	FrequencyStore m_frequencies ;

private:

	std::pair< double, int64_t > compute_regularised_frequency(
		Eigen::VectorXd const& dosages,
		Eigen::VectorXd const& ploidy,
		std::vector< metro::SampleRange > const& included_samples,
		double prior_weight = 1.0
	) {
		double result = 0.0 ;
		double N = 0.0 ;
		for( std::size_t i = 0; i < included_samples.size(); ++i ) {
			metro::SampleRange const& range = included_samples[i] ;
			result += dosages.segment( range.begin(), range.size() ).sum() ;
			N += ploidy.segment( range.begin(), range.size() ).sum() ;
		}
		// Assume we have additional data adding up to prior_weight haploid samples
		// evenly split between both alleles
		result += prior_weight / 2 ;
		N += prior_weight ;
		return std::make_pair( result / N, int64_t( N ) ) ;
	}

	void compute_regularised_correlation(
		std::vector< Eigen::VectorXd > const& dosages,
		std::vector< Eigen::VectorXd > const& ploidy,
		std::vector< metro::SampleRange > const& included_samples,
		double* covariance,
		double* correlation,
		double* number_of_samples,
		double const prior_weight = 1.0
	) {
		assert( dosages.size() == 2 ) ;
		assert( covariance ) ;
		assert( correlation ) ;
		assert( number_of_samples ) ;
		assert( ploidy[0] == ploidy[1] ) ;
		std::vector< double > frequencies( dosages.size(), 0.0 ) ;

		typedef Eigen::VectorBlock< Eigen::VectorXd const > ConstBlock ;
		typedef Eigen::VectorBlock< Eigen::VectorXi const > ConstIntegerBlock ;

		// Although frequencies are computed above, we recompute here because the
		// number of included samples may differ for the pairwise test.
		for( std::size_t v = 0; v < frequencies.size(); ++v ) {
			frequencies[v] = compute_regularised_frequency(
				dosages[v], ploidy[v], included_samples, prior_weight
			).first ;
		}

		double result = 0.0 ;
		double N = 0.0 ;
		std::vector< double > variances( dosages.size(), 0.0 ) ;
		for( std::size_t i = 0; i < included_samples.size(); ++i ) {
			metro::SampleRange const& range = included_samples[i] ;
			ConstBlock d0 = dosages[0].segment( range.begin(), range.size() ) ;
			ConstBlock d1 = dosages[1].segment( range.begin(), range.size() ) ;
			ConstBlock p = ploidy[0].segment( range.begin(), range.size() ) ;
			result += (
				( d0 - p * frequencies[0] ).array()
				* ( d1 - p * frequencies[1] ).array()
			).sum() ;
			N += p.array().sum() ;
			for( std::size_t v = 0; v < frequencies.size(); ++v ) {
				ConstBlock d = dosages[v].segment( range.begin(), range.size() ) ;
				ConstBlock p = ploidy[v].segment( range.begin(), range.size() ) ;
				variances[v] += ( d - p * frequencies[v] ).array().square().sum() ;
			}
		}

		// Add regularising information consisting of a total of
		// prior_weight haploid samples, split evenly between
		// the four possible genotype combinations
		result += (0 - frequencies[0]) * (0 - frequencies[1] ) * (prior_weight / 4) ;
		result += (0 - frequencies[0]) * (1 - frequencies[1] ) * (prior_weight / 4) ;
		result += (1 - frequencies[0]) * (0 - frequencies[1] ) * (prior_weight / 4) ;
		result += (1 - frequencies[0]) * (1 - frequencies[1] ) * (prior_weight / 4) ;
		for( std::size_t v = 0; v < frequencies.size(); ++v ) {
			variances[v] += ( 0 - frequencies[v] ) * ( 0 - frequencies[v] ) * (prior_weight / 2.0) ;
			variances[v] += ( 1 - frequencies[v] ) * ( 1 - frequencies[v] ) * (prior_weight / 2.0) ;
		}
		N += prior_weight ;

		*covariance = result / N ;
		*correlation = result / (std::sqrt( variances[0] ) * std::sqrt( variances[1] )) ;
		*number_of_samples = N ;
	}

	Eigen::MatrixXd tabulate(
		std::vector< Eigen::VectorXd > const& dosages,
		std::vector< Eigen::VectorXd > const& ploidy,
		std::vector< metro::SampleRange > const& included_samples
	) const {
		typedef Eigen::VectorBlock< Eigen::VectorXd const > ConstBlock ;
		typedef Eigen::VectorBlock< Eigen::VectorXi const > ConstIntegerBlock ;

		int maxPloidy0 = 0 ;
		int maxPloidy1 = 0 ;
		for( std::size_t i = 0; i < included_samples.size(); ++i ) {
			metro::SampleRange const& range = included_samples[i] ;
			maxPloidy0 = std::max( maxPloidy0, int( ploidy[0].segment( range.begin(), range.size() ).array().maxCoeff()) ) ;
			maxPloidy1 = std::max( maxPloidy1, int( ploidy[1].segment( range.begin(), range.size() ).array().maxCoeff()) ) ;
		}
		Eigen::MatrixXd result = Eigen::MatrixXd::Zero( maxPloidy0 + 1, maxPloidy1 + 1 ) ;
		for( std::size_t i = 0; i < included_samples.size(); ++i ) {
			metro::SampleRange const& range = included_samples[i] ;
			for( int j = range.begin(); j < range.end(); ++j ) {
				++result( dosages[0][j], dosages[1][j] ) ;
			}
		}
		return result ;
	}

	struct DosageSetter: public genfile::VariantDataReader::PerSampleSetter {
		typedef std::vector< metro::SampleRange > SampleRanges ;

		DosageSetter(
			Eigen::VectorXd* dosages,
			Eigen::VectorXd* ploidy,
			SampleRanges* nonmissing_samples,
			bool treat_as_haploid = false
		):
			m_dosages( dosages ),
			m_ploidy( ploidy ),
			m_total_prob(0.0),
			m_treat_as_haploid( treat_as_haploid ),
			m_number_of_entries( 0 ),
			m_nonmissing_samples( nonmissing_samples ),
			m_sample_i(0)
		{
			assert( dosages != 0 && nonmissing_samples != 0 && ploidy != 0 ) ;
		}

		void initialise( std::size_t nSamples, std::size_t nAlleles ) {
			if( nAlleles != 2 ) {
				throw genfile::BadArgumentError(
					"ProbSetter::initialise()",
					"nAlleles=" + genfile::string_utils::to_string( nAlleles ),
					"I only support biallelic variants"
				) ;
			}
			m_dosages->resize( nSamples ) ;
			m_dosages->setZero() ;
			m_ploidy->resize( nSamples ) ;
			m_ploidy->setZero() ;

			m_nonmissing_samples->clear() ;
			m_last_nonmissing_sample_i = 0 ;
		}

		bool set_sample( std::size_t n ) {
			m_sample_i = n ;
			return true ;
		}

		void set_number_of_entries(
			uint32_t ploidy, std::size_t n,
			genfile::OrderType const order_type,
			genfile::ValueType const value_type
		) {
			if( value_type != genfile::eProbability ) {
				throw genfile::BadArgumentError(
					"Callsetter::set_number_of_entries()",
					"value_type=" + genfile::string_utils::to_string( value_type ),
					"Expected genotype call probabilities (e.g. GP field)."
				) ;
			}
			if( order_type != genfile::ePerUnorderedGenotype ) {
				throw genfile::BadArgumentError(
					"Callsetter::set_number_of_entries()",
					"order_type=" + genfile::string_utils::to_string( order_type ),
					"Expected genotype call probabilities (e.g. GP field)."
				) ;
			}
			if( m_treat_as_haploid ) {
				(*m_ploidy)(m_sample_i) = 1 ;
			} else {
				(*m_ploidy)(m_sample_i) = ploidy ;
			}
			m_number_of_entries = n ;
			m_total_prob = 0.0 ;
		}

		void set_value( std::size_t entry_i, genfile::MissingValue const value ) {
			// This sample has missing data, end our current sample range here.
			if( m_sample_i > m_last_nonmissing_sample_i ) {
				m_nonmissing_samples->push_back(
					metro::SampleRange( m_last_nonmissing_sample_i, m_sample_i )
				) ;
#if DEBUG > 2
				std::cerr << "DosageSetter::set_value(): added sample range: " << m_nonmissing_samples->back() << ".\n" ;
#endif
			}
			// Skip this sample for next range.
			m_last_nonmissing_sample_i = m_sample_i + 1 ;
		}

		void set_value( std::size_t entry_i, Integer const value ) {
			assert(0) ; // expected a probabilty
		}

		void set_value( std::size_t entry_i, double const value ) {
			if( !sample_recorded_missing() ) {
				bool const last_entry = ((entry_i+1) == m_number_of_entries ) ;
				// we assume two alleles, so entry_i is also the count of the B allele
				// in the genotype.
				if( m_treat_as_haploid ) {
					// only homozygous genotypes are counted, others are assumed missing
					// entry_i == 0 contributes 0 dosage anyway so just count the last entry
					//
					// Sample considered missing if has nonzero probability on non-homozygous genotype
					// (caught here), or if it has total 0 probability on homozygous genotypes (handled below).
					if( entry_i == 0 || last_entry ) {
						(*m_dosages)(m_sample_i) += ( last_entry ? value : 0.0 );
						m_total_prob += value ;
					} else if( value != 0 ) {
						record_missing_sample() ;
					}
				} else {
					(*m_dosages)(m_sample_i) += value * entry_i ;
					m_total_prob += value ;
				}

				// handle missing data encoded as zeroes
				if( last_entry && m_total_prob == 0.0 ) {
					record_missing_sample() ;
				}

#if DEBUG > 2
				std::cerr << "DosageSetter::set_value(): added value " << (value * entry_i) << " to entry " << entry_i << " for sample " << m_sample_i << ".\n" ;
#endif
			}
		}

		void finalise() {
			// Add the final sample range if needed.
			if( (m_sample_i+1) > m_last_nonmissing_sample_i ) {
				m_nonmissing_samples->push_back(
					metro::SampleRange( m_last_nonmissing_sample_i, m_sample_i+1 )
				) ;
#if DEBUG > 2
				std::cerr << "DosageSetter::finalise(): added sample range: " << m_nonmissing_samples->back() << ".\n" ;
#endif
			}
#if DEBUG > 2
				std::cerr << "DosageSetter::finalise(): complete.\n" ;
#endif
		}

	private:
		Eigen::VectorXd* m_dosages ;
		Eigen::VectorXd* m_ploidy ;
		double m_total_prob ;
		bool m_treat_as_haploid ;

		std::size_t m_number_of_entries ;
		SampleRanges* m_nonmissing_samples ;
		std::size_t m_last_nonmissing_sample_i ;
		std::size_t m_sample_i ;
	private:

		bool sample_recorded_missing() const {
			return m_sample_i < m_last_nonmissing_sample_i ;
		}

		void record_missing_sample() {
			// missing
			m_nonmissing_samples->push_back(
				metro::SampleRange( m_last_nonmissing_sample_i, m_sample_i )
			) ;
			// Skip this sample for next range.
			m_last_nonmissing_sample_i = m_sample_i + 1 ;
		}
	} ;
} ;

int main( int argc, char** argv ) {
    try {
		LDbirdApplication app( argc, argv ) ;
    }
	catch( appcontext::HaltProgramWithReturnCode const& e ) {
		return e.return_code() ;
	}
	return 0 ;
}


#include "myObject.h"
#include <iostream>
#include <sstream>
#include <fstream>

#define _USE_MATH_DEFINES
#include <math.h>

#define GLM_FORCE_RADIANS
#include <glm/glm.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <glm/gtc/type_ptr.hpp>
#include <glm/gtx/intersect.hpp>
#include "helperFunctions.h"
#include "errors.h"

#include <boost/filesystem.hpp>
namespace fs = boost::filesystem;
using namespace std;

myObject::myObject()
{
    model_matrix = glm::mat4(1.0f);
    vao = nullptr;
    name = "default";
}

myObject::~myObject()
{
    clear();
}

void myObject::clear()
{
    if (vao) delete vao;

    for (std::unordered_multimap<std::string, mySubObject*>::iterator it = objects.begin(); it != objects.end(); ++it)
        delete it->second;
    objects.clear();

    vector<glm::vec3> empty; vertices.swap(empty);
    normals.swap(empty);
    vector<glm::ivec3> empty2; indices.swap(empty2);
}

std::vector<myObject*> myObject::createFromThis(size_t num, bool createVAO) const
{
    std::vector<myObject*> res(num, nullptr);
    for (size_t i = 0; i < num; ++i)
    {
        res[i] = new myObject();
        res[i]->vertices = this->vertices;
        res[i]->indices = this->indices;
        res[i]->normals = this->normals;
        res[i]->texturecoordinates = this->texturecoordinates;
        res[i]->tangents = this->tangents;
        res[i]->model_matrix = this->model_matrix;
        res[i]->name = this->name;

        for (std::unordered_multimap<std::string, mySubObject*>::const_iterator it = this->objects.begin(); it != this->objects.end(); ++it)
        {
            auto name = it->first;
            auto obj = it->second;
            mySubObject* o = new mySubObject(obj->material, obj->start, obj->end, obj->name);

            o->textures = obj->textures; // I guess textures may be the same object?
            res[i]->objects.emplace(name, o);
        }

        if (createVAO)
            res[i]->createmyVAO();
    }
    return res;
}

void myObject::readMaterials(std::string mtlfilename, unordered_map<string, myMaterial*>& materials, unordered_map<string, myTexture*>& textures)
{
    ifstream mtlfin(mtlfilename);

    if (!mtlfin.is_open())
    {
        cout << "Error! Unable to open mtl file: " << mtlfilename << "\n";
        return;
    }

    string v;
    myMaterial* curr_mat = nullptr;

    while (mtlfin >> v)
    {
        if (v == "newmtl")
        {
            curr_mat = new myMaterial();
            mtlfin >> curr_mat->mat_name;

            materials.emplace(curr_mat->mat_name, curr_mat);
        }
        else if (v == "Ns") mtlfin >> curr_mat->specular_coefficient;
        else if (v == "Ka") mtlfin >> curr_mat->ka.r >> curr_mat->ka.g >> curr_mat->ka.b;
        else if (v == "Kd") mtlfin >> curr_mat->kd.r >> curr_mat->kd.g >> curr_mat->kd.b;
        else if (v == "Ks") mtlfin >> curr_mat->ks.r >> curr_mat->ks.g >> curr_mat->ks.b;
        else if (v == "map_Kd")
        {
            string t;
            mtlfin >> t;
            fs::path texfilename = fs::path(mtlfilename).remove_filename() / t;
            if (!fs::exists(texfilename))
                texfilename = fs::path(t); // try raw absolute path

            textures.emplace(curr_mat->mat_name, new myTexture(texfilename.string()));
        }
    }

    mtlfin.close();
}

bool myObject::readObjects(std::string filename, bool individualvertices_per_face, bool tonormalize)
{
    clear();

    string s, t;
    string mtlfilename;

    fs::path filenameP(filename);
    if (!fs::exists(filenameP) || !fs::is_regular_file(filenameP))
        return false;

    name = filenameP.filename().replace_extension().string();

    ifstream fin(filename);
    if (!fin.is_open()) return false;

    size_t curr_start = 0; size_t curr_end;
    string curr_name = "noname";
    myMaterial* curr_mat = nullptr;
    myTexture* curr_texture = nullptr;

    unordered_map<string, myMaterial*> materials;
    unordered_map<string, myTexture*> textures;

    vector<glm::vec3> tmp_vertices;
    vector<glm::vec2> tmp_texturecoordinates;
    vector<glm::vec3> tmp_normals;

    while (getline(fin, s))
    {
        stringstream myline(s);
        myline >> t;
        if (t == "g" || t == "o")
        {
            curr_end = indices.size();
            mySubObject* o = new mySubObject(curr_mat, curr_start, curr_end, curr_name);
            o->setTexture(curr_texture, mySubObject::COLORMAP);
            objects.emplace(curr_name, o);

            curr_start = curr_end;
            myline >> curr_name;
        }
        else if (t == "v")
        {
            float x, y, z;
            myline >> x >> y >> z;
            tmp_vertices.push_back(glm::vec3(x, y, z));
        }
        else if (t == "vn")
        {
            float x, y, z;
            myline >> x >> y >> z;
            tmp_normals.push_back(glm::vec3(x, y, z));
        }
        else if (t == "vt")
        {
            float _s, _t;
            myline >> _s >> _t;
            tmp_texturecoordinates.push_back(glm::vec2(_s, 1.0f - _t));
        }
        else if (t == "mtllib")
        {
            myline >> mtlfilename;

            fs::path mtlpath = filenameP.remove_filename() / mtlfilename;
            readMaterials(mtlpath.string(), materials, textures);
        }
        else if (t == "usemtl")
        {
            curr_end = indices.size();
            mySubObject* o = new mySubObject(curr_mat, curr_start, curr_end, curr_name);
            o->setTexture(curr_texture, mySubObject::COLORMAP);
            objects.emplace(curr_name, o);

            curr_start = curr_end;

            string u;
            myline >> u;
            //curr_name = u;

            if (materials.count(u) != 0)
                curr_mat = materials[u];
            else curr_mat = nullptr;

            if (textures.count(u) != 0)
                curr_texture = textures[u];
            else curr_texture = nullptr;
        }
        else if (t == "s") {}
        else if (t == "f")
        {
            unsigned int vertex_index1, vertex_index2, vertex_index3;
            unsigned int texture_index1, texture_index2, texture_index3;
            unsigned int normal_index1, normal_index2, normal_index3;

            myline >> t;
            parseObjFace(t, vertex_index1, texture_index1, normal_index1);

            myline >> t;
            parseObjFace(t, vertex_index2, texture_index2, normal_index2);

            while (myline >> t)
            {
                parseObjFace(t, vertex_index3, texture_index3, normal_index3);

                if (individualvertices_per_face)
                {
                    vertices.push_back(tmp_vertices[vertex_index1]);
                    if (texture_index1 < tmp_texturecoordinates.size())
                        texturecoordinates.push_back(tmp_texturecoordinates[texture_index1]);
                    else texturecoordinates.push_back(glm::vec2(0, 0));
                    if (normal_index1 < tmp_normals.size())
                        normals.push_back(tmp_normals[normal_index1]);
                    else normals.push_back(glm::vec3(0, 0, 0));

                    vertices.push_back(tmp_vertices[vertex_index2]);
                    if (texture_index2 < tmp_texturecoordinates.size())
                        texturecoordinates.push_back(tmp_texturecoordinates[texture_index2]);
                    else texturecoordinates.push_back(glm::vec2(0, 0));
                    if (normal_index2 < tmp_normals.size())
                        normals.push_back(tmp_normals[normal_index2]);
                    else normals.push_back(glm::vec3(0, 0, 0));

                    vertices.push_back(tmp_vertices[vertex_index3]);
                    if (texture_index3 < tmp_texturecoordinates.size())
                        texturecoordinates.push_back(tmp_texturecoordinates[texture_index3]);
                    else texturecoordinates.push_back(glm::vec2(0, 0));
                    if (normal_index3 < tmp_normals.size())
                        normals.push_back(tmp_normals[normal_index3]);
                    else normals.push_back(glm::vec3(0, 0, 0));

                    indices.push_back(glm::ivec3(vertices.size() - 3, vertices.size() - 2, vertices.size() - 1));
                }
                else
                    indices.push_back(glm::ivec3(vertex_index1, vertex_index2, vertex_index3));

                vertex_index2 = vertex_index3;
                texture_index2 = texture_index3;
                normal_index2 = normal_index3;
            }
        }
        s.clear();
        t.clear();
    }

    curr_end = indices.size();
    mySubObject* o = new mySubObject(curr_mat, curr_start, curr_end, curr_name);
    o->setTexture(curr_texture, mySubObject::COLORMAP);
    objects.emplace(curr_name, o);

    if (!individualvertices_per_face)
    {
        vertices = tmp_vertices;
        computeNormals();
    }

    if (tonormalize) normalize();

    return true;
}

void myObject::normalize()
{
    unsigned int tmpxmin = 0, tmpymin = 0, tmpzmin = 0, tmpxmax = 0, tmpymax = 0, tmpzmax = 0;

    for (unsigned i = 0; i < vertices.size(); i++) {
        if (vertices[i].x < vertices[tmpxmin].x) tmpxmin = i;
        if (vertices[i].x > vertices[tmpxmax].x) tmpxmax = i;

        if (vertices[i].y < vertices[tmpymin].y) tmpymin = i;
        if (vertices[i].y > vertices[tmpymax].y) tmpymax = i;

        if (vertices[i].z < vertices[tmpzmin].z) tmpzmin = i;
        if (vertices[i].z > vertices[tmpzmax].z) tmpzmax = i;
    }

    float xmin = vertices[tmpxmin].x, xmax = vertices[tmpxmax].x,
        ymin = vertices[tmpymin].y, ymax = vertices[tmpymax].y,
        zmin = vertices[tmpzmin].z, zmax = vertices[tmpzmax].z;

    float scale = ((xmax - xmin) <= (ymax - ymin)) ? (xmax - xmin) : (ymax - ymin);
    scale = (scale >= (zmax - zmin)) ? scale : (zmax - zmin);

    for (unsigned int i = 0; i < vertices.size(); i++) {
        vertices[i].x -= (xmax + xmin) / 2;
        vertices[i].y -= (ymax + ymin) / 2;
        vertices[i].z -= (zmax + zmin) / 2;

        vertices[i].x /= scale;
        vertices[i].y /= scale;
        vertices[i].z /= scale;
    }
}

void myObject::computeNormals()
{
    normals.assign(vertices.size(), glm::vec3(0.0f, 0.0f, 0.0f));
    for (unsigned int i = 0; i < indices.size(); i++)
    {
        glm::vec3 face_normal = glm::cross(vertices[indices[i][1]] - vertices[indices[i][0]], vertices[indices[i][2]] - vertices[indices[i][1]]);
        normals[indices[i][0]] += face_normal;
        normals[indices[i][1]] += face_normal;
        normals[indices[i][2]] += face_normal;
    }
    for (unsigned int i = 0; i < vertices.size(); i++)  normals[i] = glm::normalize(normals[i]);
}

void myObject::createmyVAO()
{
    if (vao != nullptr) delete vao;
    vao = new myVAO();

    if (vertices.size()) vao->storePositions(vertices, 0);
    if (normals.size()) vao->storeNormals(normals, 1);
    if (texturecoordinates.size()) vao->storeTexturecoordinates(texturecoordinates, 2);
    if (tangents.size()) vao->storeTangents(tangents, 3);

    if (indices.size()) vao->storeIndices(indices);
}

void myObject::displayObjects(myShader* shader, glm::mat4 view_matrix, const std::map<string, bool>& exclude)
{
    myassert(vao != nullptr);
    shader->setUniform("mymodel_matrix", model_matrix);
    shader->setUniform("mynormal_matrix", glm::transpose(glm::inverse(glm::mat3(view_matrix) * glm::mat3(model_matrix))));

    if (exclude.size() == 0)
    {
        for (std::unordered_multimap<string, mySubObject*>::iterator it = objects.begin(), end = objects.end(); it != end; ++it)
            it->second->displaySubObject(vao, shader);
    }
    else
    {
        for (std::unordered_multimap<string, mySubObject*>::iterator it = objects.begin(), end = objects.end(); it != end; ++it)
            if (!exclude.at(it->first))
                it->second->displaySubObject(vao, shader);
    }

}

void myObject::displayObjects(myShader* shader, glm::mat4 view_matrix, std::string name)
{
    if (vao == nullptr)
    {
        cout << "myVAO is empty. Nothing to draw.\n";
        return;
    }
    shader->setUniform("mymodel_matrix", model_matrix);
    shader->setUniform("mynormal_matrix", glm::transpose(glm::inverse(glm::mat3(view_matrix) * glm::mat3(model_matrix))));

    auto st = objects.equal_range(name);
    for (std::unordered_multimap<std::string, mySubObject*>::iterator it = st.first; it != st.second; ++it)
        it->second->displaySubObject(vao, shader);
}

void myObject::displayNormals(myShader* shader)
{
}

glm::vec3 myObject::closestVertex(glm::vec3 ray, glm::vec3 starting_point)
{
    float min = std::numeric_limits<float>::max();
    unsigned int min_index = 0;

    ray = glm::normalize(ray);
    for (unsigned int i = 0; i < vertices.size(); i++)
    {
        float dotp = glm::dot(ray, vertices[i] - starting_point);
        if (dotp < 0) continue;

        float oq = glm::distance(starting_point, vertices[i]);
        float d = oq * oq - dotp * dotp;
        if (d < min)
        {
            min = d;
            min_index = i;
        }
    }
    return vertices[min_index];
}

glm::vec3 myObject::objectAverage() const
{
    glm::vec3 avg(0., 0., 0.);
    for (unsigned int i = 0; i < vertices.size(); i++)
    {
        avg += vertices[i];
    }
    return (avg / ((float)(vertices.size())));
}

void myObject::locate(float x, float y, float z)
{
    model_matrix = glm::translate(model_matrix, glm::vec3(x, y, z));
    //model_matrix[0][3] = x;
    //model_matrix[1][3] = y;
    //model_matrix[2][3] = z;
}

void myObject::translate(float x, float y, float z)
{
    glm::mat4 tmp = glm::translate(glm::mat4(1.0f), glm::vec3(x, y, z));
    model_matrix = tmp * model_matrix;
}

void myObject::scale(float x, float y, float z)
{
    glm::mat4 tmp = glm::scale(glm::mat4(1.0f), glm::vec3(x, y, z));
    model_matrix = tmp * model_matrix;
}

void myObject::rotate(float axis_x, float axis_y, float axis_z, float angle)
{
    if (axis_x == 0.0f && axis_y == 0.0f && axis_z == 0.0f)
    {
        std::cout << "Error: rotation vector almost zero" << std::endl;
        return;
    }

    glm::mat4 tmp = glm::rotate(glm::mat4(1.0f), static_cast<float>(angle), glm::vec3(axis_x, axis_y, axis_z));
    model_matrix = tmp * model_matrix;
}

void myObject::locate(glm::vec3 t)
{
    locate(t.x, t.y, t.z);
}

void myObject::translate(glm::vec3 v)
{
    translate(v.x, v.y, v.z);
}

void myObject::scale(glm::vec3 v)
{
    scale(v.x, v.y, v.z);
}

void myObject::rotate(glm::vec3 v, float angle)
{
    rotate(v.x, v.y, v.z, angle);
}

glm::vec3 myObject::getPos() const
{
    return glm::vec3(model_matrix[3]);
}

void myObject::computeTexturecoordinates_plane()
{
    texturecoordinates.assign(vertices.size(), glm::vec2(0.0f, 0.0f));
    for (unsigned int i = 0; i < vertices.size(); i++)
    {
        texturecoordinates[i].s = vertices[i].x / 10.0f;
        texturecoordinates[i].t = vertices[i].y / 10.0f;
    }
}

void myObject::computeTexturecoordinates_cylinder()
{
    texturecoordinates.assign(vertices.size(), glm::vec2(0.0f, 0.0f));
    for (unsigned int i = 0; i < vertices.size(); i++)
    {
        float x = vertices[i].x;
        float y = vertices[i].y;
        float z = vertices[i].z;

        texturecoordinates[i].t = y - 0.5f;
        texturecoordinates[i].s = static_cast<float>((z >= 0.0f) ? atan2(z, x) / (M_PI) : (-atan2(z, x)) / (M_PI));
    }
}

void myObject::computeTexturecoordinates_sphere()
{
    texturecoordinates.assign(vertices.size(), glm::vec2(0.0f, 0.0f));
    for (unsigned int i = 0; i < vertices.size(); i++)
    {
        glm::vec3 v = glm::normalize(vertices[i]);

        texturecoordinates[i].t = static_cast<float>(-(atan2(2 * v.y, 2 * v.x) + M_PI) / (2 * M_PI));
        texturecoordinates[i].s = static_cast<float>(acos(v.z) / M_PI);
    }
}

void myObject::computeTangents()
{
    tangents.assign(vertices.size(), glm::vec3(0.0f, 0.0f, 0.0f));
    for (unsigned int i = 0; i < indices.size(); i++)
    {
        glm::vec2 t10 = texturecoordinates[indices[i][1]] - texturecoordinates[indices[i][0]];
        glm::vec2 t20 = texturecoordinates[indices[i][2]] - texturecoordinates[indices[i][0]];
        float f = t10.s * t20.t - t10.t * t20.s;
        if (f == 0) continue;
        f = 1.0f / f;
        glm::vec3 v10 = vertices[indices[i][1]] - vertices[indices[i][0]];
        glm::vec3 v20 = vertices[indices[i][2]] - vertices[indices[i][0]];
        glm::vec3 t(f * (t20.t * v10.x - t10.t * v20.x), f * (t20.t * v10.y - t10.t * v20.y),
            f * (t20.t * v10.z - t10.t * v20.z));

        tangents[indices[i][0]] += t;
        tangents[indices[i][1]] += t;
        tangents[indices[i][2]] += t;
    }
    for (unsigned int i = 0; i < vertices.size(); i++)  tangents[i] = glm::normalize(tangents[i]);
}

void myObject::setTexture(myTexture* tex, mySubObject::TEXTURE_TYPE type)
{
    for (std::unordered_multimap<std::string, mySubObject*>::iterator it = objects.begin(); it != objects.end(); ++it)
        it->second->setTexture(tex, type);
}

float myObject::closestTriangle(glm::vec3 ray, glm::vec3 origin, size_t& picked_triangle)
{
    mySubObject* tmp;
    return closestTriangle(ray, origin, picked_triangle, tmp);
}

float myObject::closestTriangle(glm::vec3 ray, glm::vec3 origin, size_t& picked_triangle, mySubObject*& picked_object)
{
    float min_t = std::numeric_limits<float>::max();
    picked_triangle = 0;
    picked_object = nullptr;

    for (std::unordered_multimap<std::string, mySubObject*>::iterator it = objects.begin(); it != objects.end(); ++it)
    {
        mySubObject* obj = it->second;
        for (size_t i = obj->start; i < obj->end; i++)
        {
            glm::vec2 intersection_point;
            vector<glm::vec3> verts(3);
            for (unsigned int j : {0, 1, 2})
            {
                glm::vec4 t = model_matrix * glm::vec4(vertices[indices[i][j]], 1.0f);
                verts[j] = glm::vec3(t.x / t.w, t.y / t.w, t.z / t.w);
            }
            float dst;
            bool intersect = glm::intersectRayTriangle(origin, ray, verts[0], verts[1], verts[2], intersection_point, dst);
            if (intersect)
            {
                float t = dst;//intersection_point.z;
                if (t >= 0 && t < min_t)
                {
                    min_t = t;
                    picked_triangle = i;
                    picked_object = obj;
                }
            }
        }
    }

    if (picked_triangle == 0) return -1;
    else return min_t;
}


// ROS includes.
#include <ros/ros.h>
#include <ros/time.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>
#include <sensor_msgs/CameraInfo.h>
#include <cv_bridge/cv_bridge.h>

#include <string.h>
#include <iostream>
#include <fstream>
#include <math.h>

#include <boost/thread.hpp>

#include "fovis.hpp"

#include "rwth_perception_people_msgs/VisualOdometry.h"

using namespace std;
using namespace sensor_msgs;
using namespace message_filters;
using namespace rwth_perception_people_msgs;

ros::Publisher pub_message;
fovis::VisualOdometry* odom = NULL;
fovis::CameraIntrinsicsParameters cam_params;

cv::Mat img_depth_;
cv_bridge::CvImagePtr cv_depth_ptr;	// cv_bridge for depth image

bool recover = false;
bool first = true;

void callback(const ImageConstPtr &image, const ImageConstPtr &depth, const CameraInfoConstPtr &info)
{
    // Initialising odomometry at first callback call or for recovery
    if(odom == NULL)
    {
        // Only needs to be done once at startup and not for recovery
        if(first) {
            first = false;
            memset(&cam_params, 0, sizeof(fovis::CameraIntrinsicsParameters));
            cam_params.width = info->width;
            cam_params.height = info->height;
            cam_params.fx = info->K[0];
            cam_params.fy = info->K[4];
            cam_params.cx = info->K[2];
            cam_params.cy = info->K[5];
        }

        fovis::Rectification* fovis_rect;

        fovis_rect = new fovis::Rectification(cam_params);
        fovis::VisualOdometryOptions options = fovis::VisualOdometry::getDefaultOptions();
        odom = new fovis::VisualOdometry(fovis_rect, options);
    }

    cv_depth_ptr = cv_bridge::toCvCopy(depth);
    img_depth_ = cv_depth_ptr->image;

    ros::WallTime start_time = ros::WallTime::now();
    fovis::DepthImage* fv_dp = new fovis::DepthImage(cam_params,info->width,info->height);
    fv_dp->setDepthImage((float*)(img_depth_.data));

    odom->processFrame(&image->data[0], fv_dp);

    Eigen::Isometry3d cam_to_local = odom->getPose();
    Eigen::Matrix4d m1 = cam_to_local.matrix().inverse();

    VisualOdometry fovis_info_msg;
    fovis_info_msg.header = image->header;

    fovis_info_msg.change_reference_frame = odom->getChangeReferenceFrames();
    fovis_info_msg.fast_threshold = odom->getFastThreshold();
    const fovis::OdometryFrame* frame = odom->getTargetFrame();
    fovis_info_msg.num_total_detected_keypoints = frame->getNumDetectedKeypoints();
    fovis_info_msg.num_total_keypoints = frame->getNumKeypoints();
    fovis_info_msg.num_detected_keypoints.resize(frame->getNumLevels());
    fovis_info_msg.num_keypoints.resize(frame->getNumLevels());
    for (int i = 0; i < frame->getNumLevels(); ++i)
    {
        fovis_info_msg.num_detected_keypoints[i] = frame->getLevel(i)->getNumDetectedKeypoints();
        fovis_info_msg.num_keypoints[i] = frame->getLevel(i)->getNumKeypoints();
    }
    const fovis::MotionEstimator* estimator = odom->getMotionEstimator();
    fovis_info_msg.motion_estimate_status_code = estimator->getMotionEstimateStatus();
    fovis_info_msg.motion_estimate_status = fovis::MotionEstimateStatusCodeStrings[fovis_info_msg.motion_estimate_status_code];
    fovis_info_msg.num_matches = estimator->getNumMatches();
    fovis_info_msg.num_inliers = estimator->getNumInliers();
    fovis_info_msg.num_reprojection_failures = estimator->getNumReprojectionFailures();
    fovis_info_msg.motion_estimate_valid = estimator->isMotionEstimateValid();
    ros::WallDuration time_elapsed = ros::WallTime::now() - start_time;
    fovis_info_msg.runtime = time_elapsed.toSec();

    fovis_info_msg.transformation_matrix.resize(4*4);
    for(int i = 0; i < 4*4; i++)
    {
        fovis_info_msg.transformation_matrix[i] = m1.data()[i];
        if(isnan(m1.data()[i])) { // Detected nan values and trigger recovery
            recover = true;
        }
    }

    pub_message.publish(fovis_info_msg);
    delete fv_dp;

    // delete odometry and reset at next callback call.
    if(recover) {
        ROS_WARN("Detected 'nan' values in motion matrix. Will try to auto recover by resetting visual odometry.");
        recover = false;
        delete odom;
        odom = NULL;
    }
}

// Connection callback that unsubscribes from the tracker if no one is subscribed.
void connectCallback(message_filters::Subscriber<CameraInfo> &sub_cam,
                     image_transport::SubscriberFilter &sub_mon,
                     image_transport::SubscriberFilter &sub_dep,
                     image_transport::ImageTransport &it){
    if(!pub_message.getNumSubscribers()) {
        ROS_DEBUG("Visual Odometry: No subscribers. Unsubscribing.");
        sub_cam.unsubscribe();
        sub_mon.unsubscribe();
        sub_dep.unsubscribe();
    } else {
        ROS_DEBUG("Visual Odometry: New subscribers. Subscribing.");
        sub_cam.subscribe();
        sub_mon.subscribe(it,sub_mon.getTopic().c_str(),1);
        sub_dep.subscribe(it,sub_dep.getTopic().c_str(),1);
    }
}

int main(int argc, char **argv)
{
    // Set up ROS.
    ros::init(argc, argv, "visual_odometry");
    ros::NodeHandle n;

    // Declare variables that can be modified by launch file or command line.
    int queue_size;
    string cam_ns;
    string pub_topic;

    // Initialize node parameters from launch file or command line.
    // Use a private node handle so that multiple instances of the node can be run simultaneously
    // while using different parameters.
    ros::NodeHandle private_node_handle_("~");
    private_node_handle_.param("queue_size", queue_size, int(5));
    private_node_handle_.param("camera_namespace", cam_ns, string("/head_xtion"));

    string topic_image_mono = cam_ns + "/rgb/image_mono";
    string topic_depth_image = cam_ns + "/depth/image";
    string topic_camera_info = cam_ns + "/rgb/camera_info";

    ROS_DEBUG("visual_odometry: Queue size for synchronisation is set to: %i", queue_size);

    // Image transport handle
    image_transport::ImageTransport it(private_node_handle_);

    // Create a subscriber.
    // Set queue size to 1 because generating a queue here will only pile up images and delay the output by the amount of queued images
    image_transport::SubscriberFilter subscriber_mono;
    subscriber_mono.subscribe(it, topic_image_mono.c_str(), 1); subscriber_mono.unsubscribe();
    image_transport::SubscriberFilter subscriber_depth;
    subscriber_depth.subscribe(it, topic_depth_image.c_str(), 1); subscriber_depth.unsubscribe();
    Subscriber<CameraInfo> subscriber_camera_info(n, topic_camera_info.c_str(), 1); subscriber_camera_info.unsubscribe();

    ros::SubscriberStatusCallback con_cb = boost::bind(&connectCallback,
                                                       boost::ref(subscriber_camera_info),
                                                       boost::ref(subscriber_mono),
                                                       boost::ref(subscriber_depth),
                                                       boost::ref(it));

    //The real queue size for synchronisation is set here.
    sync_policies::ApproximateTime<Image, Image, CameraInfo> MySyncPolicy(queue_size);
    MySyncPolicy.setAgePenalty(1000); //set high age penalty to publish older data faster even if it might not be correctly synchronized.

    const sync_policies::ApproximateTime<Image, Image, CameraInfo> MyConstSyncPolicy = MySyncPolicy;

    Synchronizer< sync_policies::ApproximateTime<Image, Image, CameraInfo> > sync(MyConstSyncPolicy,
                                                                                subscriber_mono, subscriber_depth, subscriber_camera_info);

    sync.registerCallback(boost::bind(&callback, _1, _2, _3));
    // Create a topic publisher
    private_node_handle_.param("motion_parameters", pub_topic, string("/visual_odometry/motion_matrix"));
    pub_message = n.advertise<VisualOdometry>(pub_topic.c_str(), 10, con_cb, con_cb);

    ros::spin();

    return 0;
}