// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*

    This example program shows how to find frontal human faces in an image and
    estimate their pose.  The pose takes the form of 68 landmarks.  These are
    points on the face such as the corners of the mouth, along the eyebrows, on
    the eyes, and so forth.

    This example is essentially just a version of the face_landmark_detection_ex.cpp
    example modified to use OpenCV's VideoCapture object to read from a camera instead
    of files.

    Finally, note that the face detector is fastest when compiled with at least
    SSE2 instructions enabled.  So if you are using a PC with an Intel or AMD
    chip then you should enable at least SSE2 instructions.  If you are using
    cmake to compile this program you can enable them by using one of the
    following commands when you create the build project:
        cmake path_to_dlib_root/examples -DUSE_SSE2_INSTRUCTIONS=ON
        cmake path_to_dlib_root/examples -DUSE_SSE4_INSTRUCTIONS=ON
        cmake path_to_dlib_root/examples -DUSE_AVX_INSTRUCTIONS=ON
    This will set the appropriate compiler options for GCC, clang, Visual
    Studio, or the Intel compiler.  If you are using another compiler then you
    need to consult your compiler's manual to determine how to enable these
    instructions.  Note that AVX is the fastest but requires a CPU from at least
    2011.  SSE4 is the next fastest and is supported by most current machines.
*/

#include <dlib/opencv.h>
#include <opencv2/highgui/highgui.hpp>
#include <dlib/image_processing/frontal_face_detector.h>
#include <dlib/image_processing/render_face_detections.h>
#include <dlib/image_processing.h>
#include <dlib/gui_widgets.h>
#include <dlib/image_io.h>
#include <iostream>
#include <opencv2/opencv.hpp>

using namespace dlib;
using namespace std;

int main()
{

    // 3D model points.
    std::vector<cv::Point3d> model_points;
    model_points.push_back(cv::Point3d(0.0f, 0.0f, 0.0f));               // Nose tip
    model_points.push_back(cv::Point3d(0.0f, -330.0f, -65.0f));          // Chin
    model_points.push_back(cv::Point3d(-225.0f, 170.0f, -135.0f));       // Left eye left corner
    model_points.push_back(cv::Point3d(225.0f, 170.0f, -135.0f));        // Right eye right corner
    model_points.push_back(cv::Point3d(-150.0f, -150.0f, -125.0f));      // Left Mouth corner
    model_points.push_back(cv::Point3d(150.0f, -150.0f, -125.0f));       // Right mouth corner

    try
    {
        cv::VideoCapture cap(0);
        cap.set(CV_CAP_PROP_FRAME_WIDTH, 1280);
        cap.set(CV_CAP_PROP_FRAME_HEIGHT, 720);
        if (!cap.isOpened())
        {
            cerr << "Unable to connect to camera" << endl;
            return 1;
        }

        image_window win;

        // Load face detection and pose estimation models.
        frontal_face_detector detector = get_frontal_face_detector();
        shape_predictor pose_model;
        deserialize("shape_predictor_68_face_landmarks.dat") >> pose_model;

        // Grab and process frames until the main window is closed by the user.
        while(!win.is_closed())
        {
            // Grab a frame
            cv::Mat temp;
            if (!cap.read(temp))
            {
                break;
            }
            double focal_length = temp.cols; // Approximate focal length.
            int iIC = temp.cols;
            int iIR = temp.rows;
            //std::cout<<iIC<< " " <<iIR << std::endl;
            cv::Point2d center = cv::Point2d(iIC/2,iIR/2);
            cv::Mat camera_matrix = (cv::Mat_<double>(3,3) << focal_length, 0, center.x, 0 , focal_length, center.y, 0, 0, 1);
            cv::Mat dist_coeffs = cv::Mat::zeros(4,1,cv::DataType<double>::type); // Assuming no lens distortion

            // Turn OpenCV's Mat into something dlib can deal with.  Note that this just
            // wraps the Mat object, it doesn't copy anything.  So cimg is only valid as
            // long as temp is valid.  Also don't do anything to temp that would cause it
            // to reallocate the memory which stores the image as that will make cimg
            // contain dangling pointers.  This basically means you shouldn't modify temp
            // while using cimg.
            cv_image<bgr_pixel> cimg(temp);

            // Detect faces
            std::vector<rectangle> faces = detector(cimg);
            // Find the pose of each face.
            std::vector<full_object_detection> shapes;
            for (unsigned long i = 0; i < faces.size(); ++i)
            {
            	full_object_detection shape = pose_model(cimg, faces[i]);
            	//cout << "number of parts: "<< shape.num_parts() << endl;
            	//cout << "pixel position of first part:  " << shape.part(0)(1) << endl;
            	//cout << "pixel position of second part: " << shape.part(1) << endl;
                shapes.push_back(shape);

                std::vector<cv::Point2d> image_points;
                image_points.push_back( cv::Point2d(shape.part(30)(0), shape.part(30)(1)) );    // Nose tip
                image_points.push_back( cv::Point2d(shape.part(8)(0), shape.part(8)(1)) );    // Chin
                image_points.push_back( cv::Point2d(shape.part(36)(0), shape.part(36)(1)) );     // Left eye left corner
                image_points.push_back( cv::Point2d(shape.part(45)(0), shape.part(45)(1)) );    // Right eye right corner
                image_points.push_back( cv::Point2d(shape.part(48)(0), shape.part(48)(1)) );    // Left Mouth corner
                image_points.push_back( cv::Point2d(shape.part(54)(0), shape.part(54)(1)) );    // Right mouth corner
                //cout << "++++++++" <<endl;
                //cout << image_points[0] << endl;

                //cout << "Camera Matrix " << endl << camera_matrix << endl ;
                // Output rotation and translation
                cv::Mat rotation_vector; // Rotation in axis-angle form
                cv::Mat translation_vector;

                // Solve for pose
                cv::solvePnP(model_points, image_points, camera_matrix, dist_coeffs, rotation_vector, translation_vector);

                // Project a 3D point (0, 0, 1000.0) onto the image plane.
                // We use this to draw a line sticking out of the nose

                std::vector<cv::Point3d> nose_end_point3D;
                std::vector<cv::Point2d> nose_end_point2D;
                nose_end_point3D.push_back(cv::Point3d(0,0,1000.0));

                projectPoints(nose_end_point3D, rotation_vector, translation_vector, camera_matrix, dist_coeffs, nose_end_point2D);

                for(int i=0; i < image_points.size(); i++)
                    {
                	 circle(temp, image_points[i], 3, cv::Scalar(0,0,255), -1);
                    }

                cv::line(temp,image_points[0], nose_end_point2D[0], cv::Scalar(255,0,0), 2);

                //cout << "Rotation Vector " << endl << rotation_vector << endl;
                //cout << "Translation Vector" << endl << translation_vector << endl;
                //cout <<  nose_end_point2D << endl;

            }
            cv_image<bgr_pixel> ncimg(temp);
            // Display it all on the screen
            win.clear_overlay();
            win.set_image(ncimg);
            win.add_overlay(render_face_detections(shapes));
        }
    }
    catch(serialization_error& e)
    {
        cout << "You need dlib's default face landmarking model file to run this example." << endl;
        cout << "You can get it from the following URL: " << endl;
        cout << "   http://dlib.net/files/shape_predictor_68_face_landmarks.dat.bz2" << endl;
        cout << endl << e.what() << endl;
    }
    catch(exception& e)
    {
        cout << e.what() << endl;
    }
}


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

#include "test_utils/read_steinlib.hpp"
#include "test_utils/test_result_check.hpp"

#include "paal/greedy/steiner_tree_greedy.hpp"
#include "paal/utils/functors.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/range/join.hpp>
#include <boost/range/algorithm/copy.hpp>
#include <boost/graph/filtered_graph.hpp>
#include <boost/graph/prim_minimum_spanning_tree.hpp>
#include <boost/graph/connected_components.hpp>

template <typename Edge> struct edge_filter {
    bool operator()(Edge e) const { return edges.find(e) != edges.end(); }
    std::set<Edge> edges;
};

BOOST_AUTO_TEST_CASE(steiner_tree_greedy_test) {

    std::vector<paal::steiner_tree_test_with_metric> data;
    LOGLN("READING INPUT...");
    read_steinlib_tests(data);
    for (auto const &test : data) {
        LOGLN("TEST " << test.test_name);
        LOGLN("OPT " << test.optimal);

        auto const &g = test.graph;
        auto N = num_vertices(g);
        using Graph = puretype(g);
        using Edge = boost::graph_traits<Graph>::edge_descriptor;
        edge_filter<Edge> e_filter;

        auto result = paal::steiner_tree_greedy(
            g, std::inserter(e_filter.edges, e_filter.edges.begin()));

        LOGLN("terminals:");
        LOG_COPY_RANGE_DEL(test.terminals, ",");
        LOGLN("");

        LOGLN("chosen edges:");
        LOG_COPY_RANGE_DEL(e_filter.edges, "'");
        LOGLN("");
        boost::filtered_graph<Graph, edge_filter<Edge>> fg(g, e_filter);

        std::vector<int> components(N);
        std::vector<int> color_map(N);
        auto index = get(boost::vertex_index, g);
        auto color_p_map =
            boost::make_iterator_property_map(color_map.begin(), index);
        boost::connected_components(fg, &components[0],
                                    boost::color_map(color_p_map));
        auto tree_color = components[test.terminals.front()];

        for (auto v : boost::make_iterator_range(vertices(fg))) {
            if (std::find(test.terminals.begin(), test.terminals.end(), v) !=
                test.terminals.end()) {
                BOOST_CHECK_EQUAL(components[v], tree_color);
            }
        }

        auto weight = get(boost::edge_weight, g);
        int res(0);
        for (auto e : e_filter.edges) {
            res += weight(e);
        }
        int sum_weight =result.first;
        int lower_bound =result.second;
        BOOST_CHECK_EQUAL(res,sum_weight);
        BOOST_CHECK(lower_bound <= test.optimal);
        check_result(res, test.optimal, 2.);
    }
}


/*
 * testFindSeparator.cpp
 *
 *   Created on: Nov 23, 2010
 *       Author: nikai
 *  Description: unit tests for FindSeparator
 */

#include <boost/assign/std/list.hpp> // for operator +=
#include <boost/assign/std/set.hpp> // for operator +=
#include <boost/assign/std/vector.hpp> // for operator +=
using namespace boost::assign;
#include <boost/make_shared.hpp>
#include <CppUnitLite/TestHarness.h>

#include <gtsam_unstable/partition/FindSeparator-inl.h>
#include <gtsam_unstable/partition/GenericGraph.h>

using namespace std;
using namespace gtsam;
using namespace gtsam::partition;

/* ************************************************************************* */
// x0 - x1 - x2
// l3        l4
TEST ( Partition, separatorPartitionByMetis )
{
  GenericGraph2D graph;
  graph.push_back(std::make_shared<GenericFactor2D>(0, NODE_POSE_2D, 3, NODE_LANDMARK_2D));
  graph.push_back(std::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 4, NODE_LANDMARK_2D));
  graph.push_back(std::make_shared<GenericFactor2D>(0, NODE_POSE_2D, 1, NODE_POSE_2D));
  graph.push_back(std::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 2, NODE_POSE_2D));
  std::vector<size_t> keys; keys += 0, 1, 2, 3, 4;

  WorkSpace workspace(5);
  boost::optional<MetisResult> actual = separatorPartitionByMetis<GenericGraph2D>(graph, keys,
   workspace, true);

  CHECK(actual.is_initialized());
  vector<size_t> A_expected; A_expected += 0, 3; // frontal
  vector<size_t> B_expected; B_expected += 2, 4; // frontal
  vector<size_t> C_expected; C_expected += 1;    // separator
  CHECK(A_expected == actual->A);
  CHECK(B_expected == actual->B);
  CHECK(C_expected == actual->C);
}

/* ************************************************************************* */
// x1 - x2 - x3,     variable  not used x0, x4, l7
// l5        l6
TEST ( Partition, separatorPartitionByMetis2 )
{
  GenericGraph2D graph;
  graph.push_back(std::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
  graph.push_back(std::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 6, NODE_LANDMARK_2D));
  graph.push_back(std::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 2, NODE_POSE_2D));
  graph.push_back(std::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 3, NODE_POSE_2D));
  std::vector<size_t> keys; keys += 1, 2, 3, 5, 6;

  WorkSpace workspace(8);
  boost::optional<MetisResult> actual = separatorPartitionByMetis<GenericGraph2D>(graph, keys,
   workspace, true);

  CHECK(actual.is_initialized());
  vector<size_t> A_expected; A_expected += 1, 5; // frontal
  vector<size_t> B_expected; B_expected += 3, 6; // frontal
  vector<size_t> C_expected; C_expected += 2;    // separator
  CHECK(A_expected == actual->A);
  CHECK(B_expected == actual->B);
  CHECK(C_expected == actual->C);
}

/* *************************************************************************/
// x0 - x1 - x2 - x3
TEST ( Partition, edgePartitionByMetis )
{
  GenericGraph3D graph;
  graph.push_back(std::make_shared<GenericFactor3D>(0, 1, 0, NODE_POSE_3D, NODE_POSE_3D));
  graph.push_back(std::make_shared<GenericFactor3D>(1, 2, 1, NODE_POSE_3D, NODE_POSE_3D));
  graph.push_back(std::make_shared<GenericFactor3D>(2, 3, 2, NODE_POSE_3D, NODE_POSE_3D));
  std::vector<size_t> keys; keys += 0, 1, 2, 3;

  WorkSpace workspace(6);
  boost::optional<MetisResult> actual = edgePartitionByMetis<GenericGraph3D>(graph, keys,
   workspace, true);

  CHECK(actual.is_initialized());
  vector<size_t> A_expected; A_expected += 0, 1; // frontal
  vector<size_t> B_expected; B_expected += 2, 3; // frontal
  vector<size_t> C_expected;    // separator
//  for(const size_t a: actual->A)
//    cout << a << " ";
//  cout << endl;
//  for(const size_t b: actual->B)
//    cout << b << " ";
//  cout << endl;

  CHECK(A_expected == actual->A || A_expected == actual->B);
  CHECK(B_expected == actual->B || B_expected == actual->A);
  CHECK(C_expected == actual->C);
}

/* *************************************************************************/
// x0 - x1 - x2 - x3 - x4
TEST ( Partition, edgePartitionByMetis2 )
{
  GenericGraph3D graph;
  graph.push_back(std::make_shared<GenericFactor3D>(0, 1, 0, NODE_POSE_3D, NODE_POSE_3D, 1));
  graph.push_back(std::make_shared<GenericFactor3D>(1, 2, 1, NODE_POSE_3D, NODE_POSE_3D, 1));
  graph.push_back(std::make_shared<GenericFactor3D>(2, 3, 2, NODE_POSE_3D, NODE_POSE_3D, 20));
  graph.push_back(std::make_shared<GenericFactor3D>(3, 4, 3, NODE_POSE_3D, NODE_POSE_3D, 1));
  std::vector<size_t> keys; keys += 0, 1, 2, 3, 4;

  WorkSpace workspace(6);
  boost::optional<MetisResult> actual = edgePartitionByMetis<GenericGraph3D>(graph, keys,
   workspace, true);
  CHECK(actual.is_initialized());
  vector<size_t> A_expected; A_expected += 0, 1; // frontal
  vector<size_t> B_expected; B_expected += 2, 3, 4; // frontal
  vector<size_t> C_expected;    // separator
  CHECK(A_expected == actual->A);
  CHECK(B_expected == actual->B);
  CHECK(C_expected == actual->C);
}

/* ************************************************************************* */
// x0 - x1 - x2
// l3        l4
TEST ( Partition, findSeparator )
{
  GenericGraph2D graph;
  graph.push_back(std::make_shared<GenericFactor2D>(0, NODE_POSE_2D, 3, NODE_LANDMARK_2D));
  graph.push_back(std::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 4, NODE_LANDMARK_2D));
  graph.push_back(std::make_shared<GenericFactor2D>(0, NODE_POSE_2D, 1, NODE_POSE_2D));
  graph.push_back(std::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 2, NODE_POSE_2D));
  std::vector<size_t> keys; keys += 0, 1, 2, 3, 4;

  WorkSpace workspace(5);
  int minNodesPerMap = -1;
  bool reduceGraph = false;
  int numSubmaps = findSeparator<GenericGraph2D>(graph, keys, minNodesPerMap, workspace,
    false, boost::none, reduceGraph, 0, 0);
  LONGS_EQUAL(2, numSubmaps);
  LONGS_EQUAL(5, workspace.partitionTable.size());
  LONGS_EQUAL(1, workspace.partitionTable[0]);
  LONGS_EQUAL(0, workspace.partitionTable[1]);
  LONGS_EQUAL(2, workspace.partitionTable[2]);
  LONGS_EQUAL(1, workspace.partitionTable[3]);
  LONGS_EQUAL(2, workspace.partitionTable[4]);
}

/* ************************************************************************* */
// x1 - x2 - x3,     variable  not used x0, x4, l7
// l5        l6
TEST ( Partition, findSeparator2 )
{
  GenericGraph2D graph;
  graph.push_back(std::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
  graph.push_back(std::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 6, NODE_LANDMARK_2D));
  graph.push_back(std::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 2, NODE_POSE_2D));
  graph.push_back(std::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 3, NODE_POSE_2D));
  std::vector<size_t> keys; keys += 1, 2, 3, 5, 6;

  WorkSpace workspace(8);
  int minNodesPerMap = -1;
  bool reduceGraph = false;
  int numSubmaps = findSeparator<GenericGraph2D>(graph, keys, minNodesPerMap, workspace,
    false, boost::none, reduceGraph, 0, 0);
  LONGS_EQUAL(2, numSubmaps);
  LONGS_EQUAL(8, workspace.partitionTable.size());
  LONGS_EQUAL(-1,workspace.partitionTable[0]);
  LONGS_EQUAL(1, workspace.partitionTable[1]);
  LONGS_EQUAL(0, workspace.partitionTable[2]);
  LONGS_EQUAL(2, workspace.partitionTable[3]);
  LONGS_EQUAL(-1,workspace.partitionTable[4]);
  LONGS_EQUAL(1, workspace.partitionTable[5]);
  LONGS_EQUAL(2, workspace.partitionTable[6]);
  LONGS_EQUAL(-1,workspace.partitionTable[7]);
}

/* *************************************************************************/
/**
 *  l1-l8   l9-l16    l17-l24
 *   / |     /    \    |  \
 * x25 x26             x27 x28
 */
TEST ( Partition, findSeparator3_with_reduced_camera )
{
  GenericGraph3D graph;
  for (int j=1; j<=8; j++)
    graph.push_back(std::make_shared<GenericFactor3D>(25, j));
  for (int j=1; j<=16; j++)
    graph.push_back(std::make_shared<GenericFactor3D>(26, j));
  for (int j=9; j<=24; j++)
    graph.push_back(std::make_shared<GenericFactor3D>(27, j));
  for (int j=17; j<=24; j++)
    graph.push_back(std::make_shared<GenericFactor3D>(28, j));

  std::vector<size_t> keys;
  for(int i=1; i<=28; i++)
    keys.push_back(i);

  vector<Symbol> int2symbol;
  int2symbol.push_back(Symbol('x',0)); // dummy
  for(int i=1; i<=24; i++)
    int2symbol.push_back(Symbol('l',i));
  int2symbol.push_back(Symbol('x',25));
  int2symbol.push_back(Symbol('x',26));
  int2symbol.push_back(Symbol('x',27));
  int2symbol.push_back(Symbol('x',28));

  WorkSpace workspace(29);
  bool reduceGraph = true;
  int numIsland = findSeparator(graph, keys, 3, workspace, false, int2symbol, reduceGraph, 0, 0);
  LONGS_EQUAL(2, numIsland);

  partition::PartitionTable& partitionTable = workspace.partitionTable;
  for (int j=1; j<=8; j++)
    LONGS_EQUAL(1, partitionTable[j]);
  for (int j=9; j<=16; j++)
    LONGS_EQUAL(0, partitionTable[j]);
  for (int j=17; j<=24; j++)
    LONGS_EQUAL(2, partitionTable[j]);
  LONGS_EQUAL(1, partitionTable[25]);
  LONGS_EQUAL(1, partitionTable[26]);
  LONGS_EQUAL(2, partitionTable[27]);
  LONGS_EQUAL(2, partitionTable[28]);
}

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


/*=========================================================================

Program:   Visualization Toolkit
Module:    vtkBoostKruskalMinimumSpanningTree.cxx

Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
All rights reserved.
See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

This software is distributed WITHOUT ANY WARRANTY; without even
the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
PURPOSE.  See the above copyright notice for more information.

=========================================================================*/
/*-------------------------------------------------------------------------
  Copyright 2008 Sandia Corporation.
  Under the terms of Contract DE-AC04-94AL85000 with Sandia Corporation,
  the U.S. Government retains certain rights in this software.
  -------------------------------------------------------------------------*/
#include "vtkBoostKruskalMinimumSpanningTree.h"

#include "vtkBoostGraphAdapter.h"
#include "vtkCellArray.h"
#include "vtkCellData.h"
#include "vtkMath.h"
#include "vtkIdTypeArray.h"
#include "vtkInformation.h"
#include "vtkInformationVector.h"
#include "vtkObjectFactory.h"
#include "vtkPointData.h"
#include "vtkPoints.h"
#include "vtkFloatArray.h"
#include "vtkDataArray.h"
#include "vtkSelection.h"
#include "vtkSelectionNode.h"
#include "vtkSmartPointer.h"
#include "vtkStringArray.h"
#include "vtkMutableDirectedGraph.h"
#include "vtkUndirectedGraph.h"
#include "vtkTree.h"

#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include <boost/vector_property_map.hpp>
#include <boost/pending/queue.hpp>

using namespace boost;

vtkCxxRevisionMacro(vtkBoostKruskalMinimumSpanningTree, "1.4");
vtkStandardNewMacro(vtkBoostKruskalMinimumSpanningTree);

// Constructor/Destructor
vtkBoostKruskalMinimumSpanningTree::vtkBoostKruskalMinimumSpanningTree()
{
  this->EdgeWeightArrayName = 0;
  this->OutputSelectionType = 0;
  this->SetOutputSelectionType("MINIMUM_SPANNING_TREE_EDGES");
  this->NegateEdgeWeights = false;
  this->EdgeWeightMultiplier = 1;
}

vtkBoostKruskalMinimumSpanningTree::~vtkBoostKruskalMinimumSpanningTree()
{
  this->SetEdgeWeightArrayName(0);
}

void vtkBoostKruskalMinimumSpanningTree::SetNegateEdgeWeights(bool value)
{
  this->NegateEdgeWeights = value;
  if (this->NegateEdgeWeights)
    this->EdgeWeightMultiplier = -1.0;
  else
    this->EdgeWeightMultiplier = 1.0;

  this->Modified();
}

int vtkBoostKruskalMinimumSpanningTree::RequestData(
  vtkInformation *vtkNotUsed(request),
  vtkInformationVector **inputVector,
  vtkInformationVector *outputVector)
{
  // get the info objects
  vtkInformation *inInfo = inputVector[0]->GetInformationObject(0);
  vtkInformation *outInfo = outputVector->GetInformationObject(0);

  // get the input and output
  vtkGraph *input = vtkGraph::SafeDownCast(
    inInfo->Get(vtkDataObject::DATA_OBJECT()));
  vtkSelection *output = vtkSelection::SafeDownCast(
    outInfo->Get(vtkDataObject::DATA_OBJECT()));

  // Retrieve the edge-weight array.
  if (!this->EdgeWeightArrayName)
    {
    vtkErrorMacro("Edge-weight array name is required");
    return 0;
    }
  vtkDataArray* edgeWeightArray = input->GetEdgeData()->GetArray(this->EdgeWeightArrayName);

  // Does the edge-weight array exist at all?
  if (edgeWeightArray == NULL)
    {
    vtkErrorMacro("Could not find edge-weight array named "
                  << this->EdgeWeightArrayName);
    return 0;
    }

  // Send the property map through both the multiplier and the
  // helper (for edge_descriptor indexing)
  typedef vtkGraphPropertyMapMultiplier<vtkDataArray*> mapMulti;
  mapMulti multi(edgeWeightArray,this->EdgeWeightMultiplier);
  vtkGraphEdgePropertyMapHelper<mapMulti> weight_helper(multi);

  // Run the algorithm
  vtkstd::vector<vtkEdgeType> mstEdges;
  if (vtkDirectedGraph::SafeDownCast(input))
    {
    vtkDirectedGraph *g = vtkDirectedGraph::SafeDownCast(input);
    kruskal_minimum_spanning_tree(g, vtkstd::back_inserter(mstEdges), weight_map(weight_helper));
    }
  else
    {
    vtkUndirectedGraph *g = vtkUndirectedGraph::SafeDownCast(input);
    kruskal_minimum_spanning_tree(g, vtkstd::back_inserter(mstEdges), weight_map(weight_helper));
    }

  // Select the minimum spanning tree edges.
  if (!strcmp(OutputSelectionType,"MINIMUM_SPANNING_TREE_EDGES"))
    {
    vtkIdTypeArray* ids = vtkIdTypeArray::New();

    // Add the ids of each MST edge.
    for (vtkstd::vector<vtkEdgeType>::iterator i = mstEdges.begin();
         i != mstEdges.end(); ++i)
      {
      ids->InsertNextValue(i->Id);
      }

    vtkSmartPointer<vtkSelectionNode> node =
      vtkSmartPointer<vtkSelectionNode>::New();
    output->AddNode(node);
    node->SetSelectionList(ids);
    node->SetContentType(vtkSelectionNode::INDICES);
    node->SetFieldType(vtkSelectionNode::EDGE);
    ids->Delete();
    }

  return 1;
}

//----------------------------------------------------------------------------
int vtkBoostKruskalMinimumSpanningTree::FillInputPortInformation(
  int port, vtkInformation* info)
{
  // now add our info
  if (port == 0)
    {
    info->Set(vtkAlgorithm::INPUT_REQUIRED_DATA_TYPE(), "vtkGraph");
    }
  return 1;
}

//----------------------------------------------------------------------------
int vtkBoostKruskalMinimumSpanningTree::FillOutputPortInformation(
  int port, vtkInformation* info)
{
  // now add our info
  if (port == 0)
    {
    info->Set(vtkDataObject::DATA_TYPE_NAME(), "vtkSelection");
    }
  return 1;
}

//----------------------------------------------------------------------------
void vtkBoostKruskalMinimumSpanningTree::PrintSelf(ostream& os, vtkIndent indent)
{
  this->Superclass::PrintSelf(os, indent);

  os << indent << "EdgeWeightArrayName: "
     << (this->EdgeWeightArrayName ? this->EdgeWeightArrayName : "(none)")
     << endl;

  os << indent << "OutputSelectionType: "
     << (this->OutputSelectionType ? this->OutputSelectionType : "(none)") << endl;

  os << indent << "NegateEdgeWeights: "
     << (this->NegateEdgeWeights ? "true" : "false") << endl;

  os << indent << "EdgeWeightMultiplier: " << this->EdgeWeightMultiplier << endl;
}


#pragma once

#include <string>
#include <iostream>
#include <boost/program_options.hpp>
#include <fftw3.h>
#include "utilities.hpp"
#include "mpi_environment.hpp"

struct wisdom_sync {

	const int &fftw_flags = _fftw_flags;

	wisdom_sync(const std::string& default_mode = "sync"): default_mode(default_mode) {}

	void add_options(boost::program_options::options_description &options) {
		options.add_options()
				("wisdom_mode", boost::program_options::value(&wisdom_mode)->default_value(default_mode),
				 "Mode for wisdom syncing across MPI processes: \"sync\" calculates wisdom once and propagates to all processes, \"none\" does not propagate wisdom, other values are interpreted as filenames to read wisdom from");
	}

	void gather() const {
		fftw_forget_wisdom();
		if (wisdom_mode == "sync") {
			if (!mpi_global_coord) return;
			std::string wisdom;
			boost::mpi::broadcast(mpi_global, wisdom, 0);
			fftw_import_wisdom_from_string(wisdom.c_str());
			_fftw_flags = FFTW_WISDOM_ONLY;
		} else if (wisdom_mode != "none") {
			fftw_import_wisdom_from_filename(wisdom_mode.c_str());
			_fftw_flags = FFTW_WISDOM_ONLY;
		}
	}

	void scatter() const {
		std::string wisdom = fftw_export_wisdom_to_string();
		if (wisdom_mode == "sync" && !mpi_global_coord) boost::mpi::broadcast(mpi_global, wisdom, 0);
		if (gconf.verbose && (wisdom_mode == "none" || !mpi_global_coord))
			collect_ostream(std::cerr) << process_ident << ": FFTW wisdom:" << std::endl << wisdom << std::endl;
	}

private:

	mutable int _fftw_flags = 0;
	std::string wisdom_mode, default_mode;

};


/*
 * protonReconstruct.cpp
 *
 *  Created on: Dec 22, 2011
 *      Author: u051747
 */
#include <iostream>
#include <boost/program_options.hpp>
#include "cuNDArray.h"
#include "cuCgSolver.h"
#include "cuImageOperator.h"
#include "splineBackprojectionOperator.h"
#include "cuPartialDerivativeOperator.h"
#include "cuLaplaceOperator.h"
#include "hoNDArray_fileio.h"
#include "check_CUDA.h"
#include "cuGpBbSolver.h"
#include "identityOperator.h"

#include "hoNDArray_math.h"

#include <sstream>
#include "hdf5_utils.h"

#include "encodingOperatorContainer.h"
#include "cuSARTSolver.h"
#include "cuMLSolver.h"
#include "cuNCGSolver.h"
#include "cuNlcgSolver.h"
#include "cuCgSolver.h"
#include "vector_td_io.h"
#include "protonPreconditioner.h"
#include "cuLbfgsSolver.h"
#include "solver_utils.h"

#include "cuTvOperator.h"

#include "circulantPreconditioner.h"

#include "ADMM.h"

using namespace std;
using namespace Gadgetron;

template <class T> class EXPORTGPUSOLVERS cuLbfgsSolver2 : public lbfgsSolver<cuNDArray<T> >
  {
  public:

    cuLbfgsSolver2() : lbfgsSolver<cuNDArray<T> >() {}
    virtual ~cuLbfgsSolver2() {}

    virtual void solver_non_negativity_filter(cuNDArray<T> *x,cuNDArray<T> *g){
    	::solver_non_negativity_filter(x,g);
    }
    virtual void iteration_callback(cuNDArray<T>* x ,int iteration,typename realType<T>::Type value){
  	  if (iteration == 0){
  		  std::ofstream textFile("residual.txt",std::ios::trunc);
  	  	  textFile << value << std::endl;
  	  } else{
  		  std::ofstream textFile("residual.txt",std::ios::app);
  		  textFile << value << std::endl;
  	  }

   	  std::stringstream ss;
   	  ss << "LBFGS-" << iteration << ".real";
   	  write_nd_array(x->to_host().get(),ss.str().c_str());

    };
  };

template <class T> class EXPORTGPUSOLVERS cuNlcgSolver2 : public cuNlcgSolver<T >
  {
  public:

    cuNlcgSolver2() : cuNlcgSolver<T >() {}
    virtual ~cuNlcgSolver2() {}

    virtual void iteration_callback(cuNDArray<T>* x ,int iteration,typename realType<T>::Type value, typename realType<T>::Type val2){
  	  if (iteration == 0){
  		  std::ofstream textFile("residual.txt",std::ios::trunc);
  	  	  textFile << value << std::endl;
  	  } else{
  		  std::ofstream textFile("residual.txt",std::ios::app);
  		  textFile << value << std::endl;
  	  }

   	  std::stringstream ss;
   	  ss << "NLCG-" << iteration << ".real";
   	  write_nd_array(x->to_host().get(),ss.str().c_str());

    };
  };

/*
boost::shared_ptr< cuNDArray<float> >  recursiveSolver(cuNDArray<float> * rhs,cuCGSolver<float, float> * cg,int depth){
	std::cout << "Recursion depth " << depth << " reached" << std::endl;
	if (depth == 0){
		std::cout << "Running solver for depth " << depth  << std::endl;
		return cg->solve(rhs);
	} else {
		boost::shared_ptr< cuNDArray<float> > rhs_temp = cuNDA_downsample<float,2>(rhs);

		boost::shared_ptr< cuNDArray<float> > guess = recursiveSolver(rhs_temp.get(),cg,depth-1);
		guess = cuNDA_upsample<float,2>(guess.get());
		std::cout << "Running solver for depth " << depth  << std::endl;
		return cg->solve(rhs,guess.get());

	}

}
 */

/*
template<class T> void notify(T val){
	std::cout << val <<std::endl;
}
 */

template<class T> void notify(std::string val){
	std::cout << val << std::endl;
}

namespace po = boost::program_options;
int main( int argc, char** argv)
{

	//
	// Parse command line
	//
	float background =  0.00106;

	std::string projectionsName;
	std::string splinesName;
	std::string outputFile;
	vector_td<int,3> dimensions;
	vector_td<float,3> physical_dims;
	vector_td<float,3> origin;
	int iterations;
	int device;
	bool precon,use_hull,use_weights;
	po::options_description desc("Allowed options");
	desc.add_options()
      								("help", "produce help message")
      								("projections,p", po::value<std::string>(&projectionsName)->default_value("projections.real"), "File containing the projection data")
      								("splines,s", po::value<std::string>(&splinesName)->default_value("splines.real"), "File containing the spline trajectories")
      								("data,D", po::value<std::string>(), "HDF5 file containing projections and splines. Used instead of the *.real files")
      								("dimensions,d", po::value<vector_td<int,3> >(&dimensions)->default_value(vector_td<int,3>(512,512,1)), "Pixel dimensions of the image")
      								("size,S", po::value<vector_td<float,3> >(&physical_dims)->default_value(vector_td<float,3>(20,20,5)), "Dimensions of the image in cm")
      								("center,c", po::value<vector_td<float,3> >(&origin)->default_value(vector_td<float,3>(0,0,0)), "Center of the reconstruction")
      								("iterations,i", po::value<int>(&iterations)->default_value(10), "Dimensions of the image")
      								("output,f", po::value<std::string>(&outputFile)->default_value("image.hdf5"), "Output filename")
      								("prior,P", po::value<std::string>(),"Prior image filename")
      								("prior-weight,k",po::value<float>(),"Weight of the prior image")
      								("TV",po::value<float>(),"Total variation weight")
      								("weights,w",po::value<std::string>(),"File containing the variance of data")
      								("device",po::value<int>(&device)->default_value(0),"Number of the device to use (0 indexed)")
      								("preconditioner",po::value<bool>(&precon)->default_value(false),"Use preconditioner")
      								("use_hull",po::value<bool>(&use_hull)->default_value(true),"Estimate convex hull of object")
      								("use_weights",po::value<bool>(&use_weights)->default_value(true),"Use weights if available. Always true if variance is set")
      								;

	po::variables_map vm;
	po::store(po::parse_command_line(argc, argv, desc), vm);
	po::notify(vm);

	if (vm.count("help")) {
		cout << desc << "\n";
		return 1;
	}
	std::cout << "Command line options:" << std::endl;
	for (po::variables_map::iterator it = vm.begin(); it != vm.end(); ++it){
		boost::any a = it->second.value();
		std::cout << it->first << ": ";
		if (a.type() == typeid(std::string)) std::cout << it->second.as<std::string>();
		else if (a.type() == typeid(int)) std::cout << it->second.as<int>();
		else if (a.type() == typeid(float)) std::cout << it->second.as<float>();
		else if (a.type() == typeid(bool)) std::cout << it->second.as<bool>();
		else if (a.type() == typeid(vector_td<float,3>)) std::cout << it->second.as<vector_td<float,3> >();
		else if (a.type() == typeid(vector_td<int,3>)) std::cout << it->second.as<vector_td<int,3> >();
		else std::cout << "Unknown type" << std::endl;
		std::cout << std::endl;
	}

	cudaSetDevice(device);
	cudaDeviceReset();
	std::cout <<  std::endl;

	boost::shared_ptr<protonDataset<cuNDArray> > data;

	if (vm.count("data")){
		data = boost::shared_ptr<protonDataset<cuNDArray> >(new protonDataset<cuNDArray>(vm["data"].as<std::string>(),use_weights));

	} else {
		boost::shared_ptr<hoNDArray<vector_td<float,3> > > host_splines = read_nd_array< vector_td<float,3> >(splinesName.c_str());
		cout << "Number of spline elements: " << host_splines->get_number_of_elements() << endl;
		boost::shared_ptr< hoNDArray<float> > host_projections = read_nd_array<float >(projectionsName.c_str());
		//boost::shared_ptr<cuNDArray<float > > projections_old = projections;
		cout << "Number of elements " << host_projections->get_number_of_elements() << endl;
		cout << "Number of projection elements: " << host_projections->get_number_of_elements() << endl;
		if (vm.count("weights")){
			boost::shared_ptr< hoNDArray<float> > host_weights = read_nd_array<float >(vm["weights"].as<std::string>().c_str());
			if (host_weights->get_number_of_elements() != host_projections->get_number_of_elements())
				throw std::runtime_error("Number of elements in the variance vector does not match the number of projections ");
			reciprocal_inplace(host_weights.get());
			data = boost::shared_ptr<protonDataset<cuNDArray> >(new protonDataset<cuNDArray>(host_projections,host_splines,host_weights));

		} else data = boost::shared_ptr<protonDataset<cuNDArray> >(new protonDataset<cuNDArray>(host_projections,host_splines));

	}

	vector<size_t> ndims;
	ndims.push_back(3);

	//cuGpBbSolver<float> solver;
	cuNCGSolver<float> solver;
	//cuNlcgSolver2<float> solver;
	//cuCgSolver<float> solver;
	//cuLbfgsSolver2<float> solver;

	solver.set_max_iterations( iterations);
	solver.set_tc_tolerance((float)std::sqrt(1e-10));
	//solver.set_tc_tolerance((float)(1e-10));
	//solver.set_m(12);
	solver.set_output_mode( cuNCGSolver<float>::OUTPUT_VERBOSE );
	//if (!use_hull)
	//solver.set_non_negativity_constraint(true);

	vector<size_t> rhs_dims(&dimensions[0],&dimensions[3]); //Line to turn vector_td into std::vector.

	data->preprocess(rhs_dims,physical_dims,use_hull);

	if (data->get_weights())
		*data->get_projections() *= *data->get_weights(); //Have to scale projection data by weights before handing it to the solver. Write up the cost function and see why.

	if (use_hull) write_nd_array<float>(data->get_hull()->to_host().get(),"hull.real");

	boost::shared_ptr<cuNDArray<float> > rhs = data->get_projections();

	//E->set_codomain_dimensions(projections->get_dimensions().get());

	boost::shared_ptr< splineBackprojectionOperator<cuNDArray> > E(new splineBackprojectionOperator<cuNDArray>(data,physical_dims));
	E->set_domain_dimensions(&rhs_dims);

	boost::shared_ptr<encodingOperatorContainer<cuNDArray<float> > > enc (new encodingOperatorContainer<cuNDArray<float> >());

	if (precon){
		/*boost::shared_ptr<protonPreconditioner> P (new protonPreconditioner(rhs_dims));
		if (use_hull) P->set_hull(data->get_hull());*/
		std::cout << "Using preconditioner" << std::endl;
		boost::shared_ptr<circulantPreconditioner<cuNDArray,float> > P(new circulantPreconditioner<cuNDArray,float>(E));

		solver.set_preconditioner(P);
	}
	enc->add_operator(E);

	boost::shared_ptr<cuNDArray<float > > prior;
	if (vm.count("prior")){
		std::cout << "Prior image regularization in use" << std::endl;
		boost::shared_ptr<hoNDArray<float> > host_prior = read_nd_array<float >(vm["prior"].as<std::string>().c_str());

		host_prior->reshape(&rhs_dims);
		prior = boost::shared_ptr<cuNDArray<float> >(new cuNDArray<float>(host_prior.get()));
		float offset = float(0.01);
		//cuNDA_add(offset,prior.get());

		if (vm.count("prior-weight")){

			//boost::shared_ptr<cuImageOperator<float > > I (new cuImageOperator<float >());
			boost::shared_ptr<identityOperator<cuNDArray<float > > > I (new identityOperator<cuNDArray<float > >());
			//I->compute(prior.get());

			I->set_weight(vm["prior-weight"].as<float>());

			I->set_codomain_dimensions(&rhs_dims);
			I->set_domain_dimensions(&rhs_dims);
			cuNDArray<float> tmp = *prior;

			I->mult_M(prior.get(),&tmp);

			//cuNDA_scal(I->get_weight(),&tmp);
			std::vector<cuNDArray<float>* > proj;
			proj.push_back(rhs.get());
			proj.push_back(&tmp);
			enc->add_operator(I);
			rhs = enc->create_codomain(proj);

		} else {
			std::cout << "WARNING: Prior image set, but weight not specified" << std::endl;
		}
		solver.set_x0(prior);
	}

	if (vm.count("TV")){
			std::cout << "Total variation regularization in use" << std::endl;
			boost::shared_ptr<cuTvOperator<float,3> > tv(new cuTvOperator<float,3>);
			tv->set_weight(vm["TV"].as<float>());
			solver.add_nonlinear_operator(tv);

		}

	solver.set_encoding_operator(enc);
/*
	boost::shared_ptr< cuNDArray<float> > cgresult(new cuNDArray<float>(&rhs_dims));
	clear(cgresult.get());
	std::cout << "Groups: " << data->get_number_of_groups() << std::endl;
*/

	//E->mult_MH(data->get_projection_group(0).get(),cgresult.get());
	//E->mult_MH(rhs.get(),cgresult.get());
	//P->apply(cgresult.get(),cgresult.get());
	/*
	ADMM<cuNDArray<float>, cuCgSolver<float> > admm(&solver);
	admm.set_iterations(10);
	admm.set_weights(data->get_weights());
	E->set_use_weights(false);
	*/
	boost::shared_ptr< cuNDArray<float> > cgresult = solver.solve(rhs.get());
	//boost::shared_ptr< cuNDArray<float> > cgresult = admm.solve(rhs.get());

	//cuNDArray<float> tp = *projections;

	//E->mult_M(cgresult.get(),&tp);
	//axpy(-1.0f,projections.get(),&tp);
	//std::cout << "Total residual " << nrm2(&tp) << std::endl;

	std::cout << "GRadient norm: " << nrm2(cgresult.get()) << std::endl;
	boost::shared_ptr< hoNDArray<float> > host_result = cgresult->to_host();
	//write_nd_array<float>(host_result.get(), (char*)parms.get_parameter('f')->get_string_value());
	std::stringstream ss;
	for (int i = 0; i < argc; i++){
		ss << argv[i] << " ";
	}

	saveNDArray2HDF5<3>(host_result.get(),outputFile,physical_dims,origin,ss.str(), solver.get_max_iterations());

}


#pragma once

#include <polyfem/Common.hpp>
#include <polyfem/Mesh.hpp>

#include <polyfem/Navigation3D.hpp>
#include <polyfem/Mesh3DStorage.hpp>
#include <geogram/mesh/mesh.h>
#include <iostream>
#include <Eigen/Dense>
#include <vector>
#include <array>

namespace polyfem
{
	class Mesh3D : public Mesh
	{
	public:
		Mesh3D() = default;
		virtual ~Mesh3D() = default;
		POLYFEM_DEFAULT_MOVE_COPY(Mesh3D)

		void refine(const int n_refiniment, const double t, std::vector<int> &parent_nodes) override;

		inline bool is_volume() const override { return true; }

		int n_cells() const override { return int(mesh_.elements.size()); }
		int n_faces() const override { return int(mesh_.faces.size()); }
		int n_edges() const override { return int(mesh_.edges.size()); }
		int n_vertices() const override { return int(mesh_.points.cols()); }

		inline int n_face_vertices(const int f_id) const {return mesh_.faces[f_id].vs.size(); }
		inline int n_cell_vertices(const int c_id) const {return mesh_.elements[c_id].vs.size(); }
		inline int n_cell_faces(const int c_id) const {return mesh_.elements[c_id].fs.size(); }
		inline int cell_vertex(const int c_id, const int lv_id) const {return mesh_.elements[c_id].vs[lv_id]; }
		inline int cell_face(const int c_id, const int lf_id) const {return mesh_.elements[c_id].fs[lf_id]; }
		inline int cell_edge(const int c_id, const int le_id) const {return mesh_.elements[c_id].es[le_id]; }
		inline int face_vertex(const int f_id, const int lv_id) const {return mesh_.faces[f_id].vs[lv_id]; }

		bool is_boundary_vertex(const int vertex_global_id) const override { return mesh_.vertices[vertex_global_id].boundary; }
		bool is_boundary_edge(const int edge_global_id) const override { return mesh_.edges[edge_global_id].boundary; }
		bool is_boundary_face(const int face_global_id) const override { return mesh_.faces[face_global_id].boundary; }
		bool is_boundary_element(const int element_global_id) const override;

		bool save(const std::string &path) const override;
		bool save(const std::vector<int> &fs, const int ringN, const std::string &path) const;
		bool build_from_matrices(const Eigen::MatrixXd &V, const Eigen::MatrixXi &F) override;

		void attach_higher_order_nodes(const Eigen::MatrixXd &V, const std::vector<std::vector<int>> &nodes) override;
		RowVectorNd edge_node(const Navigation3D::Index &index, const int n_new_nodes, const int i) const;
		RowVectorNd face_node(const Navigation3D::Index &index, const int n_new_nodes, const int i, const int j) const;
		RowVectorNd cell_node(const Navigation3D::Index &index, const int n_new_nodes, const int i, const int j, const int k) const;

		void normalize() override;

		virtual double quad_area(const int gid) const override;
		virtual double tri_area(const int gid) const override;

		void compute_elements_tag() override;

		RowVectorNd kernel(const int cell_id) const;
		virtual RowVectorNd point(const int vertex_id) const override;
		virtual RowVectorNd edge_barycenter(const int e) const override;
		virtual RowVectorNd face_barycenter(const int f) const override;
		virtual RowVectorNd cell_barycenter(const int c) const override;

		virtual void bounding_box(RowVectorNd &min, RowVectorNd &max) const override;

		//navigation wrapper
		Navigation3D::Index get_index_from_element(int hi, int lf, int lv) const { return Navigation3D::get_index_from_element_face(mesh_, hi, lf, lv); }
		Navigation3D::Index get_index_from_element(int hi) const { return Navigation3D::get_index_from_element_face(mesh_, hi); }

		Navigation3D::Index get_index_from_element_edge(int hi, int v0, int v1) const { return Navigation3D::get_index_from_element_edge(mesh_, hi, v0, v1); }
		Navigation3D::Index get_index_from_element_face(int hi, int v0, int v1, int v2) const { return Navigation3D::get_index_from_element_tri(mesh_, hi, v0, v1, v2); }

		inline std::vector<uint32_t> vertex_neighs(const int v_gid) const {return mesh_.vertices[v_gid].neighbor_hs; }
		inline std::vector<uint32_t> edge_neighs(const int e_gid) const {return mesh_.edges[e_gid].neighbor_hs; }

		// Navigation in a surface mesh
		Navigation3D::Index switch_vertex(Navigation3D::Index idx) const { return Navigation3D::switch_vertex(mesh_, idx); }
		Navigation3D::Index switch_edge(Navigation3D::Index idx) const { return Navigation3D::switch_edge(mesh_, idx); }
		Navigation3D::Index switch_face(Navigation3D::Index idx) const { return Navigation3D::switch_face(mesh_, idx); }
		Navigation3D::Index switch_element(Navigation3D::Index idx) const { return Navigation3D::switch_element(mesh_, idx); }

		// Iterate in a mesh
		inline Navigation3D::Index next_around_edge(Navigation3D::Index idx) const { return Navigation3D::next_around_3Dedge(mesh_, idx); }
		inline Navigation3D::Index next_around_face(Navigation3D::Index idx) const { return Navigation3D::next_around_2Dface(mesh_, idx); }

		void to_face_functions(std::array<std::function<Navigation3D::Index(Navigation3D::Index)>, 6> &to_face) const;
		void to_vertex_functions(std::array<std::function<Navigation3D::Index(Navigation3D::Index)>, 8> &to_vertex) const;
		void to_edge_functions(std::array<std::function<Navigation3D::Index(Navigation3D::Index)>, 12> &to_edge) const;

		// Retrieves hex vertices numbered as follows:
		//   v7────v6
		//   ╱┆    ╱│
		// v4─┼──v5 │
		//  │v3┄┄┄┼v2
		//  │╱    │╱
		// v0────v1
		std::array<int, 8> get_ordered_vertices_from_hex(const int element_index) const;
		std::array<int, 4> get_ordered_vertices_from_tet(const int element_index) const;

		void get_vertex_elements_neighs(const int v_id, std::vector<int> &ids) const { ids.clear(); ids.insert(ids.begin(), mesh_.vertices[v_id].neighbor_hs.begin(), mesh_.vertices[v_id].neighbor_hs.end()); }
		void get_edge_elements_neighs(const int e_id, std::vector<int> &ids) const { ids.clear(); ids.insert(ids.begin(), mesh_.edges[e_id].neighbor_hs.begin(), mesh_.edges[e_id].neighbor_hs.end()); }

		void compute_boundary_ids(const double eps) override;
		void compute_boundary_ids(const std::function<int(const RowVectorNd&)> &marker) override;
		void compute_boundary_ids(const std::function<int(const RowVectorNd&, bool)> &marker) override;
		void compute_boundary_ids(const std::function<int(const std::vector<int>&, bool)> &marker) override;

		void compute_element_barycenters(Eigen::MatrixXd &barycenters) const override { cell_barycenters(barycenters); }
		void triangulate_faces(Eigen::MatrixXi &tris, Eigen::MatrixXd &pts, std::vector<int> &ranges) const override;
		void get_edges(Eigen::MatrixXd &p0, Eigen::MatrixXd &p1) const override;
		void get_edges(Eigen::MatrixXd &p0, Eigen::MatrixXd &p1, const std::vector<bool> &valid_elements) const override;

		//used for sweeping 2D mesh
		Mesh3DStorage &mesh_storge() { std::cerr<<"never user this function"<<std::endl; return mesh_; }
		static void geomesh_2_mesh_storage(const GEO::Mesh &gm, Mesh3DStorage &m);
	protected:
		bool load(const std::string &path) override;
		bool load(const GEO::Mesh &M) override;
	private:
		Mesh3DStorage mesh_;
	};
}


#include <gtest/gtest.h>

#include "scheme/kinematics/Scene_io.hh"
#include "scheme/actor/ActorConcept_io.hh"
#include "scheme/objective/ObjectiveFunction.hh"
#include "scheme/objective/ObjectiveVisitor.hh"
#include "scheme/numeric/X1dim.hh"

#include <boost/make_shared.hpp>
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/fusion/include/io.hpp>

#include <stdint.h>

namespace scheme { namespace kinematics { namespace objective_test {

using std::cout;
using std::endl;
using boost::tie;
using numeric::X1dim;

typedef actor::ActorConcept<X1dim,int> ADI;
typedef actor::ActorConcept<X1dim,char> ADC;
typedef size_t Index;
typedef std::pair<size_t,size_t> Index2;
typedef std::pair<Index2,Index2> Index4;

////////////// test scores ////////////////////////

	struct ScoreADI {
		typedef double Result;
		typedef ADI Interaction;
		static std::string name(){ return "ScoreADI"; }
		template<class Config>
		Result operator()(Interaction const & a, Config const& ) const {
			return a.data_;
		}
	};
	std::ostream & operator<<(std::ostream & out,ScoreADI const& s){ return out << s.name(); }

	struct ScoreADIADI {
		static size_t ncalls;
		typedef double Result;
		typedef ADI Actor1;
		typedef ADI Actor2;
		typedef std::pair<ADI,ADI> Interaction;
		static std::string name(){ return "ScoreADIADI"; }
		template<class Config>
		Result operator()(Actor1 const & a1, Actor2 const & a2, Config const& ) const {
			++ncalls;
			// cout << a.first << " " << a.second << endl;
			return distance(a1.position(),a2.position());
		}
		template<class Config>
		Result operator()(Interaction const & i, Config const& c) const {
			return this->template operator()<Config>(i.first,i.second,c);
		}
	};
	size_t ScoreADIADI::ncalls = 0;
	std::ostream & operator<<(std::ostream & out,ScoreADIADI const& s){ return out << s.name(); }

	struct ScoreADC {
		typedef double Result;
		typedef ADC Interaction;
		static std::string name(){ return "ScoreADC"; }
		template<class Config>
		Result operator()(Interaction const & a, Config const& ) const {
			switch(a.data_){
				case '1': return 1; break;
				case '2': return 2; break;
				case '3': return 3; break;
				case '4': return 4; break;
			};
			return 0;
		}
	};
	std::ostream & operator<<(std::ostream & out,ScoreADC const& s){ return out << s.name(); }

	struct ScoreADCADI {
		typedef double Result;
		typedef std::pair<ADC,ADI> Interaction;
		static std::string name(){ return "ScoreADCADI"; }
		template<class Config>
		Result operator()(ADC const & adc, ADI const & adi, Config const& ) const {
			return 10*distance(adc.position(),adi.position());
		}
		template<class Config>
		Result operator()(Interaction const & i, Config const& c ) const {
			return this->operator()( i.first, i.second, c);
		}

	};
	std::ostream & operator<<(std::ostream & out,ScoreADCADI const& s){ return out << s.name(); }

	struct Config {};

/////////////////// tests //////////////////////////

TEST(SceneObjective,basic){
	typedef	objective::ObjectiveFunction<
		m::vector<
			ScoreADI,
			ScoreADC,
			ScoreADIADI,
			ScoreADCADI
		>,
		Config
	> ObjFun;
	typedef ObjFun::Results Results;
	ObjFun score;

	typedef m::vector< ADI, ADC > Actors;
	typedef Scene<impl::Conformation<Actors>,X1dim,size_t> Scene;

	Scene scene(3);
	ASSERT_EQ( score(scene).sum(), 0 );

	scene.mutable_conformation_asym(0).add_actor( ADI(0,1) );
	scene.mutable_conformation_asym(0).add_actor( ADI(0,2) );
	scene.mutable_conformation_asym(0).add_actor( ADI(0,3) );
	scene.mutable_conformation_asym(0).add_actor( ADI(0,4) );
	ASSERT_EQ( score(scene), Results(10,0,0,0) );

	scene.mutable_conformation_asym(1).add_actor( ADI(1,1) );
	scene.mutable_conformation_asym(1).add_actor( ADI(1,2) );
	scene.mutable_conformation_asym(1).add_actor( ADI(1,3) );
	scene.mutable_conformation_asym(1).add_actor( ADI(1,4) );
	ASSERT_EQ( score(scene), Results(20,0,16,0) );

	scene.mutable_conformation_asym(0).add_actor( ADC(0,'1') );
	scene.mutable_conformation_asym(0).add_actor( ADC(0,'2') );
	ASSERT_EQ( score(scene), Results(20,3,16,80) );

	scene.mutable_conformation_asym(2).add_actor( ADI(2,100) );
	ASSERT_EQ( score(scene), Results(120,3,28,120) );

	scene.mutable_conformation_asym(2).add_actor( ADI(2,1000) );
	ASSERT_EQ( score(scene), Results(1120,3,40,160) );

}

TEST(SceneObjective,symmetry){
	typedef	objective::ObjectiveFunction<
		m::vector<
			ScoreADI,
			ScoreADC,
			ScoreADIADI,
			ScoreADCADI
		>,
		Config
	> ObjFun;
	typedef ObjFun::Results Results;

	ObjFun score;

	typedef m::vector< ADI, ADC > Actors;
	typedef Scene<impl::Conformation<Actors>,X1dim,size_t> Scene;

	Scene scene(2);
	scene.add_symframe(10);

	ASSERT_EQ( score(scene).sum(), 0 );

	scene.mutable_conformation_asym(0).add_actor( ADI(0,1) );
	scene.mutable_conformation_asym(1).add_actor( ADI(1,2) );
	// check that symmetric interactiosn are downweighted by 0.5: 40/2 = 20
	ASSERT_EQ( score(scene), Results(3,0,1+20,0) );

	scene.mutable_conformation_asym(1).add_actor( ADI(0,1) );
	scene.mutable_conformation_asym(1).add_actor( ADI(1,2) );
	// check that symmetric interactiosn are downweighted by 0.5: 160/2 = 80
	ASSERT_EQ( score(scene), Results(6,0,2+80,0) );

	// scene.mutable_conformation_asym(1).add_actor( ADC(0,'1') );
	// scene.mutable_conformation_asym(1).add_actor( ADC(0,'2') );
	// ASSERT_EQ( score(scene), Results(20,3,16,80) );

}

template<class Scene,class Visitor>
void performance_test_helper(Scene const & scene, Visitor & visitor){
	typedef ADI Actor1;
	typedef ADI Actor2;
	typedef typename Scene::Conformation Conformation;
	typedef typename Scene::Position Position;
	typedef typename Scene::Index Index;

		Index const NBOD = (Index)scene.bodies_.size();
		Index const NSYM = (Index)scene.symframes_.size()+1;
		for(Index i1 = 0; i1 < NBOD*NSYM; ++i1){
			Conformation const & c1 = scene.conformation(i1);
			Position     const & p1 =     scene.position(i1);
			Index const NACT1 = (Index)c1.template get<Actor1>().size();
			for(Index i2 = 0; i2 < NBOD*NSYM; ++i2){
				if( i1 >= NBOD && i2 >= NBOD ) continue;
				if( i2 <= i1 ) continue;
				Conformation const & c2 = scene.conformation(i2);
				Position     const & p2 =     scene.position(i2);
				Index const NACT2 = (Index)c2.template get<Actor2>().size();
				for(Index j1 = 0; j1 < NACT1; ++j1){
					Actor1 a1( c1.template get<Actor1>()[j1], p1 );
					for(Index j2 = 0; j2 < NACT2; ++j2){
						Actor2 a2( c2.template get<Actor2>()[j2], p2 );
						visitor( std::make_pair(a1,a2), i1<NBOD&&i2<NBOD?1.0:0.5 );
					}
				}
			}
		}
	cout << visitor.result_ << " " << (double)ScoreADIADI::ncalls/1000000.0 << endl;
	ScoreADIADI::ncalls = 0;
}

#ifdef SCHEME_BENCHMARK

TEST( SceneObjective, big_scene_performance ){
// TEST(SceneObjective,performance){
	// TODO: speed up SceneIter iteration
	//       iteration seems to take about 100 cycles per score call overhead
	//       much of this is probably all the conditions for symmetry checks
	//       could template out these and have both sym and asym scenes?
	// FIX with visitation pattern, seems at least 10x faster

	std::cout << "This test performs 301.934M score calls, should take about a second when compiled with optimizations." << std::endl;

	typedef	objective::ObjectiveFunction<
		m::vector<
			ScoreADI,
			ScoreADC,
			ScoreADIADI,
			ScoreADCADI
		>,
		Config
	> ObjFun;
	typedef ObjFun::Results Results;

	ObjFun score;

	typedef m::vector< ADI, ADC > Actors;
	typedef Scene<impl::Conformation<Actors>,X1dim,uint32_t> Scene;

	ScoreADIADI obj;
	Config c;
	objective::ObjectiveVisitor<ScoreADIADI,Config> visitor(obj,c);

	Scene scene; {
		Scene::Index const NBOD = 10;
		Scene::Index const NSYM = 20;
		Scene::Index const NACT = 400;
		for(Scene::Index i = 0; i < NBOD; ++i) scene.add_body();
		for(Scene::Index i = 0; i < NSYM-1; ++i) scene.add_symframe(i+1);
		for(Scene::Index i = 0; i < NBOD; ++i){
			for(Scene::Index j = 0; j < NACT; ++j){
				scene.mutable_conformation_asym(i).add_actor( ADI(j,i) );
			}
		}
	}

	cout << score(scene).get<ScoreADIADI>() << " " << (double)ScoreADIADI::ncalls/1000000.0 << "M" << endl;
	ScoreADIADI::ncalls = 0;
	return;

	if(false)
	{
			// typedef ADI Actor1;
			// typedef ADI Actor2;
			// typedef Scene::Position Position;
			// Scene::Index const NBOD = scene.bodies_.size();
			// Scene::Index const NSYM = scene.symframes_.size()+1;
			// for(Scene::Index i1 = 0; i1 < NBOD*NSYM; ++i1){
			// 	Conformation const & c1 = scene.conformation(i1);
			// 	Position     const & p1 =     scene.position(i1);
			// 	Scene::Index const NACT1 = c1.get<Actor1>().size();
			// 	for(Scene::Index i2 = 0; i2 < NBOD*NSYM; ++i2){
			// 		if( i1 >= NBOD && i2 >= NBOD ) continue;
			// 		if( i2 <= i1 ) continue;
			// 		Conformation const & c2 = scene.conformation(i2);
			// 		Position     const & p2 =     scene.position(i2);
			// 		Scene::Index const NACT2 = c2.get<Actor2>().size();
			// 		for(Scene::Index j1 = 0; j1 < NACT1; ++j1){
			// 			Actor1 a1( c1.get<Actor1>()[j1], p1 );
			// 			for(Scene::Index j2 = 0; j2 < NACT2; ++j2){
			// 				Actor2 a2( c2.get<Actor2>()[j2], p2 );
			// 				visitor( a1, a2, i1<NBOD&&i2<NBOD?1.0:0.5 );
			// 			}
			// 		}
			// 	}
			// }
			// cout << visitor.result_ << " " << (double)ScoreADIADI::ncalls/1000000.0 << endl;
			// ScoreADIADI::ncalls = 0;

	}

	if(false)
		performance_test_helper(scene,visitor);
	ScoreADIADI::ncalls = 0;

	scene.visit(visitor);
	cout << visitor.result_ << " " << (double)ScoreADIADI::ncalls/1000000.0 << "M" << endl;
	ScoreADIADI::ncalls = 0;

}

#endif

}
}
}


#include "teca_table_sort.h"

#include "teca_table.h"
#include "teca_array_collection.h"
#include "teca_variant_array.h"
#include "teca_metadata.h"

#include <algorithm>
#include <iostream>
#include <string>
#include <set>
#include <cmath>

#if defined(TECA_HAS_BOOST)
#include <boost/program_options.hpp>
#endif

#if defined(TECA_HAS_MPI)
#include <mpi.h>
#endif

using std::string;
using std::vector;
using std::set;
using std::cerr;
using std::endl;

//#define TECA_DEBUG

namespace internal
{
template<typename num_t>
class less
{
public:
    less() : m_data(nullptr) {}
    less(const num_t *data) : m_data(data) {}

    bool operator()(const size_t &l, const size_t &r)
    {
        return m_data[l] < m_data[r];
    }
private:
    const num_t *m_data;
};
};

// --------------------------------------------------------------------------
teca_table_sort::teca_table_sort() :
    index_column(""), index_column_id(0), stable_sort(0)
{
    this->set_number_of_input_connections(1);
    this->set_number_of_output_ports(1);
}

// --------------------------------------------------------------------------
teca_table_sort::~teca_table_sort()
{}

#if defined(TECA_HAS_BOOST)
// --------------------------------------------------------------------------
void teca_table_sort::get_properties_description(
    const string &prefix, options_description &global_opts)
{
    options_description opts("Options for "
        + (prefix.empty()?"teca_table_sort":prefix));

    opts.add_options()
        TECA_POPTS_GET(std::string, prefix, index_column,
            "name of the column to sort the table by")
        TECA_POPTS_GET(int, prefix, index_column_id,
            "column number to sort the table by. can be used in "
            "place of an index_column name")
        TECA_POPTS_GET(int, prefix, stable_sort,
            "if set a stable sort will be used")
        ;

    global_opts.add(opts);
}

// --------------------------------------------------------------------------
void teca_table_sort::set_properties(
    const string &prefix, variables_map &opts)
{
    TECA_POPTS_SET(opts, std::string, prefix, index_column)
    TECA_POPTS_SET(opts, int, prefix, index_column_id)
    TECA_POPTS_SET(opts, int, prefix, stable_sort)
}
#endif

// --------------------------------------------------------------------------
const_p_teca_dataset teca_table_sort::execute(
    unsigned int port,
    const std::vector<const_p_teca_dataset> &input_data,
    const teca_metadata &request)
{
#ifdef TECA_DEBUG
    cerr << teca_parallel_id() << "teca_table_sort::execute" << endl;
#endif
    (void)port;
    (void)request;

    // get the input
    const_p_teca_table in_table
        = std::dynamic_pointer_cast<const teca_table>(input_data[0]);

    // in parallel only rank 0 is required to have data
    int rank = 0;
#if defined(TECA_HAS_MPI)
    int init = 0;
    MPI_Initialized(&init);
    if (init)
        MPI_Comm_rank(this->get_communicator(), &rank);
#endif
    if (!in_table)
    {
        if (rank == 0)
        {
            TECA_ERROR("empty input")
        }
        return nullptr;
    }

    // get the array to sort by
    const_p_teca_variant_array index_col;
    if (this->index_column.empty())
        index_col = in_table->get_column(this->index_column_id);
    else
        index_col = in_table->get_column(this->index_column);
    if (!index_col)
    {
        TECA_ERROR("Failed to locate column to sort by \""
            <<  this->index_column << "\"")
        return nullptr;
    }

    // create the index
    unsigned long n_rows = index_col->size();
    unsigned long *index = static_cast<unsigned long*>(
        malloc(n_rows*sizeof(unsigned long)));
    for (unsigned long i = 0; i < n_rows; ++i)
        index[i] = i;
    TEMPLATE_DISPATCH(const teca_variant_array_impl,
        index_col.get(),
        const NT *col = static_cast<TT*>(index_col.get())->get();
        if (this->stable_sort)
            std::stable_sort(index, index+n_rows, internal::less<NT>(col));
        else
            std::sort(index, index+n_rows, internal::less<NT>(col));
        )

    // transfer data and reorder
    p_teca_table out_table = teca_table::New();
    out_table->copy_metadata(in_table);
    out_table->copy_structure(in_table);
    unsigned int n_cols = out_table->get_number_of_columns();
    for (unsigned int j = 0; j < n_cols; ++j)
    {
        const_p_teca_variant_array in_col = in_table->get_column(j);
        p_teca_variant_array out_col = out_table->get_column(j);
        out_col->resize(n_rows);
        TEMPLATE_DISPATCH(teca_variant_array_impl,
            out_col.get(),
            const NT *p_in_col = static_cast<const TT*>(in_col.get())->get();
            NT *p_out_col = static_cast<TT*>(out_col.get())->get();
            for (unsigned long i = 0; i < n_rows; ++i)
                p_out_col[i] = p_in_col[index[i]];
            )
    }

    free(index);

    return out_table;
}


//
// Copyright (c) 2002--2010
// Toon Knapen, Karl Meerbergen, Kresimir Fresl,
// Thomas Klimpel and Rutger ter Borg
//
// 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)
//
// THIS FILE IS AUTOMATICALLY GENERATED
// PLEASE DO NOT EDIT!
//

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_AUXILIARY_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_AUXILIARY_HPP

#include <boost/numeric/bindings/lapack/auxiliary/labrd.hpp>
#include <boost/numeric/bindings/lapack/auxiliary/lacgv.hpp>
#include <boost/numeric/bindings/lapack/auxiliary/lacon.hpp>
#include <boost/numeric/bindings/lapack/auxiliary/laebz.hpp>
#include <boost/numeric/bindings/lapack/auxiliary/lalsd.hpp>
#include <boost/numeric/bindings/lapack/auxiliary/langb.hpp>
#include <boost/numeric/bindings/lapack/auxiliary/lange.hpp>
#include <boost/numeric/bindings/lapack/auxiliary/lanhb.hpp>
#include <boost/numeric/bindings/lapack/auxiliary/lanhe.hpp>
#include <boost/numeric/bindings/lapack/auxiliary/lanhp.hpp>
#include <boost/numeric/bindings/lapack/auxiliary/lanhs.hpp>
#include <boost/numeric/bindings/lapack/auxiliary/lansb.hpp>
#include <boost/numeric/bindings/lapack/auxiliary/lansp.hpp>
#include <boost/numeric/bindings/lapack/auxiliary/lansy.hpp>
#include <boost/numeric/bindings/lapack/auxiliary/lantb.hpp>
#include <boost/numeric/bindings/lapack/auxiliary/lantp.hpp>
#include <boost/numeric/bindings/lapack/auxiliary/lantr.hpp>
#include <boost/numeric/bindings/lapack/auxiliary/larf.hpp>
#include <boost/numeric/bindings/lapack/auxiliary/larfb.hpp>
#include <boost/numeric/bindings/lapack/auxiliary/larfg.hpp>
#include <boost/numeric/bindings/lapack/auxiliary/larft.hpp>
#include <boost/numeric/bindings/lapack/auxiliary/larfx.hpp>
#include <boost/numeric/bindings/lapack/auxiliary/largv.hpp>
#include <boost/numeric/bindings/lapack/auxiliary/larnv.hpp>
#include <boost/numeric/bindings/lapack/auxiliary/larrb.hpp>
#include <boost/numeric/bindings/lapack/auxiliary/larre.hpp>
#include <boost/numeric/bindings/lapack/auxiliary/larz.hpp>
#include <boost/numeric/bindings/lapack/auxiliary/latrd.hpp>
#include <boost/numeric/bindings/lapack/auxiliary/latrs.hpp>
#include <boost/numeric/bindings/lapack/auxiliary/latrz.hpp>

#endif


/**
 All of this code is written by Aman Agrawal
 (Indian Institute of Technology, Delhi)
*/

#include <bits/stdc++.h>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/SVD>
#include <Eigen/QR>
#include "time.h"
#include <thread>
#include <chrono>

#include "genotype.h"
#include "mailman.h"
#include "arguments.h"
#include "helper.h"
#include "storage.h"

#if SSE_SUPPORT==1
	#define fastmultiply fastmultiply_sse
	#define fastmultiply_pre fastmultiply_pre_sse
#else
	#define fastmultiply fastmultiply_normal
	#define fastmultiply_pre fastmultiply_pre_normal
#endif

using namespace Eigen;
using namespace std;

// Storing in RowMajor Form
typedef Matrix<double, Dynamic, Dynamic, RowMajor> MatrixXdr;

//Intermediate Variables
//
// How to batch columns:
int blocksize;
double **partialsums;
double *sum_op;

// Intermediate computations in E-step.
// Size = 3^(log_3(n)) * k
double **yint_e;
//  n X k
double ***y_e;

// Intermediate computations in M-step.
// Size = nthreads X 3^(log_3(n)) * k
double **yint_m;
//  nthreads X log_3(n) X k
double ***y_m;

struct timespec t0;

clock_t total_begin = clock();

genotype g;
MatrixXdr geno_matrix; //(p,n)

int MAX_ITER;
int k,p,n;
int k_orig;

MatrixXdr c; //(p,k)
MatrixXdr x; //(k,n)
MatrixXdr v; //(p,k)
MatrixXdr means; //(p,1)
MatrixXdr stds; //(p,1)

options command_line_opts;

bool debug = false;
bool check_accuracy = false;
bool var_normalize=false;
int accelerated_em=0;
double convergence_limit;
bool memory_efficient = false;
bool missing=false;
bool fast_mode = true;
bool text_version = false;
int nthreads = 1;

void multiply_y_pre_fast_thread (int begin, int end, MatrixXdr &op, int Ncol_op, double *yint_m, double **y_m, double *partialsums, MatrixXdr &res){
	for(int seg_iter = begin; seg_iter < end; seg_iter++){
		mailman::fastmultiply ( g.segment_size_hori, g.Nindv, Ncol_op, g.p[seg_iter], op, yint_m, partialsums, y_m);
		int p_base = seg_iter * g.segment_size_hori;
		for(int p_iter=p_base; (p_iter < p_base + g.segment_size_hori) && (p_iter < g.Nsnp) ; p_iter++ ){
			for(int k_iter = 0; k_iter < Ncol_op; k_iter++)
				res(p_iter, k_iter) = y_m [p_iter - p_base][k_iter];
		}
	}
}

void multiply_y_post_fast_thread (int begin, int end, MatrixXdr &op, int Ncol_op, double *yint_e, double **y_e, double *partialsums){
	for (int i = 0 ; i < g.Nindv ; i++) {
		memset (y_e[i], 0, blocksize * sizeof(double));
	}

	for(int seg_iter = begin; seg_iter < end; seg_iter++){
		mailman::fastmultiply_pre (g.segment_size_hori, g.Nindv, Ncol_op, seg_iter * g.segment_size_hori, g.p[seg_iter], op, yint_e, partialsums, y_e);
	}
}

/*
 * M-step: Compute C = Y E
 * Y : p X n genotype matrix
 * E : n K k matrix: X^{T} (XX^{T})^{-1}
 * C = p X k matrix
 *
 * op : E
 * Ncol_op : k
 * res : C
 * subtract_means :
 */
void multiply_y_pre_fast(MatrixXdr &op, int Ncol_op ,MatrixXdr &res,bool subtract_means){

	for(int k_iter = 0; k_iter < Ncol_op; k_iter++){
		sum_op[k_iter] = op.col(k_iter).sum();
	}

	#if DEBUG==1
		if(debug){
			print_time ();
			cout <<"Starting mailman on premultiply"<<endl;
			cout << "Nops = " << Ncol_op << "\t" <<g.Nsegments_hori << endl;
			cout << "Segment size = " << g.segment_size_hori << endl;
			cout << "Matrix size = " <<g.segment_size_hori<<"\t" <<g.Nindv << endl;
			cout << "op = " <<  op.rows () << "\t" << op.cols () << endl;
		}
	#endif

	//TODO: Memory Effecient SSE FastMultipy

	nthreads = (nthreads > g.Nsegments_hori) ? g.Nsegments_hori: nthreads;

	std::thread th [nthreads];
	int perthread = g.Nsegments_hori/nthreads;
//	cout << g.Nsegments_hori << "\t" << nthreads << "\t" << perthread << endl;
	int t = 0;
	for (; t < nthreads - 1; t++) {
//		cout << "Launching thread " << t << endl;
		th[t] = std::thread (multiply_y_pre_fast_thread, t * perthread , (t+1)*perthread, std::ref (op), Ncol_op, yint_m[t], y_m[t], partialsums[t], std::ref(res));
	}

	th[t] = std::thread (multiply_y_pre_fast_thread, t * perthread , g.Nsegments_hori  - 1, std::ref (op), Ncol_op, yint_m[t], y_m[t], partialsums[t], std::ref(res));

	for (int t = 0 ; t < nthreads; t++) {
		th[t].join ();
	}

/*
	for(int seg_iter = 0; seg_iter < g.Nsegments_hori - 1; seg_iter++){
		mailman::fastmultiply ( g.segment_size_hori, g.Nindv, Ncol_op, g.p[seg_iter], op, yint_m, partialsums, y_m);
		int p_base = seg_iter * g.segment_size_hori;
		for(int p_iter=p_base; (p_iter < p_base + g.segment_size_hori) && (p_iter < g.Nsnp) ; p_iter++ ){
			for(int k_iter = 0; k_iter < Ncol_op; k_iter++)
				res(p_iter, k_iter) = y_m [p_iter - p_base][k_iter];
		}
	}
*/

	int last_seg_size = (g.Nsnp % g.segment_size_hori !=0 ) ? g.Nsnp % g.segment_size_hori : g.segment_size_hori;
	mailman::fastmultiply(last_seg_size, g.Nindv, Ncol_op, g.p[g.Nsegments_hori-1], op, yint_m[0], partialsums[0], y_m[0]);
	int p_base = (g.Nsegments_hori - 1) * g.segment_size_hori;
	for(int p_iter = p_base; (p_iter < p_base + g.segment_size_hori) && (p_iter < g.Nsnp) ; p_iter++){
		for(int k_iter = 0; k_iter < Ncol_op; k_iter++)
			res(p_iter, k_iter) = y_m [0][p_iter - p_base][k_iter];
	}

	#if DEBUG==1
		if(debug){
			print_time ();
			cout <<"Ending mailman on premultiply"<<endl;
		}
	#endif

	if(!subtract_means)
		return;

	for(int p_iter = 0; p_iter < p; p_iter++){
 		for(int k_iter = 0 ; k_iter < Ncol_op; k_iter++){
			res(p_iter, k_iter) = res(p_iter, k_iter) - (g.get_col_mean(p_iter)*sum_op[k_iter]);
			if(var_normalize)
				res(p_iter, k_iter) = res(p_iter, k_iter)/(g.get_col_std(p_iter));
 		}
 	}

}

/*
 * E-step: Compute X = D Y
 * Y : p X n genotype matrix
 * D : k X p matrix: (C^T C)^{-1} C^{T}
 * X : k X n matrix
 *
 * op_orig : D
 * Nrows_op : k
 * res : X
 * subtract_means :
 */
void multiply_y_post_fast(MatrixXdr &op_orig, int Nrows_op, MatrixXdr &res,bool subtract_means){

	MatrixXdr op;
	op = op_orig.transpose();

	if(var_normalize && subtract_means){
		for(int p_iter = 0; p_iter < p ; p_iter++){
			for(int k_iter = 0; k_iter < Nrows_op; k_iter++)
				op(p_iter,k_iter) = op(p_iter,k_iter) / (g.get_col_std(p_iter));
		}
	}

	#if DEBUG==1
		if(debug){
			print_time ();
			cout <<"Starting mailman on postmultiply"<<endl;
		}
	#endif

	int Ncol_op = Nrows_op;

	nthreads = (nthreads > g.Nsegments_hori) ? g.Nsegments_hori: nthreads;

	std::thread th [nthreads];
	int perthread = g.Nsegments_hori/nthreads;
//	cout << "post: " << g.segment_size_hori << "\t" << g.Nsegments_hori << "\t" << nthreads << "\t" << perthread << endl;
	int t = 0;
	for (; t < nthreads - 1; t++) {
//		cout << "Launching " << t << endl;
		th[t] = std::thread ( multiply_y_post_fast_thread,t * perthread, (t+1)*perthread, std::ref(op), Ncol_op, yint_e[t], y_e[t], partialsums[t]);

	}
//	cout << "Launching " << t << endl;
	th[t] = std::thread ( multiply_y_post_fast_thread, t * perthread, g.Nsegments_hori - 1, std::ref(op), Ncol_op, yint_e[t], y_e[t], partialsums[t]);
	for (int t = 0 ; t < nthreads; t++) {
		th[t].join ();
	}
//	cout << "Joined "<< endl;

/*
	int seg_iter;
	for(seg_iter = 0; seg_iter < g.Nsegments_hori-1; seg_iter++){
		mailman::fastmultiply_pre (g.segment_size_hori, g.Nindv, Ncol_op, seg_iter * g.segment_size_hori, g.p[seg_iter], op, yint_e, partialsums[0], y_e);
	}
*/

	for (int t = 1 ; t < nthreads; t++){
		for(int n_iter = 0; n_iter < n; n_iter++)
			for(int k_iter = 0; k_iter < Ncol_op; k_iter++)
				y_e [0][n_iter][k_iter] += y_e[t][n_iter][k_iter];
	}

	int last_seg_size = (g.Nsnp % g.segment_size_hori !=0 ) ? g.Nsnp % g.segment_size_hori : g.segment_size_hori;
	mailman::fastmultiply_pre(last_seg_size, g.Nindv, Ncol_op, (g.Nsegments_hori-1) * g.segment_size_hori, g.p[g.Nsegments_hori-1], op, yint_e[0], partialsums[0], y_e[0]);

	for(int n_iter = 0; n_iter < n; n_iter++)  {
		for(int k_iter = 0; k_iter < Ncol_op; k_iter++) {
			res(k_iter,n_iter) = y_e[0][n_iter][k_iter];
			y_e[0][n_iter][k_iter] = 0;
		}
	}

	#if DEBUG==1
		if(debug){
			print_time ();
			cout <<"Ending mailman on postmultiply"<<endl;
		}
	#endif

	if(!subtract_means)
		return;

	double *sums_elements = new double[Ncol_op];
 	memset (sums_elements, 0, Nrows_op * sizeof(int));

 	for(int k_iter=0;k_iter<Ncol_op;k_iter++){
 		double sum_to_calc=0.0;
 		for(int p_iter=0;p_iter<p;p_iter++)
 			sum_to_calc += g.get_col_mean(p_iter)*op(p_iter,k_iter);
 		sums_elements[k_iter] = sum_to_calc;
 	}
 	for(int k_iter=0;k_iter<Ncol_op;k_iter++){
 		for(int n_iter=0;n_iter<n;n_iter++)
 			res(k_iter,n_iter) = res(k_iter,n_iter) - sums_elements[k_iter];
 	}

}

void multiply_y_pre_naive_mem(MatrixXdr &op, int Ncol_op ,MatrixXdr &res){
	for(int p_iter=0;p_iter<p;p_iter++){
		for(int k_iter=0;k_iter<Ncol_op;k_iter++){
			double temp=0;
			for(int n_iter=0;n_iter<n;n_iter++)
				temp+= g.get_geno(p_iter,n_iter,var_normalize)*op(n_iter,k_iter);
			res(p_iter,k_iter)=temp;
		}
	}
}

void multiply_y_post_naive_mem(MatrixXdr &op, int Nrows_op ,MatrixXdr &res){
	for(int n_iter=0;n_iter<n;n_iter++){
		for(int k_iter=0;k_iter<Nrows_op;k_iter++){
			double temp=0;
			for(int p_iter=0;p_iter<p;p_iter++)
				temp+= op(k_iter,p_iter)*(g.get_geno(p_iter,n_iter,var_normalize));
			res(k_iter,n_iter)=temp;
		}
	}
}

void multiply_y_pre_naive(MatrixXdr &op, int Ncol_op ,MatrixXdr &res){
	res = geno_matrix * op;
}

void multiply_y_post_naive(MatrixXdr &op, int Nrows_op ,MatrixXdr &res){
	res = op * geno_matrix;
}

void multiply_y_post(MatrixXdr &op, int Nrows_op ,MatrixXdr &res,bool subtract_means){
    if(fast_mode)
        multiply_y_post_fast(op,Nrows_op,res,subtract_means);
    else{
		if(memory_efficient)
			multiply_y_post_naive_mem(op,Nrows_op,res);
		else
			multiply_y_post_naive(op,Nrows_op,res);
	}
}

void multiply_y_pre(MatrixXdr &op, int Ncol_op ,MatrixXdr &res,bool subtract_means){
    if(fast_mode)
        multiply_y_pre_fast(op,Ncol_op,res,subtract_means);
    else{
		if(memory_efficient)
			multiply_y_pre_naive_mem(op,Ncol_op,res);
		else
			multiply_y_pre_naive(op,Ncol_op,res);
	}
}

pair<double,double> get_error_norm(MatrixXdr &c){
	HouseholderQR<MatrixXdr> qr(c);
	MatrixXdr Q;
	Q = qr.householderQ() * MatrixXdr::Identity(p,k);
	MatrixXdr q_t(k,p);
	q_t = Q.transpose();
	MatrixXdr b(k,n);
	// Need this for subtracting the correct mean in case of missing data
	if(missing){
		multiply_y_post(q_t,k,b,false);
		// Just calculating b from seen data
		MatrixXdr M_temp(k,1);
		M_temp = q_t * means;
		for(int j=0;j<n;j++){
			MatrixXdr M_to_remove(k,1);
			M_to_remove = MatrixXdr::Zero(k,1);
			for(int i=0;i<g.not_O_j[j].size();i++){
				int idx = g.not_O_j[j][i];
				M_to_remove = M_to_remove + (Q.row(idx).transpose()*g.get_col_mean(idx));
			}
			b.col(j) -= (M_temp - M_to_remove);
		}
	}
	else{
		multiply_y_post(q_t,k,b,true);
	}

	JacobiSVD<MatrixXdr> b_svd(b, ComputeThinU | ComputeThinV);
	MatrixXdr u_l,d_l,v_l;
	if(fast_mode)
        u_l = b_svd.matrixU();
    else
        u_l = Q * b_svd.matrixU();
	v_l = b_svd.matrixV();
	d_l = MatrixXdr::Zero(k,k);
	for(int kk=0;kk<k; kk++)
		d_l(kk,kk) = (b_svd.singularValues())(kk);

	MatrixXdr u_k,v_k,d_k;
	u_k = u_l.leftCols(k_orig);
	v_k = v_l.leftCols(k_orig);
	d_k = MatrixXdr::Zero(k_orig,k_orig);
	for(int kk =0 ; kk < k_orig ; kk++)
		d_k(kk,kk)  =(b_svd.singularValues())(kk);

	MatrixXdr b_l,b_k;
    b_l = u_l * d_l * (v_l.transpose());
    b_k = u_k * d_k * (v_k.transpose());

    if(fast_mode){
        double temp_k = b_k.cwiseProduct(b).sum();
        double temp_l = b_l.cwiseProduct(b).sum();
        double b_knorm = b_k.norm();
        double b_lnorm = b_l.norm();
        double norm_k = (b_knorm*b_knorm) - (2*temp_k);
        double norm_l = (b_lnorm*b_lnorm) - (2*temp_l);
        return make_pair(norm_k,norm_l);
    }
    else{
        MatrixXdr e_l(p,n);
        MatrixXdr e_k(p,n);
        for(int p_iter=0;p_iter<p;p_iter++){
            for(int n_iter=0;n_iter<n;n_iter++){
                e_l(p_iter,n_iter) = g.get_geno(p_iter,n_iter,var_normalize) - b_l(p_iter,n_iter);
                e_k(p_iter,n_iter) = g.get_geno(p_iter,n_iter,var_normalize) - b_k(p_iter,n_iter);
            }
        }

        double ek_norm = e_k.norm();
        double el_norm = e_l.norm();
        return make_pair(ek_norm,el_norm);
    }
}

/* Run one iteration of EM when genotypes are not missing
 * c_orig : p X k matrix
 * Output: c_new : p X k matrix
 */
MatrixXdr run_EM_not_missing(MatrixXdr &c_orig){

	#if DEBUG==1
		if(debug){
			print_time ();
			cout << "Enter: run_EM_not_missing" << endl;
		}
	#endif

 	// c_temp : k X p matrix: (C^T C)^{-1} C^{T}
	MatrixXdr c_temp(k,p);
	MatrixXdr c_new(p,k);
	c_temp = ( (c_orig.transpose()*c_orig).inverse() ) * (c_orig.transpose());

	#if DEBUG==1
		if(debug){
			print_timenl ();
		}
	#endif

 	/* E-step: Compute X = D Y
 	* Y : p X n genotype matrix
 	* D : k X p matrix: (C^T C)^{-1} C^{T}
 	* X : k X n matrix
 	*  x_fn: X
 	*  c_temp: D
 	*/
	MatrixXdr x_fn(k,n);
	multiply_y_post(c_temp,k,x_fn,true);

	#if DEBUG==1
		if(debug){
			print_timenl ();
		}
	#endif

	// x_temp : n X k matrix X^{T} (XX^{T})^{-1}
	MatrixXdr x_temp(n,k);
	x_temp = (x_fn.transpose()) * ((x_fn*(x_fn.transpose())).inverse());

	/* M-step: C = Y E
 	* Y : p X n genotype matrix
 	* E : n K k matrix: X^{T} (XX^{T})^{-1}
 	* C = p X k matrix
 	* c_new : C
 	* x_temp : E
 	*/
	multiply_y_pre(x_temp,k,c_new,true);

	#if DEBUG==1
		if(debug){
			print_time ();
			cout << "Exiting: run_EM_not_missing" << endl;
		}
	#endif

	return c_new;
}

MatrixXdr run_EM_missing(MatrixXdr &c_orig){

	MatrixXdr c_new(p,k);

	MatrixXdr mu(k,n);

	// E step
	MatrixXdr c_temp(k,k);
	c_temp = c_orig.transpose() * c_orig;

	MatrixXdr T(k,n);
	MatrixXdr c_fn;
	c_fn = c_orig.transpose();
	multiply_y_post(c_fn,k,T,false);

	MatrixXdr M_temp(k,1);
	M_temp = c_orig.transpose() *  means;

	for(int j=0;j<n;j++){
		MatrixXdr D(k,k);
		MatrixXdr M_to_remove(k,1);
		D = MatrixXdr::Zero(k,k);
		M_to_remove = MatrixXdr::Zero(k,1);
		for(int i=0;i<g.not_O_j[j].size();i++){
			int idx = g.not_O_j[j][i];
			D = D + (c_orig.row(idx).transpose() * c_orig.row(idx));
			M_to_remove = M_to_remove + (c_orig.row(idx).transpose()*g.get_col_mean(idx));
		}
		mu.col(j) = (c_temp-D).inverse() * ( T.col(j) - M_temp + M_to_remove);
	}

	#if DEBUG==1
		if(debug){
			ofstream x_file;
			x_file.open((string(command_line_opts.OUTPUT_PATH)+string("x_in_fn_vals.txt")).c_str());
			x_file<<std::setprecision(15)<<mu<<endl;
			x_file.close();
		}
	#endif

	// M step

	MatrixXdr mu_temp(k,k);
	mu_temp = mu * mu.transpose();
	MatrixXdr T1(p,k);
	MatrixXdr mu_fn;
	mu_fn = mu.transpose();
	multiply_y_pre(mu_fn,k,T1,false);
	MatrixXdr mu_sum(k,1);
	mu_sum = MatrixXdr::Zero(k,1);
	mu_sum = mu.rowwise().sum();

	for(int i=0;i<p;i++){
		MatrixXdr D(k,k);
		MatrixXdr mu_to_remove(k,1);
		D = MatrixXdr::Zero(k,k);
		mu_to_remove = MatrixXdr::Zero(k,1);
		for(int j=0;j<g.not_O_i[i].size();j++){
			int idx = g.not_O_i[i][j];
			D = D + (mu.col(idx) * mu.col(idx).transpose());
			mu_to_remove = mu_to_remove + (mu.col(idx));
		}
		c_new.row(i) = (((mu_temp-D).inverse()) * (T1.row(i).transpose() -  ( g.get_col_mean(i) * (mu_sum-mu_to_remove)))).transpose();
		double mean;
		mean = g.get_col_sum(i);
		mean = mean -  (c_orig.row(i)*(mu_sum-mu_to_remove))(0,0);
		mean = mean * 1.0 / (n-g.not_O_i[i].size());
		g.update_col_mean(i,mean);
	}

	// IMPORTANT: Update the value of means variable present locally, so that for next iteration, updated value of means is used.
	for(int i=0;i<p;i++){
		means(i,0) = g.get_col_mean(i);
		// Also updating std, just for consistency, though, it is not used presently.
		stds(i,0) = g.get_col_std(i);
	}

	return c_new;
}

MatrixXdr run_EM(MatrixXdr &c_orig){

	if(missing)
		return run_EM_missing(c_orig);
	else
		return run_EM_not_missing(c_orig);
}

void print_vals(){

	HouseholderQR<MatrixXdr> qr(c);
	MatrixXdr Q;
	Q = qr.householderQ() * MatrixXdr::Identity(p,k);
	MatrixXdr q_t(k,p);
	q_t = Q.transpose();
	MatrixXdr b(k,n);

	// Need this for subtracting the correct mean in case of missing data
	if(missing){
		multiply_y_post(q_t,k,b,false);
		// Just calculating b from seen data
		MatrixXdr M_temp(k,1);
		M_temp = q_t * means;
		for(int j=0;j<n;j++){
			MatrixXdr M_to_remove(k,1);
			M_to_remove = MatrixXdr::Zero(k,1);
			for(int i=0;i<g.not_O_j[j].size();i++){
				int idx = g.not_O_j[j][i];
				M_to_remove = M_to_remove + (Q.row(idx).transpose()*g.get_col_mean(idx));
			}
			b.col(j) -= (M_temp - M_to_remove);
		}
	}
	else{
		multiply_y_post(q_t,k,b,true);
	}

	JacobiSVD<MatrixXdr> b_svd(b, ComputeThinU | ComputeThinV);
	MatrixXdr u_l;
	u_l = b_svd.matrixU();
	MatrixXdr v_l;
	v_l = b_svd.matrixV();
	MatrixXdr u_k;
	MatrixXdr v_k,d_k;
	u_k = u_l.leftCols(k_orig);
	v_k = v_l.leftCols(k_orig);

	ofstream evec_file;
	evec_file.open((string(command_line_opts.OUTPUT_PATH)+string("evecs.txt")).c_str());
	evec_file<< std::setprecision(15) << Q*u_k << endl;
	evec_file.close();
	ofstream eval_file;
	eval_file.open((string(command_line_opts.OUTPUT_PATH)+string("evals.txt")).c_str());
	for(int kk =0 ; kk < k_orig ; kk++)
		eval_file << std::setprecision(15)<< (b_svd.singularValues())(kk)*(b_svd.singularValues())(kk)/g.Nsnp<<endl;
	eval_file.close();

	ofstream proj_file;
	proj_file.open((string(command_line_opts.OUTPUT_PATH) + string("projections.txt")).c_str());
	proj_file << std::setprecision(15)<< v_k<<endl;
	proj_file.close();
	if(debug){
		ofstream c_file;
		c_file.open((string(command_line_opts.OUTPUT_PATH)+string("cvals.txt")).c_str());
		c_file<<std::setprecision(15)<<c<<endl;
		c_file.close();

		ofstream means_file;
		means_file.open((string(command_line_opts.OUTPUT_PATH)+string("means.txt")).c_str());
		means_file<<std::setprecision(15)<<means<<endl;
		means_file.close();

		d_k = MatrixXdr::Zero(k_orig,k_orig);
		for(int kk =0 ; kk < k_orig ; kk++)
			d_k(kk,kk)  =(b_svd.singularValues())(kk);
		MatrixXdr x_k;
		x_k = d_k * (v_k.transpose());
		ofstream x_file;
		x_file.open((string(command_line_opts.OUTPUT_PATH) + string("xvals.txt")).c_str());
		x_file<<std::setprecision(15)<<x_k.transpose()<<endl;
		x_file.close();
	}
}

int main(int argc, char const *argv[]){

	auto start = std::chrono::system_clock::now();

	clock_t io_begin = clock();
    clock_gettime (CLOCK_REALTIME, &t0);

	pair<double,double> prev_error = make_pair(0.0,0.0);
	double prevnll=0.0;

	parse_args(argc,argv);

	//TODO: Memory Effecient Version of Mailman

	memory_efficient = command_line_opts.memory_efficient;
	text_version = command_line_opts.text_version;
    fast_mode = command_line_opts.fast_mode;
	missing = command_line_opts.missing;
    MAX_ITER =  command_line_opts.max_iterations ;
	k_orig = command_line_opts.num_of_evec ;
	debug = command_line_opts.debugmode ;
	check_accuracy = command_line_opts.getaccuracy;
	var_normalize = command_line_opts.var_normalize;
	accelerated_em = command_line_opts.accelerated_em;
	nthreads = command_line_opts.nthreads;

	if(text_version){
		if(fast_mode)
			g.read_txt_mailman(command_line_opts.GENOTYPE_FILE_PATH,missing);
		else
			g.read_txt_naive(command_line_opts.GENOTYPE_FILE_PATH,missing);
	}
	else{
		g.read_plink(command_line_opts.GENOTYPE_FILE_PATH,missing,fast_mode);
	}

	//TODO: Implement these codes.
	if(missing && !fast_mode){
		cout<<"Missing version works only with mailman i.e. fast mode\n EXITING..."<<endl;
		exit(-1);
	}
	if(fast_mode && memory_efficient){
		cout<<"Memory effecient version for mailman EM not yet implemented"<<endl;
		cout<<"Ignoring Memory effecient Flag"<<endl;
	}
	if(missing && var_normalize){
		cout<<"Missing version works only without variance normalization\n EXITING..."<<endl;
		exit(-1);
	}

	k = k_orig + command_line_opts.l;
	k = (int)ceil(k/10.0)*10;
	command_line_opts.l = k - k_orig;
	p = g.Nsnp;
	n = g.Nindv;
	convergence_limit = command_line_opts.convergence_limit;
	bool toStop=false;
	if(convergence_limit!=-1)
		toStop=true;

	if (command_line_opts.given_seed)
		srand(command_line_opts.seed);
	else
		srand((unsigned int) time(0));

	c.resize(p,k);
	x.resize(k,n);
	v.resize(p,k);
	means.resize(p,1);
	stds.resize(p,1);

	if(!fast_mode && !memory_efficient){
		geno_matrix.resize(p,n);
		g.generate_eigen_geno(geno_matrix,var_normalize);
	}

	clock_t io_end = clock();

	//TODO: Initialization of c with gaussian distribution
	c = MatrixXdr::Random(p,k);

	// Initial intermediate data structures
	// Operate in blocks to improve caching
	//
	blocksize = k;
	int hsegsize = g.segment_size_hori; 	// = log_3(n)
	int hsize = pow(3,hsegsize);
	int vsegsize = g.segment_size_ver; 		// = log_3(p)
	int vsize = pow(3,vsegsize);

	partialsums = new double* [nthreads];
	yint_m = new double* [nthreads];
	for (int t = 0 ; t < nthreads ; t++) {
		partialsums[t] = new double [blocksize];
		yint_m[t] = new double [hsize*blocksize];
		memset (yint_m[t], 0, hsize*blocksize * sizeof(double));
	}

	sum_op = new double[blocksize];

	yint_e = new double* [nthreads];
	for (int t = 0 ; t < nthreads ; t++) {
		yint_e[t] = new double [hsize*blocksize];
		memset (yint_e[t], 0, hsize*blocksize * sizeof(double));
	}

	y_e  = new double**[nthreads];
	for (int t = 0 ; t < nthreads ; t++) {
		y_e[t]  = new double*[g.Nindv];
		for (int i = 0 ; i < g.Nindv ; i++) {
			y_e[t][i] = new double[blocksize];
			memset (y_e[t][i], 0, blocksize * sizeof(double));
		}
	}

	y_m = new double**[nthreads];
	for (int t = 0 ; t < nthreads; t++){
		y_m[t] = new double*[hsegsize];
		for (int i = 0 ; i < hsegsize ; i++)
			y_m[t][i] = new double[blocksize];

	}

	for(int i=0;i<p;i++){
		means(i,0) = g.get_col_mean(i);
		stds(i,0) = g.get_col_std(i);
	}

	ofstream c_file;
	if(debug){
		c_file.open((string(command_line_opts.OUTPUT_PATH)+string("cvals_orig.txt")).c_str());
		c_file<<std::setprecision(15)<<c<<endl;
		c_file.close();
		printf("Read Matrix\n");
	}

	cout<<"Running on Dataset of "<<g.Nsnp<<" SNPs and "<<g.Nindv<<" Individuals"<<endl;

	#if SSE_SUPPORT==1
		if(fast_mode)
			cout<<"Using Optimized SSE FastMultiply"<<endl;
	#endif

	clock_t it_begin = clock();
	for(int i=0;i<MAX_ITER;i++){

		MatrixXdr c1,c2,cint,r,v;
		double a,nll;
		if(debug){
			print_time ();
			cout << "*********** Begin epoch " << i << "***********" << endl;
		}
		if(accelerated_em!=0){
			#if DEBUG==1
				if(debug){
					print_time();
					cout << "Before EM" << endl;
				}
			#endif
			c1 = run_EM(c);
			c2 = run_EM(c1);
			#if DEBUG==1
				if(debug){
					print_time();
					cout << "After EM but before acceleration" << endl;
				}
			#endif
			r = c1-c;
			v = (c2-c1) - r;
			a = -1.0 * r.norm() / (v.norm()) ;
			if(accelerated_em==1){
				if(a>-1){
					a=-1;
					cint=c2;
				}
				else {
					cint = c - 2*a*r + a*a*v;
					nll = get_error_norm(cint).second;
					if(i>0){
						while(nll>prevnll && a<-1){
							a = 0.5 * (a-1);
							cint = c - 2*a*r +(a*a*v);
							nll = get_error_norm(cint).second;
						}
					}
				}
				c = cint;
			}
			else if(accelerated_em==2){
				cint = c - 2*a*r + a*a*v;
				c = cint;
				// c = run_EM(cint);
			}
		}
		else{
			c = run_EM(c);
		}

		if ( accelerated_em == 1 || check_accuracy || toStop) {
			pair<double,double> e = get_error_norm(c);
			prevnll = e.second;
			if(check_accuracy)
				cout<<"Iteration "<<i+1<<"  "<<std::setprecision(15)<<e.first<<"  "<<e.second<<endl;
			if(abs(e.first-prev_error.first)<=convergence_limit){
				cout<<"Breaking after "<<i+1<<" iterations"<<endl;
				break;
			}
			prev_error = e;
		}
		if(debug){
			print_time ();
			cout << "*********** End epoch " << i << "***********" << endl;
		}

	}
	clock_t it_end = clock();

    print_vals();

	clock_t total_end = clock();
	double io_time = double(io_end - io_begin) / CLOCKS_PER_SEC;
	double avg_it_time = double(it_end - it_begin) / (MAX_ITER * 1.0 * CLOCKS_PER_SEC);
	double total_time = double(total_end - total_begin) / CLOCKS_PER_SEC;
	cout<<"IO Time:  "<< io_time << "\nAVG Iteration Time:  "<<avg_it_time<<"\nTotal runtime:   "<<total_time<<endl;

	std::chrono::duration<double> wctduration = std::chrono::system_clock::now() - start;
	cout <<"Wall clock time = " <<  wctduration.count() << endl;

	delete[] sum_op;
	for (int t = 0 ; t < nthreads ; t++){
		delete[] yint_e[t];
	}
	delete[] yint_e;

	for (int t = 0 ; t < nthreads ; t++){
		delete[] yint_m[t];
		delete[] partialsums[t];
	}
	delete[] yint_m;
	delete[] partialsums;

	for (int t = 0 ; t < nthreads ; t++){
		for (int i  = 0 ; i < hsegsize; i++)
			delete[] y_m [t][i];
		delete[] y_m[t];
	}
	delete[] y_m;

	for (int t = 0 ; t < nthreads ; t++){
		for (int i  = 0 ; i < g.Nindv; i++)
			delete[] y_e[t][i];
		delete[] y_e[t];
	}
	delete[] y_e;

	return 0;
}


// test_max.cc
#include "menon/utility.hh"
#include <boost/core/lightweight_test.hpp>
#include <string>
#include <string_view>

int main()
{
  using namespace std::literals;

  // max関数のつとコード
  { // int型どうしのテストコード
    constexpr auto r = menon::max(1, 2);
    BOOST_TEST_EQ(r, 2);
  }
  { // 符号付き整数型どうしのテストコード
    constexpr auto r = menon::max(1, 2L);
    BOOST_TEST_EQ(r, 2L);
  }
  { // 浮動小数点型どうしのテストコード
    constexpr double x = 1.23;
    constexpr double y = 2.34f;
    constexpr auto r = menon::max(x, y);
    BOOST_TEST_EQ(r, y);
  }
  { // 浮動小数点型と整数型が混在する場合のテストコード
    constexpr double x = 1.23;
    constexpr int y = 2;
    constexpr auto r = menon::max(x, y);
    BOOST_TEST_EQ(r, y);
  }
  { // string_viewどうしのテストコード
    constexpr auto r = menon::max("abc"sv, "def"sv);
    BOOST_TEST_EQ(r, "def"sv);
  }
  { // int型3引数のテストコード
    constexpr auto r = menon::max(1, 2, 3);
    BOOST_TEST_EQ(r, 3);
  }
  { // 符号付き整数型5引数のテストコード
    constexpr auto r = menon::max(1, 2, 3, 4L, 5LL);
    BOOST_TEST_EQ(r, 5LL);
  }

  // min関数のテストコード
  { // int型どうしのテストコード
    constexpr auto r = menon::min(1, 2);
    BOOST_TEST_EQ(r, 1);
  }
  { // 符号付き整数型どうしのテストコード
    constexpr auto r = menon::min(1, 2L);
    BOOST_TEST_EQ(r, 1L);
  }
  { // 浮動小数点型どうしのテストコード
    constexpr double x = 1.23;
    constexpr double y = 2.34f;
    constexpr auto r = menon::min(x, y);
    BOOST_TEST_EQ(r, x);
  }
  { // 浮動小数点型と整数型が混在する場合のテストコード
    constexpr unsigned x = 1u;
    constexpr double y = 2.34f;
    constexpr auto r = menon::min(x, y);
    BOOST_TEST_EQ(r, x);
  }
  { // string_viewどうしのテストコード
    constexpr auto r = menon::min("abc"sv, "def"sv);
    BOOST_TEST_EQ(r, "abc"sv);
  }
  { // int型3引数のテストコード
   constexpr auto r = menon::min(1, 2, 3);
    BOOST_TEST_EQ(r, 1);
  }
  { // 符号付き整数型5引数のテストコード
    constexpr auto r = menon::min(-1LL, 0L, 1, 2, 3);
    BOOST_TEST_EQ(r, -1LL);
  }
  return boost::report_errors();
}


/**
 * \ file GainExpanderFilter.cpp
 */

#include <ATK/Dynamic/GainExpanderFilter.h>

#include <ATK/Core/InPointerFilter.h>
#include <ATK/Core/OutPointerFilter.h>
#include <ATK/Core/Utilities.h>

#include <gtest/gtest.h>

#include <boost/math/constants/constants.hpp>

constexpr gsl::index PROCESSSIZE = 64;

TEST(GainExpanderFilter, softness_test)
{
  ATK::GainFilter<ATK::GainExpanderFilter<double>> filter;
  filter.set_softness(0.5);
  ASSERT_EQ(filter.get_softness(), 0.5);
}

TEST(GainExpanderFilter, softness_range_test)
{
  ATK::GainFilter<ATK::GainExpanderFilter<double>> filter;
  ASSERT_THROW(filter.set_softness(-0.000001), ATK::RuntimeError);
}

TEST(GainExpanderFilter, const_1_test)
{
  std::array<double, PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainExpanderFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    ASSERT_NEAR(1, outdata[i], 0.1);
  }
}

TEST(GainExpanderFilter, const_0_test)
{
  std::array<double, PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 0;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainExpanderFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(0.1);
  filter.set_ratio(2);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    ASSERT_NEAR(outdata[i], 0, 0.001);
  }
}

TEST(GainExpanderFilter, const_1_threshold_2_ratio_2_test)
{
  std::array<double, PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainExpanderFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(2);
  filter.set_ratio(2);
  filter.set_softness(1);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    ASSERT_NEAR(0.700553358, outdata[i], 0.1);
  }
}

TEST(GainExpanderFilter, const_1_threshold_2_ratio_2_test_LUT)
{
  std::array<double, PROCESSSIZE> data;
  for (gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainExpanderFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(2);
  filter.set_ratio(2);
  filter.set_softness(1);
  filter.wait_for_LUT_completion();

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for (gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    ASSERT_NEAR(0.700553358, outdata[i], 0.1);
  }
}

TEST(GainExpanderFilter, const_1_threshold_2_ratio_4_test)
{
  std::array<double, PROCESSSIZE> data;
  for (gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainExpanderFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(2);
  filter.set_ratio(4);
  filter.set_softness(1);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for (gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    ASSERT_NEAR(0.343814075, outdata[i], 0.1);
  }
}

TEST(GainExpanderFilter, const_1_threshold_2_ratio_4_test_LUT)
{
  std::array<double, PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainExpanderFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(2);
  filter.set_ratio(4);
  filter.set_softness(1);
  filter.wait_for_LUT_completion();

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    ASSERT_NEAR(0.343814075, outdata[i], 0.1);
  }
}

TEST(GainExpanderFilter, const_1_threshold_2_ratio_4_test_twice)
{
  std::array<double, PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainExpanderFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(2);
  filter.set_ratio(4);
  filter.set_softness(1);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    ASSERT_NEAR(0.343814075, outdata[i], 0.1);
  }

  generator.set_pointer(data.data(), PROCESSSIZE);
  output.set_pointer(outdata.data(), PROCESSSIZE);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    ASSERT_NEAR(0.343814075, outdata[i], 0.1);
  }
}

TEST(GainExpanderFilter, const_1_threshold_2_ratio_4_test_twice_LUT)
{
  std::array<double, PROCESSSIZE> data;
  for (gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainExpanderFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(2);
  filter.set_ratio(4);
  filter.set_softness(1);
  filter.wait_for_LUT_completion();

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for (gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    ASSERT_NEAR(0.343814075, outdata[i], 0.1);
  }

  generator.set_pointer(data.data(), PROCESSSIZE);
  output.set_pointer(outdata.data(), PROCESSSIZE);

  output.process(PROCESSSIZE);

  for (gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    ASSERT_NEAR(0.343814075, outdata[i], 0.1);
  }
}