/// ---------------------------------------------------------------------------
/// @section LICENSE
///
/// Copyright (c) 2016 Georgia Tech Research Institute (GTRI)
///               All Rights Reserved
///
/// The above copyright notice and this permission notice shall be included in
/// all copies or substantial portions of the Software.
///
/// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
/// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
/// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
/// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
/// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
/// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
/// DEALINGS IN THE SOFTWARE.
/// ---------------------------------------------------------------------------
/// @file filename.ext
/// @author Kevin DeMarco <kevin.demarco@gtri.gatech.edu>
/// @author Eric Squires <eric.squires@gtri.gatech.edu>
/// @version 1.0
/// ---------------------------------------------------------------------------
/// @brief A brief description.
///
/// @section DESCRIPTION
/// A long description.
/// ---------------------------------------------------------------------------
#include <iostream>
#include <limits>
#include <cmath>
#include <cfloat>

#include <Eigen/Geometry>

#include <scrimmage/plugin_manager/RegisterPlugin.h>
#include <scrimmage/entity/Entity.h>
#include <scrimmage/math/State.h>
#include <scrimmage/common/Utilities.h>
#include <scrimmage/parse/ParseUtils.h>

#include <scrimmage/proto/Shape.pb.h>
#include <scrimmage/proto/ProtoConversions.h>

#include "MotorSchemas.h"

using std::cout;
using std::endl;

namespace sc = scrimmage;
namespace sp = scrimmage_proto;

REGISTER_PLUGIN(scrimmage::Autonomy, MotorSchemas, MotorSchemas_plugin)

MotorSchemas::MotorSchemas()
{
}

void MotorSchemas::init(std::map<std::string,std::string> &params)
{
    show_shapes_ = sc::get("show_shapes", params, false);

    max_speed_ = sc::get<double>("max_speed", params, 21);

    move_to_goal_gain_ = sc::get<double>("move_to_goal_gain", params, 1.0);

    sqrt_axis_ratio_ = std::sqrt(sc::get<double>("axis_ratio", params, 1.0));

    if (sc::get("use_initial_heading", params, false)) {
        Eigen::Vector3d rel_pos = Eigen::Vector3d::UnitX()*2000;
        Eigen::Vector3d unit_vector = rel_pos.normalized();
        unit_vector = state_->quat().rotate(unit_vector);
        goal_ = state_->pos() + unit_vector * rel_pos.norm();
    } else {
        std::vector<double> goal_vec;
        if (sc::get_vec<double>("goal", params, " ", goal_vec, 3)) {
            goal_ = sc::vec2eigen(goal_vec);
        }
    }

    avoid_robot_gain_ = sc::get<double>("avoid_robot_gain", params, 1.0);
    sphere_of_influence_ = sc::get<double>("sphere_of_influence", params, 10);
    minimum_range_ = sc::get<double>("minimum_range", params, 5);

    desired_state_->vel() = Eigen::Vector3d::UnitX()*21;
    desired_state_->quat().set(0,0,state_->quat().yaw());
    desired_state_->pos() = Eigen::Vector3d::UnitZ()*state_->pos()(2);
}

bool MotorSchemas::step_autonomy(double t, double dt)
{
    // move-to-goal schema
    Eigen::Vector3d v_goal = (goal_ - state_->pos()).normalized();

    // Compute repulsion vector from each robot contact
    std::vector<Eigen::Vector3d> O_vecs;

    double yaw = state_->quat().yaw();

    Eigen::AngleAxisd rot(yaw, Eigen::Vector3d(0.0, 0.0, 1.0));
    Eigen::DiagonalMatrix<double, 3> d(sqrt_axis_ratio_, 1.0 / sqrt_axis_ratio_, 1.0);
    auto transform = rot*d*rot.inverse();
    auto transform_inv = transform.inverse();

    for (auto it = contacts_->begin(); it != contacts_->end(); it++) {

        // Ignore own position / id
        if (it->second.id().id() == parent_.lock()->id().id()) continue;

        Eigen::Vector3d diff = it->second.state()->pos() - state_->pos();
        Eigen::Vector3d diff_rot = transform_inv*diff;

        double O_mag = 0;
        double dist = diff_rot.norm();

        if (dist > sphere_of_influence_) {
            O_mag = 0;
        } else if (minimum_range_ < dist && dist <= sphere_of_influence_) {
            O_mag = (sphere_of_influence_ - dist) /
                (sphere_of_influence_ - minimum_range_);
        } else if (dist <= minimum_range_) {
            // O_mag = std::numeric_limits<double>::infinity();
            O_mag = 1e10;
        }

        Eigen::Vector3d O_dir = - O_mag * diff.normalized();
        O_vecs.push_back(O_dir);
    }

    // Normalize each repulsion vector and sum
    Eigen::Vector3d O_vec(0,0,0);
    for (auto it = O_vecs.begin(); it != O_vecs.end(); it++) {
        if (it->hasNaN()) {
            continue; // ignore misbehaved vectors
        }
        O_vec += *it;
    }

    // Apply gains to independent behaviors
    Eigen::Vector3d v_goal_w_gain = v_goal * move_to_goal_gain_;
    Eigen::Vector3d O_vec_w_gain = O_vec * avoid_robot_gain_;

    // Sum vectors across behaviors
    double sum_norms = v_goal_w_gain.norm() + O_vec_w_gain.norm();
    Eigen::Vector3d v_sum =  (v_goal_w_gain + O_vec_w_gain) / sum_norms;

    // Scale velocity to max speed:
    Eigen::Vector3d vel_result = v_sum * max_speed_;

    ///////////////////////////////////////////////////////////////////////////
    // Convert resultant vector into heading / speed / altitude command:
    ///////////////////////////////////////////////////////////////////////////

    // Forward speed is normed value of vector:
    desired_state_->vel()(0) = vel_result.norm();

    // Desired heading
    double heading = sc::Angles::angle_2pi(atan2(vel_result(1), vel_result(0)));
    desired_state_->quat().set(0, 0, heading);

    // Set Desired Altitude by projecting velocity
    desired_state_->pos()(2) = state_->pos()(2) + vel_result(2);

    ///////////////////////////////////////////////////////////////////////////
    // Draw important shapes
    ///////////////////////////////////////////////////////////////////////////
    // Draw sphere of influence:
    if (show_shapes_) {
        sc::ShapePtr shape(new scrimmage_proto::Shape);
        shape->set_type(scrimmage_proto::Shape::Circle);
        shape->set_opacity(0.2);
        shape->set_radius(sphere_of_influence_);
        sc::set(shape->mutable_center(), state_->pos());
        sc::set(shape->mutable_color(), 0, 255, 0);
        shapes_.push_back(shape);

        // Draw resultant vector:
        sc::ShapePtr arrow(new scrimmage_proto::Shape);
        arrow->set_type(scrimmage_proto::Shape::Line);
        sc::set(arrow->mutable_color(), 255,255,0);
        arrow->set_opacity(0.75);
        sc::add_point(arrow, state_->pos());
        sc::add_point(arrow, vel_result + state_->pos());
        shapes_.push_back(arrow);
    }
    return true;
}


#pragma once
#include <vector>
#include <opencv2/highgui.hpp>
#include <vpp/vpp.hh>
#include <Eigen/Core>

using namespace Eigen;

using namespace vpp;

double execution_time = 0;
double execution_t= 0;

enum class Theta_max : int32_t { XXSMALL = 67 , XSMALL = 127 , SMALL = 255, MEDIUM = 500, LARGE = 1000 , XLARGE = 1500};
enum class Sclare_rho : int32_t { ONE_QUART = 3, HALF = 2 , THREE_QUART = 1 , SAME = 0};
enum class Type_video_hough : int8_t { ONLY_CLUSTERS = 1 , ALL_POINTS = 2 };
enum class Type_capture : int16_t { webcam = 0 , photo = 1 , video = 3 } ;
enum class Type_tracking : int16_t { no_tracking = 0 , lucas_kanade = 1 , kalman_track = 2 , video_extruder_vid = 3};
enum class Type_output : int16_t { VIDEO_HOUGH = 0 , ORIGINAL_VIDEO = 1 , GRADIENT_VIDEO = 2,ORIGINAL_AND_HOUGH_VIDEO = 3   };
enum class Type_Lines : int8_t { EXTREMITE = 0 , ALL_POINTS = 1 , ONLY_POLAR = 2 };
enum class Frequence : int8_t { ALL_FRAME = 0 , NOT_ALL = 1 };
enum class Mode : int8_t { ONLY_VPP = 0 , OPTIMIZED = 1 };
enum class With_Kalman_Filter : int8_t { YES = 1 , NO = 0 };
enum class With_Transparency : int8_t { YES = 1 , NO = 0 };
enum class With_Entries : int8_t { YES = 1 , NO = 0 };

float  motion_threshold = 100;
#define dense_ht 2
#define feature_matching 7
Matrix<float,3,3> GySobel3x3;
Matrix<float,3,3> GxSobel3x3;
Matrix<float,5,5> GySobel5x5;
Matrix<float,5,5> GxSobel5x5;
Matrix<float,7,7> GySobel7x7;
Matrix<float,7,7> GxSobel7x7;
Matrix<float,9,9> GySobel9x9;
Matrix<float,9,9> GxSobel9x9;


/*
 Copyright (C) 2016 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <boost/make_shared.hpp>
#include <ored/portfolio/builders/fxforward.hpp>
#include <ored/portfolio/enginefactory.hpp>
#include <ored/portfolio/fxforward.hpp>
#include <ored/utilities/log.hpp>
#include <ored/utilities/parsers.hpp>
#include <ored/utilities/xmlutils.hpp>
#include <ql/cashflows/simplecashflow.hpp>
#include <qle/instruments/fxforward.hpp>

namespace ore {
namespace data {

void FxForward::build(const boost::shared_ptr<EngineFactory>& engineFactory) {
    // If you Buy EURUSD forward, then you buy EUR and sell USD.
    // EUR = foreign, USD = Domestic.
    // You pay in USD, so the Domestic / Sold ccy is the "payer" currency
    Currency boughtCcy = parseCurrency(boughtCurrency_);
    Currency soldCcy = parseCurrency(soldCurrency_);
    Date maturityDate = parseDate(maturityDate_);

    QL_REQUIRE(tradeActions().empty(), "TradeActions not supported for FxForward");

    try {
        DLOG("Build FxForward with maturity date " << QuantLib::io::iso_date(maturityDate));

        boost::shared_ptr<QuantLib::Instrument> instrument = boost::make_shared<QuantExt::FxForward>(
            boughtAmount_, boughtCcy, soldAmount_, soldCcy, maturityDate, false);
        instrument_.reset(new VanillaInstrument(instrument));

        npvCurrency_ = soldCurrency_;
        notional_ = soldAmount_;
        maturity_ = maturityDate;

    } catch (std::exception&) {
        instrument_.reset();
        throw;
    }

    // Set up Legs
    legs_ = {{boost::make_shared<SimpleCashFlow>(boughtAmount_, maturityDate)},
             {boost::make_shared<SimpleCashFlow>(soldAmount_, maturityDate)}};
    legCurrencies_ = {boughtCurrency_, soldCurrency_};
    legPayers_ = {false, true};

    // set Pricing engine
    boost::shared_ptr<EngineBuilder> builder = engineFactory->builder(tradeType_);
    QL_REQUIRE(builder, "No builder found for " << tradeType_);
    boost::shared_ptr<FxForwardEngineBuilder> fxBuilder = boost::dynamic_pointer_cast<FxForwardEngineBuilder>(builder);

    instrument_->qlInstrument()->setPricingEngine(fxBuilder->engine(boughtCcy, soldCcy));

    DLOG("FxForward leg 0: " << legs_[0][0]->date() << " " << legs_[0][0]->amount());
    DLOG("FxForward leg 1: " << legs_[1][0]->date() << " " << legs_[1][0]->amount());
}

void FxForward::fromXML(XMLNode* node) {
    Trade::fromXML(node);
    XMLNode* fxNode = XMLUtils::getChildNode(node, "FxForwardData");
    QL_REQUIRE(fxNode, "No FxForwardData Node");
    maturityDate_ = XMLUtils::getChildValue(fxNode, "ValueDate", true);
    boughtCurrency_ = XMLUtils::getChildValue(fxNode, "BoughtCurrency", true);
    soldCurrency_ = XMLUtils::getChildValue(fxNode, "SoldCurrency", true);
    boughtAmount_ = XMLUtils::getChildValueAsDouble(fxNode, "BoughtAmount", true);
    soldAmount_ = XMLUtils::getChildValueAsDouble(fxNode, "SoldAmount", true);
    settlement_ = XMLUtils::getChildValue(fxNode, "Settlement", false);
    if (settlement_ == "")
        settlement_ = "Physical";
}

XMLNode* FxForward::toXML(XMLDocument& doc) {
    XMLNode* node = Trade::toXML(doc);
    XMLNode* fxNode = doc.allocNode("FxForwardData");
    XMLUtils::appendNode(node, fxNode);
    XMLUtils::addChild(doc, fxNode, "ValueDate", maturityDate_);
    XMLUtils::addChild(doc, fxNode, "BoughtCurrency", boughtCurrency_);
    XMLUtils::addChild(doc, fxNode, "BoughtAmount", boughtAmount_);
    XMLUtils::addChild(doc, fxNode, "SoldCurrency", soldCurrency_);
    XMLUtils::addChild(doc, fxNode, "SoldAmount", soldAmount_);
    XMLUtils::addChild(doc, fxNode, "Settlement", settlement_);
    return node;
}
} // namespace data
} // namespace ore


/* Copyright 2018 Ignacio Torroba (ignaciotb@kth.se)
 *
 * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
 *
 * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef EKF_LOCALIZATION_HPP
#define EKF_LOCALIZATION_HPP

#include <ros/timer.h>
#include <ros/ros.h>

#include <eigen3/Eigen/Eigen>
#include <eigen3/Eigen/Dense>
#include <Eigen/SparseCore>

#include "gazebo_msgs/GetWorldProperties.h"
#include "gazebo_msgs/GetModelState.h"

#include <queue>
#include <math.h>

#include <boost/thread/mutex.hpp>
#include <boost/assign.hpp>
#include <boost/bind.hpp>
#include <boost/scoped_ptr.hpp>

#include <boost/math/distributions/chi_squared.hpp>
#include <boost/math/distributions/inverse_chi_squared.hpp>

#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/TwistWithCovarianceStamped.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/TransformStamped.h>
#include <visualization_msgs/MarkerArray.h>
#include <visualization_msgs/Marker.h>
#include <geometry_msgs/PoseArray.h>

#include <tf/tf.h>
#include <tf2/transform_datatypes.h>
#include <tf2/utils.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>

#include "ekf_slam_core/ekf_slam_core.hpp"
#include "smarc_lm_visualizer/init_map.h"

/**
 * @brief The EKFSLAM class
 * EKF-based localization node for LoLo
 * Inputs:
 * IMU, DVL and landmarks positions from measurements
 * Map as a collection of landmarks with respect to world frame
 * Outputs:
 * nav_msgs/Odometry with an estimate of the 6DOF pose of LoLo
 * updated tf transform odom --> base_link
 */

class EKFSLAM{

public:

    EKFSLAM(std::string node_name, ros::NodeHandle &nh);
    void ekfLocalize(const ros::TimerEvent&);
    ~EKFSLAM();
    void init(std::vector<double> sigma_diag, std::vector<double> r_diag, std::vector<double> q_fls_diag, std::vector<double> q_mbes_diag, double delta, double mhl_dist_fls, double mhl_dist_mbes);

private:

    // ROS variables
    ros::NodeHandle *nh_;
    std::string node_name_;
    ros::Timer timer_;

    // Comms
    ros::Subscriber odom_subs_;
    ros::Subscriber observs_subs_;
    ros::Publisher pose_pub_;
    ros::Publisher vis_pub_;
    ros::ServiceClient init_map_client_;

    // Handlers for sensors
    std::deque<geometry_msgs::PoseArray> measurements_t_;
    std::deque<nav_msgs::Odometry> odom_queue_t_;
    boost::mutex msg_lock_;

    // EKF state variables
    Eigen::VectorXd mu_;
    Eigen::MatrixXd Sigma_;
    EKFCore* ekf_filter_;

    // Mapping variables
    int lm_num_;

    // Aux
    unsigned int size_odom_q_;
    unsigned int size_measurements_q_;

    // tf
    tf::TransformBroadcaster map_bc_;
    tf::TransformListener tf_listener_;
    tf::StampedTransform transf_dvl_base_;
    tf::StampedTransform transf_world_odom_;
    tf::StampedTransform transf_map_world_;
    tf::StampedTransform transf_base_sssr_;
    geometry_msgs::TransformStamped msg_odom_map_;

    std::string odom_frame_;
    std::string fls_frame_;
    std::string mbes_frame_;
    std::string map_frame_;
    std::string world_frame_;
    std::string base_frame_;
    std::string sssr_frame_;
    std::string map_srv_name_;
    std::string lm_srv_name_;
    std::string map_srv_;
    bool mbes_input_;

    // Input callbacks
    void odomCB(const nav_msgs::Odometry &odom_msg);

    void observationsCB(const geometry_msgs::PoseArray &observ_msg);

    /**
     * @brief createMapMarkers
     * Publishes the map as an array of markers for visualization in RVIZ
     */
    void updateMapMarkers(double color);

    /**
     * @brief EKFSLAM::sendOutput
     * @param t
     * @return
     * Publishes AUV odometry info and tf odom --> base_link
     */
    bool sendOutput(ros::Time t_meas);

    bool bcMapOdomTF(ros::Time t_meas);

};

#endif // EKF_LOCALIZATION_HPP


/*******************************************************************************
*
*  Filename    : RooFit_CompareBackground.cc
*  Description : Control flow of background function comparison fitting and plotting
*  Author      : Yi-Mu "Enoch" Chen [ ensc@hep1.phys.ntu.edu.tw ]
*
*******************************************************************************/
#include "TstarAnalysis/Common/interface/PlotStyle.hpp"
#include "TstarAnalysis/LimitCalc/interface/Common.hpp"
#include "TstarAnalysis/LimitCalc/interface/SampleRooFitMgr.hpp"

#include "ManagerUtils/Maths/interface/RooFitExt.hpp"
#include "ManagerUtils/PlotUtils/interface/Common.hpp"
#include "ManagerUtils/PlotUtils/interface/RooFitUtils.hpp"

#include "RooDataSet.h"
#include "RooFitResult.h"
#include "RooMinimizer.h"
#include "RooNLLVar.h"
#include "TMath.h"

#include <boost/format.hpp>
#include <vector>

using namespace std;

static Color_t sequence[] = {
  KBLUE,
  KGREEN,
  KRED,
  KPURPLE
};

struct MyFitResult
{
  RooFitResult*      fitresult;
  TGraph*            fitplot;
  TGraphAsymmErrors* pullplot;
  double             ksvalue;
};

/******************************************************************************/

void
CompareFitFunc( SampleRooFitMgr* mgr )
{
  TCanvas* c        = mgr::NewCanvas();
  TPad* toppad      = mgr::NewTopPad();
  TPad* botpad      = mgr::NewBottomPad();
  RooPlot* frame    = SampleRooFitMgr::x().frame();
  const double xmin = SampleRooFitMgr::x().getMin();
  const double xmax = SampleRooFitMgr::x().getMax();

  vector<MyFitResult> fitlist;
  const vector<string> funclist = limnamer.GetInputList<string>( "fitfunc" );

  /*******************************************************************************
  *   Making the top plots
  *******************************************************************************/
  toppad->Draw();
  toppad->cd();

  TGraphAsymmErrors* setplot = (TGraphAsymmErrors*)mgr::PlotOn(
    frame, mgr->DataSet( "" ),
    RooFit::DrawOption( PGS_DATA )
    );

  vector<double> prevfitvaluelist = {280, 0.51};

  for( const auto& fitfunc : funclist ){
    RooAbsPdf* pdf  = mgr->NewPdf( fitfunc, fitfunc );
    RooDataSet* set = (RooDataSet*)( mgr->DataSet( "" ) );

    // Setting up fitting parameters with previous fit results for faster convertions
    vector<RooRealVar*> fitparams = mgr->VarContains( fitfunc );

    for( size_t i = 0; i < fitparams.size(); ++i ){
      if( i < prevfitvaluelist.size() ){
        *( fitparams.at( i ) ) = prevfitvaluelist.at( i );
      } else {
        *( fitparams.at( i ) ) = 0;
      }
    }

    // Manually calling NNL minizer functions
    static RooCmdArg min    = RooFit::Minimizer( "Minuit", "Migrad" );
    static RooCmdArg sumerr = RooFit::SumW2Error( kTRUE );
    static RooCmdArg minos  = RooFit::Minos( kTRUE );
    static RooCmdArg save   = RooFit::Save();
    // static RooCmdArg ncpu   = RooFit::NumCPU( 6 );
    static RooCmdArg verb   = RooFit::Verbose( kFALSE );
    static RooCmdArg printl = RooFit::PrintLevel( -1 );
    static RooCmdArg printe = RooFit::PrintEvalErrors( -1 );
    static RooCmdArg printw = RooFit::Warnings( kFALSE );

    RooLinkedList fitopt;
    fitopt.Add( &min    );
    fitopt.Add( &sumerr );
    fitopt.Add( &minos  );
    fitopt.Add( &save   );
    // fitopt.Add( &ncpu   );
    fitopt.Add( &verb   );
    fitopt.Add( &printl );
    fitopt.Add( &printe );
    fitopt.Add( &printw );

    RooFitResult* ans = NULL;
    const unsigned maxiter = 50;
    unsigned iter = 0;
    while( !ans ){
      ans = pdf->fitTo( *set, fitopt );
      if( ans->status() ){// Not properly converged
        ++iter;
        if( iter > maxiter ) { break; }
        delete ans;
        ans = NULL;
      }
    }

    MyFitResult x;

    // Saving fit results
    x.fitresult = ans;
    x.fitplot   = mgr::PlotOn( frame, pdf );
    x.ksvalue   = KSTest( *set, *pdf, SampleRooFitMgr::x() );
    x.pullplot  = mgr::DividedGraph( setplot, x.fitplot );

    // Small information dump after fit
    prevfitvaluelist.clear();
    cout << "Minimum NLL:" << x.fitresult->minNll() << endl;
    cout << "Parameter fit values: " << endl;

    for( int i = 0; i < x.fitresult->floatParsFinal().getSize(); ++i ){
      RooRealVar* var = (RooRealVar*)( x.fitresult->floatParsFinal().at( i ) );
      cout << var->GetName() << " " << var->getVal() << " " << var->getError() << endl;
      prevfitvaluelist.push_back( var->getVal() );
    }

    cout << "===" << endl;

    fitlist.push_back( x );
  }

  frame->Draw();
  frame->SetMinimum( 0.3 );
  mgr::SetTopPlotAxis( frame );
  frame->SetTitle( "" );
  c->cd();

  /*******************************************************************************
  *   Drawing bottom pad.
  *******************************************************************************/
  botpad->Draw();
  botpad->cd();

  TMultiGraph* mg = new TMultiGraph;

  for( const auto fitresult : fitlist ){
    mg->Add( fitresult.pullplot, "Z" );
  }

  TLine line( xmin, 1, xmax, 1 );
  TLine line_top( xmin, 1.5, xmax, 1.5 );
  TLine line_bot( xmin, 0.5, xmax, 0.5 );

  mg->Draw( "AZ" );
  line.Draw( "SAME" );
  line_top.Draw( "SAME" );
  line_bot.Draw( "SAME" );
  mg->GetXaxis()->SetTitle( frame->GetXaxis()->GetTitle() );
  mg->GetYaxis()->SetTitle( "Data/Fit" );
  mg->GetXaxis()->SetRangeUser( xmin, xmax );
  mgr::SetBottomPlotAxis( mg );
  mg->SetMaximum( 1.6 );
  mg->SetMinimum( 0.4 );
  c->cd();

  /*******************************************************************************
  *   Object styling
  *******************************************************************************/
  tstar::SetDataStyle( setplot );

  line.SetLineColor( KRED );
  line.SetLineStyle( 1 );
  line_top.SetLineColor( kBlack );
  line_bot.SetLineColor( kBlack );
  line_top.SetLineStyle( 3 );
  line_bot.SetLineStyle( 3 );

  // Drawing common
  mgr::DrawCMSLabel();
  mgr::DrawLuminosity( mgr::SampleMgr::TotalLuminosity() );

  // Drawing legends
  TLegend* l = mgr::NewLegend( 0.45, 0.6 );
  if( limnamer.GetInput<string>( "sample" ) == "Data" ){
    l->AddEntry( setplot, "Data", "lp" );
  } else {
    l->AddEntry( setplot, "MC background", "lp" );
  }

  for( size_t i = 0; i < fitlist.size(); ++i ){
    auto fitplot  = fitlist.at( i ).fitplot;
    auto pullplot = fitlist.at( i ).pullplot;
    fitplot->SetLineColor( sequence[i] );
    pullplot->SetLineColor( sequence[i] );
    fitplot->SetLineWidth( 2 );
    pullplot->SetLineWidth( 2 );
    auto funcname = funclist.at( i );

    boost::format fitentryfmt( "%s K=%.3lf" );
    const string fitentry = boost::str(
      fitentryfmt
      % limnamer.ExtQuery<string>( "fitfunc", funcname, "Full Name" )
      % fitlist.at( i ).ksvalue
      );
    l->AddEntry( fitplot, fitentry.c_str(), "l" );
  }

  l->Draw();

  mgr::LatexMgr latex;
  latex.SetOrigin( PLOT_X_TEXT_MIN, PLOT_Y_TEXT_MAX, TOP_LEFT )
  .WriteLine( limnamer.GetChannelEXT( "Root Name" ) );

  boost::format chi2testfmt( "LR Test_{%d/%d} = %.3lf" );

  // Writing LR test results
  for( size_t i = 0; i+1 < fitlist.size(); ++i ){
    const double thisnll  = fitlist[i].fitresult->minNll();
    const double nextnll  = fitlist[i+1].fitresult->minNll();
    const int thisorder   = fitlist[i].fitresult->floatParsFinal().getSize();
    const int nextorder   = fitlist[i+1].fitresult->floatParsFinal().getSize();
    const double chi2test = TMath::Prob( 2*( std::max( fabs( thisnll-nextnll ), 0. ) ), nextorder-thisorder );

    latex.WriteLine( boost::str( chi2testfmt % thisorder % nextorder % chi2test ) );
  }

  /*******************************************************************************
  *   Updating everything and saving
  *******************************************************************************/
  toppad->Update();
  botpad->Update();
  c->Update();

  frame->SetMaximum( mgr::GetYmax( setplot )*1.5 );
  mgr::SaveToPDF( c, limnamer.PlotFileName( "bkgcomp" ) );
  mgr::SaveToROOT( c, limnamer.PlotRootFile(), limnamer.PlotFileName( "bkgcomp" ) );
  toppad->SetLogy( kTRUE );
  frame->SetMaximum( mgr::GetYmax( setplot )*300 );
  mgr::SaveToPDF( c, limnamer.PlotFileName( "bkgcomp", "log" ) );

  /*******************************************************************************
  *   Printing results and leaving
  *******************************************************************************/
  for( auto fit : fitlist ){
    cout << boost::str( boost::format( "%.6lf" ) % fit.fitresult->minNll() ) << endl;

    for( int i = 0; i < fit.fitresult->floatParsFinal().getSize(); ++i ){
      RooRealVar* var = (RooRealVar*)( fit.fitresult->floatParsFinal().at( i ) );
      cout << var->GetName() << " " << var->getVal() << " " << var->getError() << endl;
    }

    cout << "===" << endl;
  }

  delete toppad;
  delete botpad;
  delete c;
  delete frame;
}


/* -*- c-basic-offset: 4 indent-tabs-mode: nil -*-  vi:set ts=8 sts=4 sw=4: */

#include "cq/CQKernel.h"

#include <cmath>
#include <vector>
#include <iostream>

using std::vector;
using std::cerr;
using std::endl;

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>

static int rate = 123;
static int max = 60;
static int min = 12;
static int bpo = 4;

BOOST_AUTO_TEST_SUITE(TestCQKernel)

// Just some simple tests on kernel construction -- make sure it's the
// right size, etc

BOOST_AUTO_TEST_CASE(sampleRate) {
    CQParameters params(rate, min, max, bpo);
    CQKernel k(params);
    BOOST_CHECK_EQUAL(k.getProperties().sampleRate, rate);
}

BOOST_AUTO_TEST_CASE(binsPerOctave) {
    CQParameters params(rate, min, max, bpo);
    CQKernel k(params);
    BOOST_CHECK_EQUAL(k.getProperties().binsPerOctave, bpo);
}

BOOST_AUTO_TEST_CASE(maxFrequency) {
    CQParameters params(rate, min, max, bpo);
    CQKernel k(params);
    BOOST_CHECK_EQUAL(k.getProperties().maxFrequency, max);
}

BOOST_AUTO_TEST_CASE(minFrequency) {
    CQParameters params(rate, min, max, bpo);
    CQKernel k(params);
    BOOST_CHECK_CLOSE(k.getProperties().minFrequency,
		      (max / 2.0) * pow(2, 1.0/bpo),
		      1e-8);
}

BOOST_AUTO_TEST_CASE(atomsPerFrame) {
    CQParameters params(rate, min, max, bpo);
    CQKernel k(params);
    BOOST_CHECK_EQUAL(k.getProperties().atomsPerFrame, 5);
}

BOOST_AUTO_TEST_CASE(fftSize) {
    CQParameters params(rate, min, max, bpo);
    CQKernel k(params);
    BOOST_CHECK_EQUAL(k.getProperties().fftSize, 32);
}

BOOST_AUTO_TEST_SUITE_END()


#include "DlgCheck.h"
#include "ui_DlgCheck.h"
#include "Configure.h"
#include <QFileDialog>
#include <QMessageBox>
#include "UtilityTool.h"
#include <fstream>
#include <vector>
#include <boost/filesystem.hpp>
#include <boost/format.hpp>

DlgCheck::DlgCheck(QWidget *parent) :
    QDialog(parent),
    ui(new Ui::DlgCheck)
{
    ui->setupUi(this);
    ui->le_front->setText(CODE_TO_UI("E:\\Projects\\qt\\panorama\\build-panorama-Desktop_Qt_5_14_0_MSVC2017_64bit-Debug\\bd"));
}

DlgCheck::~DlgCheck()
{
    delete ui;
}

void DlgCheck::on_pb_check_clicked()
{
    try{
        auto pconfig = Configure::get_instance();
        auto path_front = ui->le_front->text().toStdString();
        if(path_front.empty()){
            return;
        }
        std::vector<cv::Mat> imgs;
        boost::filesystem::path myPath(path_front);
        boost::filesystem::directory_iterator endIter;
        for (boost::filesystem::directory_iterator iter(myPath); iter != endIter; iter++) {
            if (boost::filesystem::is_directory(*iter)) {
                continue;
            } else {
                auto extent = iter->path().extension();
                if(".jpg" != extent.string()){
                    continue;
                }
                cv::Mat img_front = cv::imread(iter->path().string());
                imgs.push_back(img_front);
            }
        }
        cv::Mat map_x = cv::Mat(cv::Size(1920, 1080), CV_32FC1);
        cv::Mat map_y = cv::Mat(cv::Size(1920, 1080), CV_32FC1);
        cv::Size board_size(6, 9);
        if(!check_camera(map_x, map_y, imgs, board_size)){
            return;
        }
    }catch(const std::exception& e){
        LOG_ERROR("发生错误:"<<e.what());
        QMessageBox::critical(this, CODE_TO_UI("错误"), CODE_TO_UI(e.what()));
    }

}

bool DlgCheck::check_camera( cv::Mat& map_x, cv::Mat& map_y, std::vector<cv::Mat>& imgs, cv::Size& board_size){
    std::vector<cv::Point2f> corners;                  /****    缓存每幅图像上检测到的角点       ****/
    std::vector<std::vector<cv::Point2f>>  corners_Seq;    /****  保存检测到的所有角点       ****/
    int count = 0;
    std::vector<cv::Mat> image_Seq;
    for (auto& img : imgs)
    {
        corners.clear();
        // 灰度化图像
        cv::Mat imageGray;
        cv::cvtColor(img, imageGray, cv::COLOR_RGB2GRAY);
        // 自动获取图像上的角点坐标
        bool patternfound = findChessboardCorners(imageGray, board_size, corners, cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_NORMALIZE_IMAGE +
                                                  cv::CALIB_CB_FAST_CHECK);
        if (!patternfound)
        {
            continue;
        }
        count = count + corners.size();
        corners_Seq.push_back( corners);
        image_Seq.push_back(img);
    }
    cv::Size square_size = cv::Size(20, 20);
    std::vector<std::vector<cv::Point3f> >  object_Points;        /****  保存定标板上角点的三维坐标   ****/

    cv::Mat image_points = cv::Mat(1, count, CV_32FC2, cv::Scalar::all(0));  /*****   保存提取的所有角点   *****/
    std::vector<int>  point_counts;
    // 这是是输入棋盘角点实际坐标，通过与图像上检测到的角点坐标做计算，就可以计算出相关的矩阵
    for (std::size_t t = 0; t<image_Seq.size(); ++t)
    {
        std::vector<cv::Point3f> tempPointSet;
        for (int i = 0; i<board_size.height; i++)
        {
            for (int j = 0; j<board_size.width; j++)
            {
                /* 假设定标板放在世界坐标系中z=0的平面上 */
                cv::Point3f tempPoint;
                tempPoint.x = i*square_size.width;
                tempPoint.y = j*square_size.height;
                tempPoint.z = 0;
                tempPointSet.push_back(tempPoint);
            }
        }
        object_Points.push_back(tempPointSet);
    }
    for (std::size_t i = 0; i< image_Seq.size(); ++i)
    {
        point_counts.push_back(board_size.width*board_size.height);
    }
    /* 开始定标 */
    cv::Size image_size = image_Seq[0].size();
    cv::Matx33d intrinsic_matrix;    /*****    摄像机内参数矩阵    ****/
    cv::Vec4d distortion_coeffs;     /* 摄像机的4个畸变系数：k1,k2,k3,k4*/
    std::vector<cv::Vec3d> rotation_vectors;                           /* 每幅图像的旋转向量 */
    std::vector<cv::Vec3d> translation_vectors;                        /* 每幅图像的平移向量 */
    int flags = 0;
    flags |= cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC;
    flags |= cv::fisheye::CALIB_CHECK_COND;
    flags |= cv::fisheye::CALIB_FIX_SKEW;
    cv::fisheye::calibrate(object_Points, corners_Seq, image_size, intrinsic_matrix, distortion_coeffs, rotation_vectors, translation_vectors, flags, cv::TermCriteria(3, 20, 1e-6));

    cv::Mat rotation_matrix = cv::Mat(3, 3, CV_32FC1, cv::Scalar::all(0)); /* 保存每幅图像的旋转矩阵 */
    cv::Mat R = cv::Mat::eye(3, 3, CV_32F);
    cv::fisheye::initUndistortRectifyMap(intrinsic_matrix,distortion_coeffs,R,intrinsic_matrix,image_size,CV_32FC1,map_x,map_y);
    //cv::fisheye::initUndistortRectifyMap(intrinsic_matrix, distortion_coeffs, R,
    //    cv::getOptimalNewCameraMatrix(intrinsic_matrix, distortion_coeffs, image_size, 1, image_size, 0),image_size, CV_32FC1, map_x, map_y);
    for (std::size_t i = 0; i < image_Seq.size(); ++i)
    {
        cv::Mat t = image_Seq[i].clone();
        cv::remap(image_Seq[i], t, map_x, map_y, cv::INTER_LINEAR);
        cv::imwrite((boost::format("./%d.jpg") % i).str(), t);
    }
    return true;
}

void DlgCheck::on_pb_front_clicked()
{
    auto path_folder = QFileDialog::getExistingDirectory(this, CODE_TO_UI("选择文件夹"), CODE_TO_UI("./"));
    if(path_folder.isEmpty()){
        return;
    }
    ui->le_front->setText(path_folder);
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_SWAR_FUNCTIONS_SIMD_VMX_ALTIVEC_ENUMERATE_HPP_INCLUDED
#define BOOST_SIMD_SWAR_FUNCTIONS_SIMD_VMX_ALTIVEC_ENUMERATE_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_VMX_SUPPORT

#include <boost/simd/swar/functions/enumerate.hpp>
#include <boost/simd/include/functions/simd/splat.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::enumerate_
                                    , boost::simd::tag::altivec_
                                    , (A0)(T)
                                    , (scalar_< arithmetic_<A0> >)
                                      ((target_
                                        < simd_ < ints8_<T>
                                                , boost::simd::tag::altivec_
                                                >
                                        >
                                      ))
                                    )
  {
    typedef typename T::type result_type;
    typedef typename result_type::native_type native_type;

    result_type operator()(A0 const& a0, T const& ) const
    {
      // add [a0 ... a0] with [0 1 2 ... 12 15]
      return vec_add ( splat<result_type>(a0)()
                     , bitwise_cast<result_type>(vec_lvsl(0,(unsigned char*)(0)))()
                     );
    }
  };
} } }

#endif
#endif


// Copyright 2017 Emanuele Palazzolo (emanuele.palazzolo@uni-bonn.de)
#include "fastcd/processed_image.h"
#include <algorithm>
#include <string>
#include <vector>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <Eigen/Core>
#include "fastcd/camera.h"

namespace fastcd {

ProcessedImage::ProcessedImage(const Image &img,
                               const std::vector<glow::vec4> &depth)
    : Image(img), depth_(depth) {
  inconsistencies_ =
        cv::Mat(raw_image_.rows, raw_image_.cols, CV_32FC1, cv::Scalar(0));
}

Image ProcessedImage::Warp(const ProcessedImage &target_image) const {
  cv::Mat result(raw_image_.rows, raw_image_.cols, raw_image_.type(),
                 cv::Scalar(0, 0, 0));
  cv::Mat zbuffer(raw_image_.rows / 2, raw_image_.cols / 2, CV_64FC1,
                  cv::Scalar(1000));
  Eigen::Vector2i coord;
  #pragma omp parallel for collapse(2)
  for (int i = 0; i < raw_image_.rows; ++i) {
    for (int j = 0; j < raw_image_.cols; j++) {
      int idx = i * raw_image_.cols + j;
      if (depth_[idx].w > 0.5) {
        coord = target_image.GetCamera().Project(
            static_cast<double>(depth_[idx].x),
            static_cast<double>(depth_[idx].y),
            static_cast<double>(depth_[idx].z));
        // Check if the point is seen in both images
        // (taking into account occlusions)
        if (coord(1) > 0 && coord(1) < raw_image_.rows && coord(0) > 0 &&
            coord(0) < raw_image_.cols) {
          double distance =
              sqrt(pow(static_cast<double>(depth_[idx].x) -
                           target_image.GetCamera().GetPosition()(0), 2) +
                   pow(static_cast<double>(depth_[idx].y) -
                           target_image.GetCamera().GetPosition()(1), 2) +
                   pow(static_cast<double>(depth_[idx].z) -
                           target_image.GetCamera().GetPosition()(2), 2));
          if (distance < zbuffer.at<double>(coord(1) / 2, coord(0) / 2)) {
            zbuffer.at<double>(coord(1) / 2, coord(0) / 2) = distance;
            int depth_idx =
                (raw_image_.rows - coord(1) - 1) * target_image.Width() +
                coord(0);
            double dist_target =
                sqrt(pow(target_image.GetCamera().GetPosition()(0) -
                             target_image.GetDepth()[depth_idx].x, 2) +
                     pow(target_image.GetCamera().GetPosition()(1) -
                             target_image.GetDepth()[depth_idx].y, 2) +
                     pow(target_image.GetCamera().GetPosition()(2) -
                             target_image.GetDepth()[depth_idx].z, 2));
            if (dist_target >= distance - 0.5) {
              result.at<cv::Vec3b>(coord(1), coord(0)) =
                  raw_image_.at<cv::Vec3b>(raw_image_.rows - i - 1, j);
            }
          }
        }
      }
    }
  }

  // Interpolation
  cv::Mat interpolation(result.rows, result.cols, result.type());
  int dilation_size = 1;
  cv::Mat dilation_kernel = cv::getStructuringElement(
      cv::MORPH_RECT, cv::Size(2 * dilation_size + 1, 2 * dilation_size + 1),
      cv::Point(dilation_size, dilation_size));
  cv::dilate(result, interpolation, dilation_kernel);
  for (int i = 0; i < result.rows; i++) {
    for (int j = 0; j < result.cols; j++) {
      if (result.at<cv::Vec3b>(i, j) == cv::Vec3b(0, 0, 0)) {
        result.at<cv::Vec3b>(i, j) = interpolation.at<cv::Vec3b>(i, j);
      }
    }
  }
  return Image(result, target_image.GetCamera());
}

void ProcessedImage::UpdateInconsistencies(const Image &image,
                                           int kernel_size) {
  cv::Mat inconsistency =
      Subtract(raw_image_, image.GetRawImage(), kernel_size);

  cv::normalize(inconsistency, inconsistency, 0.0f, 1.0f, cv::NORM_MINMAX);
  if (num_img_compared_ > 0)
    inconsistencies_ = Minimum(inconsistencies_, inconsistency);
  else
    inconsistencies_ = inconsistency;
  cv::medianBlur(inconsistencies_, inconsistencies_, 3);
  cv::normalize(inconsistencies_, inconsistencies_, 0.0f, 1.0f,
                cv::NORM_MINMAX);

  num_img_compared_++;
}

cv::Mat ProcessedImage::GetInconsistencies() { return inconsistencies_; }

std::vector<int> ProcessedImage::ComputeMeansCovariances(double chi_square) {
  std::vector<int> found_labels;
  for (int i = 0; i <= max_region_label_; i++) {
    std::vector<int> idx;
    for (size_t j = 0; j < regions_.size(); j++) {
      if (regions_[j].label_ == i) {
        idx.push_back(j);
      }
    }
    if (!idx.empty()) {
      found_labels.push_back(i);
      cv::Mat region(Height(), Width(), CV_8UC1, cv::Scalar(0));
      std::vector<std::vector<cv::Point>> contours;
      for (int j : idx) {
        contours.push_back(regions_[j].contour_);
      }
      cv::drawContours(region, contours, -1, cv::Scalar(255), CV_FILLED);
      cv::Mat non_zero = FindNonZero(region);
      cv::Mat mean;
      cv::Mat covariance(2, 2, CV_64F);
      cv::calcCovarMatrix(non_zero, covariance, mean,
                        CV_COVAR_NORMAL | CV_COVAR_ROWS | CV_COVAR_SCALE);
      Eigen::Vector2d emean;
      Eigen::Matrix2d ecovariance;
      emean << mean.at<double>(0, 0), mean.at<double>(0, 1);
      ecovariance << covariance.at<double>(0, 0), covariance.at<double>(0, 1),
      covariance.at<double>(1, 0), covariance.at<double>(1, 1);
      PointCovariance2d mean_covariance(emean, ecovariance, chi_square);
      regions_mean_cov_[i] = mean_covariance;
    }
  }
  return found_labels;
}

void ProcessedImage::ComputeRegions(int threshold_change_area) {
  cv::Mat image(Height(), Width(), CV_8UC1);
  inconsistencies_.convertTo(image, CV_8UC1, 255);
  cv::threshold(image, image, 30, 255.0f, cv::THRESH_TOZERO);

  cv::Mat dilation_kernel =
      cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(11, 11));
  cv::Mat erosion_kernel =
      cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5, 5));
  cv::erode(image, image, erosion_kernel);
  cv::dilate(image, image, dilation_kernel);
  image = AverageSegmentedRegions(image, threshold_change_area);

  cv::equalizeHist(image, image);
  cv::normalize(image, image, 0, 255, cv::NORM_MINMAX);
  cv::threshold(image, image, 50, 255.0f, cv::THRESH_TOZERO);

  std::vector<std::vector<cv::Point>> contours;
  cv::findContours(image, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);
  for (size_t i = 0; i < contours.size(); i++) {
    ImageRegion region(contours[i]);
    regions_.push_back(region);
  }
}

const std::vector<ImageRegion>& ProcessedImage::GetRegions() const {
  return regions_;
}

const std::vector<glow::vec4>& ProcessedImage::GetDepth() const {
  return depth_;
}

void ProcessedImage::UpdateLabels(const std::vector<int> &labels) {
  for (size_t i = 0; i < regions_.size(); i++) {
    if (labels[i] > max_region_label_) max_region_label_ = labels[i];
    regions_[i].label_ = labels[i];
  }
}

cv::Mat ProcessedImage::AverageSegmentedRegions(const cv::Mat &img,
                                                int threshold_change_area) {
  cv::Mat labels(img.rows, img.cols, img.type(), cv::Scalar(0));
  cv::Mat dst(img.rows, img.cols, img.type(), cv::Scalar(0));
  cv::Mat tmp = img.clone();
  std::vector<std::vector<cv::Point>> contours;
  cv::findContours(tmp, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);
  std::vector<float> cont_avgs(contours.size(), 0.f);
  for (size_t i = 0; i < contours.size(); ++i) {
    cv::drawContours(labels, contours, i, cv::Scalar(i + 1), CV_FILLED);
    cv::Rect roi = cv::boundingRect(contours[i]);
    cv::Mat mask;
    cv::bitwise_and(labels(roi) == (i + 1), img(roi) != 0, mask);
    cv::Scalar mean = cv::mean(img(roi), mask);
    cont_avgs[i] = mean[0];
  }
  for (size_t i = 0; i < contours.size(); ++i) {
    if (cv::contourArea(contours[i]) > threshold_change_area)
      cv::drawContours(dst, contours, i, cv::Scalar(cont_avgs[i]), CV_FILLED);
  }
  return dst;
}

std::unordered_map<int, PointCovariance2d>
ProcessedImage::GetMeansCovariances() const {
  return regions_mean_cov_;
}

cv::Mat ProcessedImage::Subtract(const cv::Mat &img1, const cv::Mat &img2,
                                 int kernel_size) {
  cv::Mat result(img1.rows, img1.cols, CV_32FC1, 0.0f);

  for (int i = 0; i < img1.rows; ++i) {
    for (int j = 0; j < img1.cols; ++j) {
      Eigen::Vector3f img1_color, img2_color;
      img1_color << static_cast<float>(img1.at<cv::Vec3b>(i, j)[0]),
          static_cast<float>(img1.at<cv::Vec3b>(i, j)[1]),
          static_cast<float>(img1.at<cv::Vec3b>(i, j)[2]);
      if (img1_color.norm() > 0.01) {
        if (j - kernel_size / 2 > 0 &&
            i - kernel_size / 2 > 0 &&
            j + kernel_size / 2 < img2.cols &&
            i + kernel_size / 2 < img2.rows) {
          cv::Rect window(j - kernel_size / 2,
                          i - kernel_size / 2, kernel_size,
                          kernel_size);
          // Loop on the window
          float min = 10000;
          for (int subi = 0; subi < window.height; subi++) {
            for (int subj = 0; subj < window.width; subj++) {
              cv::Mat area = img2(window);
              img2_color << static_cast<float>(
                  area.at<cv::Vec3b>(subi, subj)[0]),
                  static_cast<float>(area.at<cv::Vec3b>(subi, subj)[1]),
                  static_cast<float>(area.at<cv::Vec3b>(subi, subj)[2]);
            // Compare the values here and keep track of the minimum difference
              if (img2_color.norm() > 0.01) {
                float difference = (img1_color - img2_color).norm();
                if (difference < min) min = difference;
              }
            }
          }
          if (min < 1000) result.at<float>(i, j) = min;
        }
      }
    }
  }
  return result;
}

cv::Mat ProcessedImage::Minimum(const cv::Mat &img1, const cv::Mat &img2) {
  cv::Mat result(img1.rows, img1.cols, CV_32FC1, 0.0f);
  #pragma omp parallel for collapse(2)
  for (int i = 0; i < img1.rows; ++i) {
    for (int j = 0; j < img1.cols; ++j) {
      result.at<float>(i, j) =
          std::min(img1.at<float>(i, j), img2.at<float>(i, j));
    }
  }
  return result;
}

cv::Mat ProcessedImage::FindNonZero(const cv::Mat &img) {
  cv::Mat result(img.rows * img.cols, 2, CV_64F);
  int idx = 0;
  for (int i = 0; i < img.rows; i++) {
    for (int j = 0; j < img.cols; j++) {
      if (img.at<uchar>(i, j) > 0) {
        result.at<double>(idx, 0) = j;
        result.at<double>(idx, 1) = i;
        idx++;
      }
    }
  }
  return result(cv::Rect(0, 0, 2, idx));
}

int ProcessedImage::ImageCompared() { return num_img_compared_; }

}  // namespace fastcd


// Distributed under the MIT License.
// See LICENSE.txt for details.

#include "Evolution/Systems/GrMhd/ValenciaDivClean/FiniteDifference/MonotisedCentral.hpp"

#include <array>
#include <boost/functional/hash.hpp>
#include <cstddef>
#include <memory>
#include <pup.h>
#include <utility>
#include <vector>

#include "DataStructures/DataVector.hpp"
#include "DataStructures/FixedHashMap.hpp"
#include "DataStructures/Index.hpp"
#include "DataStructures/Tensor/Tensor.hpp"
#include "DataStructures/Variables.hpp"
#include "Domain/Structure/Direction.hpp"
#include "Domain/Structure/Element.hpp"
#include "Domain/Structure/ElementId.hpp"
#include "Domain/Structure/MaxNumberOfNeighbors.hpp"
#include "Evolution/DiscontinuousGalerkin/Actions/NormalCovectorAndMagnitude.hpp"
#include "Evolution/Systems/GrMhd/ValenciaDivClean/FiniteDifference/ReconstructWork.tpp"
#include "NumericalAlgorithms/FiniteDifference/MonotisedCentral.hpp"
#include "NumericalAlgorithms/Spectral/Mesh.hpp"
#include "PointwiseFunctions/Hydro/EquationsOfState/EquationOfState.hpp"
#include "Utilities/GenerateInstantiations.hpp"
#include "Utilities/Gsl.hpp"
#include "Utilities/TMPL.hpp"

namespace grmhd::ValenciaDivClean::fd {
MonotisedCentralPrim::MonotisedCentralPrim(CkMigrateMessage* const msg)
    : Reconstructor(msg) {}

std::unique_ptr<Reconstructor> MonotisedCentralPrim::get_clone() const {
  return std::make_unique<MonotisedCentralPrim>(*this);
}

void MonotisedCentralPrim::pup(PUP::er& p) { Reconstructor::pup(p); }

// NOLINTNEXTLINE
PUP::able::PUP_ID MonotisedCentralPrim::my_PUP_ID = 0;

template <size_t ThermodynamicDim, typename TagsList>
void MonotisedCentralPrim::reconstruct(
    const gsl::not_null<std::array<Variables<TagsList>, dim>*>
        vars_on_lower_face,
    const gsl::not_null<std::array<Variables<TagsList>, dim>*>
        vars_on_upper_face,
    const Variables<hydro::grmhd_tags<DataVector>>& volume_prims,
    const EquationsOfState::EquationOfState<true, ThermodynamicDim>& eos,
    const Element<dim>& element,
    const FixedHashMap<
        maximum_number_of_neighbors(dim),
        std::pair<Direction<dim>, ElementId<dim>>, std::vector<double>,
        boost::hash<std::pair<Direction<dim>, ElementId<dim>>>>& neighbor_data,
    const Mesh<dim>& subcell_mesh) const {
  reconstruct_prims_work(
      vars_on_lower_face, vars_on_upper_face,
      [](auto upper_face_vars_ptr, auto lower_face_vars_ptr,
         const auto& volume_vars, const auto& ghost_cell_vars,
         const auto& subcell_extents, const size_t number_of_variables) {
        ::fd::reconstruction::monotised_central(
            upper_face_vars_ptr, lower_face_vars_ptr, volume_vars,
            ghost_cell_vars, subcell_extents, number_of_variables);
      },
      volume_prims, eos, element, neighbor_data, subcell_mesh,
      ghost_zone_size());
}

template <size_t ThermodynamicDim, typename TagsList>
void MonotisedCentralPrim::reconstruct_fd_neighbor(
    const gsl::not_null<Variables<TagsList>*> vars_on_face,
    const Variables<hydro::grmhd_tags<DataVector>>& subcell_volume_prims,
    const EquationsOfState::EquationOfState<true, ThermodynamicDim>& eos,
    const Element<dim>& element,
    const FixedHashMap<
        maximum_number_of_neighbors(dim),
        std::pair<Direction<dim>, ElementId<dim>>, std::vector<double>,
        boost::hash<std::pair<Direction<dim>, ElementId<dim>>>>& neighbor_data,
    const Mesh<dim>& subcell_mesh,
    const Direction<dim> direction_to_reconstruct) const {
  reconstruct_fd_neighbor_work(
      vars_on_face,
      [](const auto tensor_component_on_face_ptr,
         const auto& tensor_component_volume,
         const auto& tensor_component_neighbor,
         const Index<dim>& subcell_extents,
         const Index<dim>& ghost_data_extents,
         const Direction<dim>& local_direction_to_reconstruct) {
        ::fd::reconstruction::reconstruct_neighbor<
            Side::Lower,
            ::fd::reconstruction::detail::MonotisedCentralReconstructor>(
            tensor_component_on_face_ptr, tensor_component_volume,
            tensor_component_neighbor, subcell_extents, ghost_data_extents,
            local_direction_to_reconstruct);
      },
      [](const auto tensor_component_on_face_ptr,
         const auto& tensor_component_volume,
         const auto& tensor_component_neighbor,
         const Index<dim>& subcell_extents,
         const Index<dim>& ghost_data_extents,
         const Direction<dim>& local_direction_to_reconstruct) {
        ::fd::reconstruction::reconstruct_neighbor<
            Side::Upper,
            ::fd::reconstruction::detail::MonotisedCentralReconstructor>(
            tensor_component_on_face_ptr, tensor_component_volume,
            tensor_component_neighbor, subcell_extents, ghost_data_extents,
            local_direction_to_reconstruct);
      },
      subcell_volume_prims, eos, element, neighbor_data, subcell_mesh,
      direction_to_reconstruct, ghost_zone_size());
}

bool operator==(const MonotisedCentralPrim& /*lhs*/,
                const MonotisedCentralPrim& /*rhs*/) {
  return true;
}

bool operator!=(const MonotisedCentralPrim& lhs,
                const MonotisedCentralPrim& rhs) {
  return not(lhs == rhs);
}

#define THERMO_DIM(data) BOOST_PP_TUPLE_ELEM(0, data)
#define TAGS_LIST(data)                                                      \
  tmpl::list<Tags::TildeD, Tags::TildeTau, Tags::TildeS<Frame::Inertial>,    \
             Tags::TildeB<Frame::Inertial>, Tags::TildePhi,                  \
             hydro::Tags::RestMassDensity<DataVector>,                       \
             hydro::Tags::SpecificInternalEnergy<DataVector>,                \
             hydro::Tags::SpatialVelocity<DataVector, 3>,                    \
             hydro::Tags::MagneticField<DataVector, 3>,                      \
             hydro::Tags::DivergenceCleaningField<DataVector>,               \
             hydro::Tags::LorentzFactor<DataVector>,                         \
             hydro::Tags::Pressure<DataVector>,                              \
             hydro::Tags::SpecificEnthalpy<DataVector>,                      \
             hydro::Tags::LorentzFactorTimesSpatialVelocity<DataVector, 3>,  \
             ::Tags::Flux<Tags::TildeD, tmpl::size_t<3>, Frame::Inertial>,   \
             ::Tags::Flux<Tags::TildeTau, tmpl::size_t<3>, Frame::Inertial>, \
             ::Tags::Flux<Tags::TildeS<Frame::Inertial>, tmpl::size_t<3>,    \
                          Frame::Inertial>,                                  \
             ::Tags::Flux<Tags::TildeB<Frame::Inertial>, tmpl::size_t<3>,    \
                          Frame::Inertial>,                                  \
             ::Tags::Flux<Tags::TildePhi, tmpl::size_t<3>, Frame::Inertial>, \
             gr::Tags::Lapse<DataVector>,                                    \
             gr::Tags::Shift<3, Frame::Inertial, DataVector>,                \
             gr::Tags::SpatialMetric<3>,                                     \
             gr::Tags::SqrtDetSpatialMetric<DataVector>,                     \
             gr::Tags::InverseSpatialMetric<3, Frame::Inertial, DataVector>, \
             evolution::dg::Actions::detail::NormalVector<3>>

#define INSTANTIATION(r, data)                                                \
  template void MonotisedCentralPrim::reconstruct(                            \
      gsl::not_null<std::array<Variables<TAGS_LIST(data)>, 3>*>               \
          vars_on_lower_face,                                                 \
      gsl::not_null<std::array<Variables<TAGS_LIST(data)>, 3>*>               \
          vars_on_upper_face,                                                 \
      const Variables<hydro::grmhd_tags<DataVector>>& volume_prims,           \
      const EquationsOfState::EquationOfState<true, THERMO_DIM(data)>& eos,   \
      const Element<3>& element,                                              \
      const FixedHashMap<                                                     \
          maximum_number_of_neighbors(3),                                     \
          std::pair<Direction<3>, ElementId<3>>, std::vector<double>,         \
          boost::hash<std::pair<Direction<3>, ElementId<3>>>>& neighbor_data, \
      const Mesh<3>& subcell_mesh) const;                                     \
  template void MonotisedCentralPrim::reconstruct_fd_neighbor(                \
      gsl::not_null<Variables<TAGS_LIST(data)>*> vars_on_face,                \
      const Variables<hydro::grmhd_tags<DataVector>>& subcell_volume_prims,   \
      const EquationsOfState::EquationOfState<true, THERMO_DIM(data)>& eos,   \
      const Element<3>& element,                                              \
      const FixedHashMap<                                                     \
          maximum_number_of_neighbors(3),                                     \
          std::pair<Direction<3>, ElementId<3>>, std::vector<double>,         \
          boost::hash<std::pair<Direction<3>, ElementId<3>>>>& neighbor_data, \
      const Mesh<3>& subcell_mesh,                                            \
      const Direction<3> direction_to_reconstruct) const;

GENERATE_INSTANTIATIONS(INSTANTIATION, (1, 2))

#undef INSTANTIATION
#undef TAGS_LIST
#undef THERMO_DIM
}  // namespace grmhd::ValenciaDivClean::fd


/*
   For more information, please see: http://software.sci.utah.edu

   The MIT License

   Copyright (c) 2009 Scientific Computing and Imaging Institute,
   University of Utah.

   Permission is hereby granted, free of charge, to any person obtaining a
   copy of this software and associated documentation files (the "Software"),
   to deal in the Software without restriction, including without limitation
   the rights to use, copy, modify, merge, publish, distribute, sublicense,
   and/or sell copies of the Software, and to permit persons to whom the
   Software is furnished to do so, subject to the following conditions:

   The above copyright notice and this permission notice shall be included
   in all copies or substantial portions of the Software.

   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
   OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
   THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
   FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
   DEALINGS IN THE SOFTWARE.
*/

///@brief Matrix operations -- concatenate, replace
///
///@author
///   David Weinstein &
///   Chris Butson
///   Department of Computer Science
///   University of Utah
///@date  July 2002

#include <Core/Datatypes/DenseMatrix.h>
#include <Core/Datatypes/SparseRowMatrix.h>
#include <Core/Datatypes/MatrixTypeConverter.h>
#include <Core/Algorithms/Math/CollectMatrices/CollectMatricesAlgorithm.h>
#include <Dataflow/GuiInterface/GuiVar.h>
#include <Dataflow/Network/Ports/MatrixPort.h>
#include <Dataflow/Network/Module.h>

#include <Core/Util/StringUtil.h>
#include <iostream>
#include <sstream>
#include <boost/scoped_ptr.hpp>

namespace SCIRun {

/// @class CollectMatrices
/// @detail This module appends/replaces rows or columns of a matrix while
/// looping through a network.

class CollectMatrices : public Module
{
public:
  explicit CollectMatrices(GuiContext* ctx);
  virtual void execute();
  virtual void tcl_command(GuiArgs&, void *);
private:
  MatrixHandle matrixH_;
  GuiInt append_;   // append or replace
  GuiInt row_;      // row or column
  GuiInt front_;    // append at the beginning or the end

  Algo::CollectMatricesAlgorithmBase* create_algo(MatrixHandle aH, MatrixHandle bH) const;
};

DECLARE_MAKER(CollectMatrices)
CollectMatrices::CollectMatrices(GuiContext* ctx)
: Module("CollectMatrices", ctx, Filter,"Math", "SCIRun"),
  append_(get_ctx()->subVar("append"), 0),
  row_(get_ctx()->subVar("row"), 0),
  front_(get_ctx()->subVar("front"), 0)
{
}

/// @todo: match output matrix type with input type.
void
CollectMatrices::execute()
{
  update_state(NeedData);

  MatrixHandle aH, bH;
  get_input_handle("Optional BaseMatrix", aH, false);

  if (!get_input_handle("SubMatrix", bH))
    return;

  bool append = append_.get();
  bool row = row_.get();
  bool front = front_.get();

  MatrixHandle omatrix;

  if (!append)               // Replace -- just send B matrix
  {
    matrixH_ = bH;
    send_output_handle("CompositeMatrix", matrixH_, true);
    return;
  }
  else if (!aH.get_rep())    // No A matrix
  {
    if (!matrixH_.get_rep())
    {
      // No previous CompositeMatrix, so send B
      matrixH_ = bH;
      send_output_handle("CompositeMatrix", matrixH_, true);
      return;
    }
    else
    {
      boost::scoped_ptr<Algo::CollectMatricesAlgorithmBase> algo(create_algo(matrixH_, bH));
      // Previous CompositeMatrix exists, concatenate with B
      if (row)
      {
        if (matrixH_->ncols() != bH->ncols())
        {
          warning("SubMatrix and CompositeMatrix must have same number of columns");
          return;
        }
        else
        {
          if (front)
            omatrix = algo->concat_rows(bH,matrixH_);
          else
            omatrix = algo->concat_rows(matrixH_,bH);
        }
      }
      else
      {
        if (matrixH_->nrows() != bH->nrows())
        {
          warning("SubMatrix and CompositeMatrix must have same number of rows");
          return;
        }
        else
        {
          if (front)
            omatrix = algo->concat_cols(bH,matrixH_);
          else
            omatrix = algo->concat_cols(matrixH_,bH);
        } // columns
      } // rows - columns
    } // previous matrix exists
  }
  else
  { // A exists
    boost::scoped_ptr<Algo::CollectMatricesAlgorithmBase> algo(create_algo(aH, bH));
    if (row)
    {
      if (aH->ncols() != bH->ncols())
      {
        warning("BaseMatrix and CompositeMatrix must have same number of columns");
        return;
      }
      else
      {
        if (front)
          omatrix = algo->concat_rows(bH,aH);
        else
          omatrix = algo->concat_rows(aH,bH);
      }
    }
    else
    {
      if (aH->nrows() != bH->nrows())
      {
        warning("BaseMatrix and CompositeMatrix must have same number of rows");
        return;
      }
      else
      {
        if (front)
          omatrix = algo->concat_cols(bH,aH);
        else
          omatrix = algo->concat_cols(aH,bH);
      } // columns
    } // rows - columns
  } // A exists

  if (omatrix.get_rep())
  {
    matrixH_ = omatrix;
    send_output_handle("CompositeMatrix", matrixH_, true);
  }
}

void
CollectMatrices::tcl_command(GuiArgs& args, void* userdata)
{
  if (args[1] == "clear")
  {
    matrixH_ = 0;
    remark("Collected matrix buffer has been cleared.");
  }
  else
  {
    Module::tcl_command(args, userdata);
  }
}

Algo::CollectMatricesAlgorithmBase*
CollectMatrices::create_algo(MatrixHandle aH, MatrixHandle bH) const
{
  if (matrix_is::sparse(aH) && matrix_is::sparse(bH))
    return new Algo::CollectSparseRowMatricesAlgorithm;
  else
    return new Algo::CollectDenseMatricesAlgorithm;
}

} // End namespace SCIRun


/*!
 * @file
 * Contains unit tests for `boost::mpl11::flip`.
 */

#include <boost/mpl11/functional.hpp>

#include <boost/mpl11/detail/std_is_same.hpp>

using namespace boost::mpl11;
using detail::std_is_same;

template <typename, typename>
struct binary { struct type; };

template <typename ...>
struct variadic { struct type; };

struct a0; struct a1; struct a2; struct a3; struct a4; struct a5;

static_assert(std_is_same<
    flip<lift<binary>>::type::apply<a0, a1>::type,
    binary<a1, a0>::type
>::value, "");

static_assert(std_is_same<
    flip<lift<variadic>>::type::apply<a0, a1, a2>::type,
    variadic<a1, a0, a2>::type
>::value, "");

static_assert(std_is_same<
    flip<lift<variadic>>::type::apply<a0, a1, a2, a3>::type,
    variadic<a1, a0, a2, a3>::type
>::value, "");

static_assert(std_is_same<
    flip<lift<variadic>>::type::apply<a0, a1, a2, a3, a4>::type,
    variadic<a1, a0, a2, a3, a4>::type
>::value, "");

static_assert(std_is_same<
    flip<lift<variadic>>::type::apply<a0, a1, a2, a3, a4, a5>::type,
    variadic<a1, a0, a2, a3, a4, a5>::type
>::value, "");

int main() { }


#define BOOST_CHRONO_VERSION 2
#include <boost/chrono.hpp>
#include <boost/chrono/chrono_io.hpp>
#include <iostream>

using namespace boost::chrono;

int main()
{
  std::cout << symbol_format << minutes{10} << '\n';

  std::cout << time_fmt(boost::chrono::timezone::local, "%H:%M:%S") <<
    system_clock::now() << '\n';
}


/***************************************************************************
 * Copyright 1998-2015 by authors (see AUTHORS.txt)                        *
 *                                                                         *
 *   This file is part of LuxRender.                                       *
 *                                                                         *
 * Licensed under the Apache License, Version 2.0 (the "License");         *
 * you may not use this file except in compliance with the License.        *
 * You may obtain a copy of the License at                                 *
 *                                                                         *
 *     http://www.apache.org/licenses/LICENSE-2.0                          *
 *                                                                         *
 * Unless required by applicable law or agreed to in writing, software     *
 * distributed under the License is distributed on an "AS IS" BASIS,       *
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.*
 * See the License for the specific language governing permissions and     *
 * limitations under the License.                                          *
 ***************************************************************************/

#include <boost/lexical_cast.hpp>

#include "luxrays/core/color/color.h"
#include "slg/samplers/sampler.h"
#include "slg/samplers/sobol.h"

using namespace luxrays;
using namespace slg;

//------------------------------------------------------------------------------
// Sobol sampler
//
// This sampler is based on Blender Cycles Sobol implementation.
//------------------------------------------------------------------------------

void SobolSampler::RequestSamples(const u_int size) {
	directions = new u_int[size * SOBOL_BITS];
	SobolGenerateDirectionVectors(directions, size);
}

u_int SobolSampler::SobolDimension(const u_int index, const u_int dimension) const {
	const u_int offset = dimension * SOBOL_BITS;
	u_int result = 0;
	u_int i = index;

	for (u_int j = 0; i; i >>= 1, j++) {
		if (i & 1)
			result ^= directions[offset + j];
	}

	return result;
}

float SobolSampler::GetSample(const u_int index) {
	const u_int iResult = SobolDimension(pass, index);
	const float fResult = iResult * (1.f / 0xffffffffu);

	// Cranley-Patterson rotation to reduce visible regular patterns
	const float shift = (index & 1) ? rng0 : rng1;
	const float val = fResult + shift;

	return val - floorf(val);
}

void SobolSampler::NextSample(const std::vector<SampleResult> &sampleResults) {
	film->AddSampleCount(1.0);
	AddSamplesToFilm(sampleResults);

	++pass;
}


#ifndef ASLAM_BACKEND_LEVENBERG_MARQUARDT_TRUST_REGION_POLICY_HPP
#define ASLAM_BACKEND_LEVENBERG_MARQUARDT_TRUST_REGION_POLICY_HPP

#include <aslam/backend/TrustRegionPolicy.hpp>
#include <aslam/backend/LinearSystemSolver.hpp>
#include <boost/shared_ptr.hpp>

namespace sm {
class ConstPropertyTree;
} // namespace sm

namespace aslam {
    namespace backend {

        class LevenbergMarquardtTrustRegionPolicy : public TrustRegionPolicy
        {
        public:
          LevenbergMarquardtTrustRegionPolicy();
          LevenbergMarquardtTrustRegionPolicy(const sm::ConstPropertyTree & config);
          LevenbergMarquardtTrustRegionPolicy(double lambdaInit);
          ~LevenbergMarquardtTrustRegionPolicy() override;

          /// \brief called by the optimizer when an optimization is starting
          void optimizationStartingImplementation(double J) override;

          // Returns true if the solution was successful
          bool solveSystemImplementation(double J, bool previousIterationFailed, int nThreads, Eigen::VectorXd& outDx) override;

          /// \brief should the optimizer revert on failure? You should probably return true
          bool revertOnFailure() override;

          /// \brief print the current state to a stream (no newlines).
          std::ostream & printState(std::ostream & out) const override;
          bool requiresAugmentedDiagonal() const override;
          std::string name() const override { return "levenberg_marquardt"; }
        private:
          double getLmRho(const Eigen::VectorXd & dx);
          double _lambdaInit;
          double _gammaInit;
          double _betaInit;
          int _pInit;
          double _muInit;

          double _lambda;
          double _gamma;
          double _beta;
          int _p;
          double _mu;
        };

    } // namespace backend
} // namespace aslam

#endif /* ASLAM_BACKEND_LEVENBERG_MARQUARDT_TRUST_REGION_POLICY_HPP */


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */
#pragma once

#include <Eigen/Dense>
#include <boost/shared_ptr.hpp>
#include <fstream>

#include <dpMM/global.hpp>
#include <dpMM/clGMMData.hpp>

using namespace Eigen;
using boost::shared_ptr;

template<class T>
class DpMM
{
public:
  DpMM()
  {};
  virtual ~DpMM()
  {};

  virtual void initialize(const shared_ptr<Matrix<T,Dynamic,Dynamic> >& spx)
    {assert(false);};
  virtual void initialize(const Matrix<T,Dynamic,Dynamic>& x)
  {spx_=shared_ptr<Matrix<T,Dynamic,Dynamic> >(new Matrix<T,Dynamic,Dynamic>(x));
  initialize(spx_);};
  virtual void initialize(const shared_ptr<ClGMMData<T> >& cld)
    {assert(false);};
  virtual void sampleLabels() = 0;
  virtual void sampleParameters() = 0;
  virtual void proposeSplits() {};
  virtual void proposeMerges() {};
  // call this right before sampleParameters()
  virtual void resampleFromBase(uint32_t Kmax) {};
  virtual const VectorXu & getLabels() = 0;
  virtual Matrix<T,Dynamic,1> getCounts() = 0;
  virtual uint32_t getK() const = 0;
  virtual double logJoint() { return 0.0;};

  virtual double evalLogLikelihood(const Matrix<T,Dynamic,1>& x){
    return 0;}

//  virtual MatrixXu mostLikelyInds(uint32_t n)
//{ return MatrixXu::Zero(n,1);};
  virtual MatrixXu mostLikelyInds(uint32_t n, Matrix<T,Dynamic,Dynamic>& logLikes)
  { return MatrixXu::Zero(n,1);};

  virtual void dump(std::ofstream& fOutMeans, std::ofstream& fOutCovs)
  {};

  shared_ptr<Matrix<T,Dynamic,Dynamic> > spx_;
private:
};


#include "BoundingBox.h"

#include <Eigen/Dense>
#include <memory>

BoundingBox::BoundingBox()
{
    mMin.setZero();
    mMax.setZero();
    mMid.setZero();
    mSize.setZero();
}


#include <stdlib.h>
#include <netcdf.h>
#include <vector>
#include <map>
#include <queue>
#include <fstream>
#include <Eigen/Sparse>
#include "def.h"
#include "graph.h"
#include "coarsening.h"

using namespace std;
using namespace Eigen;

size_t nCells, nEdges, nVertices, nVertLevels, maxEdges, vertexDegree, Time;
vector<double> latVertex, lonVertex, xVertex, yVertex, zVertex;
vector<Cell> cells;
vector<double> latCell, lonCell, bottomDepth;
vector<int> indexToVertexID, indexToCellID, indexToEdgeID;
vector<int> verticesOnEdge, cellsOnEdge,
cellsOnVertex, edgesOnVertex,
cellsOnCell, verticesOnCell, nEdgesOnCell, maxLevelCell;
vector<double> thickness, maxThickness;

map<int, int> vertexIndex, cellIndex;

const int minNumEdges = 1;
const int maxNumEdges = 7;
const double PI = 3.14159265358979323846;

void loadMeshFromNetCDF(const string& filename) {
	int ncid;
	int dimid_cells, dimid_edges, dimid_vertices, dimid_vertLevels, dimid_maxEdges,
		dimid_vertexDegree, dimid_Time;
	int varid_latVertex, varid_lonVertex, varid_xVertex, varid_yVertex, varid_zVertex,
		varid_latCell, varid_lonCell, varid_xCell, varid_yCell, varid_zCell, varid_bottomDepth,
		varid_edgesOnVertex, varid_cellsOnVertex,
		varid_indexToVertexID, varid_indexToCellID, varid_indexToEdgeID,
		varid_nEdgesOnCell, varid_cellsOnCell, varid_verticesOnCell, varid_maxLevelCell,
		varid_verticesOnEdge, varid_cellsOnEdge, varid_thickness;

	NC_SAFE_CALL(nc_open(filename.c_str(), NC_NOWRITE, &ncid));

	NC_SAFE_CALL(nc_inq_dimid(ncid, "nCells", &dimid_cells));
	NC_SAFE_CALL(nc_inq_dimid(ncid, "nEdges", &dimid_edges));
	NC_SAFE_CALL(nc_inq_dimid(ncid, "nVertices", &dimid_vertices));
	NC_SAFE_CALL(nc_inq_dimid(ncid, "nVertLevels", &dimid_vertLevels));
	NC_SAFE_CALL(nc_inq_dimid(ncid, "maxEdges", &dimid_maxEdges));
	NC_SAFE_CALL(nc_inq_dimid(ncid, "vertexDegree", &dimid_vertexDegree));
	NC_SAFE_CALL(nc_inq_dimid(ncid, "Time", &dimid_Time));

	NC_SAFE_CALL(nc_inq_dimlen(ncid, dimid_cells, &nCells));
	NC_SAFE_CALL(nc_inq_dimlen(ncid, dimid_edges, &nEdges));
	NC_SAFE_CALL(nc_inq_dimlen(ncid, dimid_vertices, &nVertices));
	NC_SAFE_CALL(nc_inq_dimlen(ncid, dimid_vertLevels, &nVertLevels));
	NC_SAFE_CALL(nc_inq_dimlen(ncid, dimid_maxEdges, &maxEdges));
	NC_SAFE_CALL(nc_inq_dimlen(ncid, dimid_vertexDegree, &vertexDegree));
	NC_SAFE_CALL(nc_inq_dimlen(ncid, dimid_Time, &Time));

	NC_SAFE_CALL(nc_inq_varid(ncid, "indexToVertexID", &varid_indexToVertexID));
	NC_SAFE_CALL(nc_inq_varid(ncid, "indexToCellID", &varid_indexToCellID));
	NC_SAFE_CALL(nc_inq_varid(ncid, "indexToEdgeID", &varid_indexToEdgeID));
	NC_SAFE_CALL(nc_inq_varid(ncid, "latCell", &varid_latCell));
	NC_SAFE_CALL(nc_inq_varid(ncid, "lonCell", &varid_lonCell));
	NC_SAFE_CALL(nc_inq_varid(ncid, "xCell", &varid_xCell));
	NC_SAFE_CALL(nc_inq_varid(ncid, "yCell", &varid_yCell));
	NC_SAFE_CALL(nc_inq_varid(ncid, "zCell", &varid_zCell));
	NC_SAFE_CALL(nc_inq_varid(ncid, "bottomDepth", &varid_bottomDepth));
	NC_SAFE_CALL(nc_inq_varid(ncid, "nEdgesOnCell", &varid_nEdgesOnCell));
	NC_SAFE_CALL(nc_inq_varid(ncid, "maxLevelCell", &varid_maxLevelCell));
	NC_SAFE_CALL(nc_inq_varid(ncid, "latVertex", &varid_latVertex));
	NC_SAFE_CALL(nc_inq_varid(ncid, "lonVertex", &varid_lonVertex));
	NC_SAFE_CALL(nc_inq_varid(ncid, "xVertex", &varid_xVertex));
	NC_SAFE_CALL(nc_inq_varid(ncid, "yVertex", &varid_yVertex));
	NC_SAFE_CALL(nc_inq_varid(ncid, "zVertex", &varid_zVertex));
	NC_SAFE_CALL(nc_inq_varid(ncid, "edgesOnVertex", &varid_edgesOnVertex));
	NC_SAFE_CALL(nc_inq_varid(ncid, "cellsOnVertex", &varid_cellsOnVertex));
	NC_SAFE_CALL(nc_inq_varid(ncid, "cellsOnCell", &varid_cellsOnCell));
	NC_SAFE_CALL(nc_inq_varid(ncid, "verticesOnCell", &varid_verticesOnCell));
	NC_SAFE_CALL(nc_inq_varid(ncid, "verticesOnEdge", &varid_verticesOnEdge));
	NC_SAFE_CALL(nc_inq_varid(ncid, "cellsOnEdge", &varid_cellsOnEdge));
	NC_SAFE_CALL(nc_inq_varid(ncid, "restingThickness", &varid_thickness));

	const size_t start_cells[1] = { 0 }, size_cells[1] = { nCells };

	latCell.resize(nCells);
	lonCell.resize(nCells);
	indexToCellID.resize(nCells);
	nEdgesOnCell.resize(nCells);
	maxLevelCell.resize(nCells);
	bottomDepth.resize(nCells);

	NC_SAFE_CALL(nc_get_vara_double(ncid, varid_latCell, start_cells, size_cells, &latCell[0]));
	NC_SAFE_CALL(nc_get_vara_double(ncid, varid_lonCell, start_cells, size_cells, &lonCell[0]));
	NC_SAFE_CALL(nc_get_vara_int(ncid, varid_indexToCellID, start_cells, size_cells, &indexToCellID[0]));
	for (int i = 0; i < nCells; i++) {
		cellIndex[indexToCellID[i]] = i;
		//fprintf(stderr, "%d, %d\n", i, indexToCellID[i]);
	}
	NC_SAFE_CALL(nc_get_vara_int(ncid, varid_nEdgesOnCell, start_cells, size_cells, &nEdgesOnCell[0]));
	NC_SAFE_CALL(nc_get_vara_int(ncid, varid_maxLevelCell, start_cells, size_cells, &maxLevelCell[0]));
	NC_SAFE_CALL(nc_get_vara_double(ncid, varid_bottomDepth, start_cells, size_cells, &bottomDepth[0]));
	/*freopen("depth.csv", "w", stdout);
	for (int i = 0; i < nCells; i++) {
		printf("%.2lf,%d,%.2lf\n", bottomDepth[i], maxLevelCell[i], bottomDepth[i] / maxLevelCell[i]);
	}
	fclose(stdout);*/

	const size_t start_vertices[1] = { 0 }, size_vertices[1] = { nVertices };
	latVertex.resize(nVertices);
	lonVertex.resize(nVertices);
	xVertex.resize(nVertices);
	yVertex.resize(nVertices);
	zVertex.resize(nVertices);
	indexToVertexID.resize(nVertices);

	NC_SAFE_CALL(nc_get_vara_int(ncid, varid_indexToVertexID, start_vertices, size_vertices, &indexToVertexID[0]));
	NC_SAFE_CALL(nc_get_vara_double(ncid, varid_latVertex, start_vertices, size_vertices, &latVertex[0]));
	NC_SAFE_CALL(nc_get_vara_double(ncid, varid_lonVertex, start_vertices, size_vertices, &lonVertex[0]));
	NC_SAFE_CALL(nc_get_vara_double(ncid, varid_xVertex, start_vertices, size_vertices, &xVertex[0]));
	NC_SAFE_CALL(nc_get_vara_double(ncid, varid_yVertex, start_vertices, size_vertices, &yVertex[0]));
	NC_SAFE_CALL(nc_get_vara_double(ncid, varid_zVertex, start_vertices, size_vertices, &zVertex[0]));

	//for (int i = 0; i < nVertices; i++) {
	//	double x = max_rho * cos(latVertex[i]) * cos(lonVertex[i]);
	//	double y = max_rho * cos(latVertex[i]) * sin(lonVertex[i]);
	//	double z = max_rho * sin(latVertex[i]);
	//	assert(abs(x - xVertex[i]) < eps && abs(y - yVertex[i]) < eps && abs(z - zVertex[i]) < eps);
	//}

	for (int i = 0; i < nVertices; i++) {
		vertexIndex[indexToVertexID[i]] = i;
		// fprintf(stderr, "%d, %d\n", i, indexToVertexID[i]);
	}

	const size_t start_edges[1] = { 0 }, size_edges[1] = { nEdges };
	indexToEdgeID.resize(nEdges);

	NC_SAFE_CALL(nc_get_vara_int(ncid, varid_indexToEdgeID, start_edges, size_edges, &indexToEdgeID[0]));

	const size_t start_edges2[2] = { 0, 0 }, size_edges2[2] = { nEdges, 2 };
	verticesOnEdge.resize(nEdges * 2);
	cellsOnEdge.resize(nEdges * 2);

	NC_SAFE_CALL(nc_get_vara_int(ncid, varid_verticesOnEdge, start_edges2, size_edges2, &verticesOnEdge[0]));
	NC_SAFE_CALL(nc_get_vara_int(ncid, varid_cellsOnEdge, start_edges2, size_edges2, &cellsOnEdge[0]));

	//for (int i=0; i<nEdges; i++)
	//   fprintf(stderr, "%d, %d\n", verticesOnEdge[i*2], verticesOnEdge[i*2+1]);

	const size_t start_vertex_cell[2] = { 0, 0 }, size_vertex_cell[2] = { nVertices, 3 };
	cellsOnVertex.resize(nVertices * 3);
	edgesOnVertex.resize(nVertices * 3);

	NC_SAFE_CALL(nc_get_vara_int(ncid, varid_cellsOnVertex, start_vertex_cell, size_vertex_cell, &cellsOnVertex[0]));
	NC_SAFE_CALL(nc_get_vara_int(ncid, varid_edgesOnVertex, start_vertex_cell, size_vertex_cell, &edgesOnVertex[0]));

	const size_t start_cell_vertex[2] = { 0, 0 }, size_cell_vertex[2] = { nCells, maxEdges };
	verticesOnCell.resize(nCells * maxEdges);
	cellsOnCell.resize(nCells * maxEdges);

	NC_SAFE_CALL(nc_get_vara_int(ncid, varid_verticesOnCell, start_cell_vertex, size_cell_vertex, &verticesOnCell[0]));
	NC_SAFE_CALL(nc_get_vara_int(ncid, varid_cellsOnCell, start_cell_vertex, size_cell_vertex, &cellsOnCell[0]));

	const size_t start_cell_vertLevel[2] = { 0, 0 }, size_cell_vertLevel[2] = { nCells, nVertLevels };
	thickness.resize(nCells * nVertLevels);

	NC_SAFE_CALL(nc_get_vara_double(ncid, varid_thickness, start_cell_vertLevel, size_cell_vertLevel, &thickness[0]));
	maxThickness.reserve(nVertLevels);
	for (int j = 0; j < nVertLevels; j++) {
		float maxThick = 0;
		for (int i = 0; i < nCells; i++) {
			if (thickness[i * nVertLevels + j] > maxThick)
				maxThick = thickness[i * nVertLevels + j];
		}
		maxThickness.push_back(maxThick);
	}

	NC_SAFE_CALL(nc_close(ncid));

	fprintf(stderr, "%zu, %zu, %zu, %zu\n", nCells, nEdges, nVertices, nVertLevels);
}

int main(int argc, char **argv) {
	loadMeshFromNetCDF(argv[1]); // such as /fs/project/PAS0027/MPAS/Results/0031_1.00949.nc

	vector<int> indicesByBFS;	// stores the original index by edge order
	indicesByBFS.reserve(nCells);
	map<int, int> BFSOrdFromIndices;

	vector<bool> visited(nCells, false);
	queue<int> q;
	// first add all boundary cells
	for (int i = 0; i < nCells; i++) {
		int idx1 = i;
		for (int j = 0; j < nEdgesOnCell[idx1]; j++) {
			if (!cellsOnCell[idx1 * maxEdges + j]) {
				q.push(i);
				visited[i] = true;
				indicesByBFS.push_back(i);
				break;
			}
		}
	}

	while (!q.empty()) {
		int idx1 = q.front();
		q.pop();
		for (int j = 0; j < nEdgesOnCell[idx1]; j++) {
			if (cellsOnCell[idx1 * maxEdges + j]) {
				int idx2 = cellIndex[cellsOnCell[idx1 * maxEdges + j]];	// original index
				if (!visited[idx2]) {
					q.push(idx2);
					visited[idx2] = true;
					indicesByBFS.push_back(idx2);
				}
			}
		}
	}
	assert(indicesByBFS.size() == nCells);

	for (int i = 0; i < nCells; i++) {
		int idx = indicesByBFS[i];
		BFSOrdFromIndices[idx] = i;
	}

// 	nVertLevels = 1;
	cells.reserve(nCells * nVertLevels);
	for (int i = 0; i < nCells; i++) {
		int idx = indicesByBFS[i];	// original index
		float x = cos(latCell[idx]) * cos(lonCell[idx]);
		float y = cos(latCell[idx]) * sin(lonCell[idx]);
		float z = sin(latCell[idx]);
		float depth = 0.0;
		for (int j = 0; j < nVertLevels; j++) {
			Cell cell = Cell(x, y, z, depth, true);
			if (j >= maxLevelCell[idx])
				cell.real = false;
			assert(abs(cell.lat - latCell[idx]) < eps && abs(cell.lon - lonCell[idx]) < eps);
			depth += maxThickness[j];
			cells.push_back(cell);
		}
	}

	Graph oriGraph = Graph(cells, nVertLevels);
	for (int i =0; i < oriGraph.edgeNumAttr; i++)
		oriGraph.A[i].reserve(VectorXi::Constant(nCells * nVertLevels, maxEdges + 2));

	oriGraph.avgSqrDistH = 0.0;
	int totalEdgesH = 0;
	// add horizontal edges
	for (int i = 0; i < nCells; i++) {
		int idx1 = indicesByBFS[i];	// original index
		assert(BFSOrdFromIndices[idx1] == i);
		int mtIdx1 = i * nVertLevels;
		for (int k = 0; k < nEdgesOnCell[idx1]; k++) {
			if (cellsOnCell[idx1 * maxEdges + k]) {
				int idx2 = cellIndex[cellsOnCell[idx1 * maxEdges + k]];	// original index
				if (visited[idx2]) {
					int mtIdx2 = BFSOrdFromIndices[idx2] * nVertLevels;
					float sqrDistH = (cells[mtIdx1].x - cells[mtIdx2].x) * (cells[mtIdx1].x - cells[mtIdx2].x) +
						(cells[mtIdx1].y - cells[mtIdx2].y) * (cells[mtIdx1].y - cells[mtIdx2].y) +
						(cells[mtIdx1].z - cells[mtIdx2].z) * (cells[mtIdx1].z - cells[mtIdx2].z);
					oriGraph.avgSqrDistH += sqrDistH;
					totalEdgesH++;
					for (int j = 0; j < nVertLevels; j++) {
						if (cells[mtIdx1 + j].real && cells[mtIdx2 + j].real){
							Edge edge = Edge(cells[mtIdx1], cells[mtIdx2], sqrDistH);
							oriGraph.A[0].insert(mtIdx1 + j, mtIdx2 + j) = edge.deltaHighLat;
							oriGraph.A[1].insert(mtIdx1 + j, mtIdx2 + j) = edge.deltaLowLat;
							oriGraph.A[2].insert(mtIdx1 + j, mtIdx2 + j) = edge.deltaWest;
							oriGraph.A[3].insert(mtIdx1 + j, mtIdx2 + j) = edge.deltaEast;
							oriGraph.A[4].insert(mtIdx1 + j, mtIdx2 + j) = edge.isHorizontal;
							oriGraph.A[5].insert(mtIdx1 + j, mtIdx2 + j) = edge.isUp;
							oriGraph.A[6].insert(mtIdx1 + j, mtIdx2 + j) = edge.isDown;
							oriGraph.A[7].insert(mtIdx1 + j, mtIdx2 + j) = edge.weight;
						}
					}
				}
			}
		}
	}
	oriGraph.avgSqrDistH /= totalEdgesH;

	for (int k = 0; k < oriGraph.A[7].outerSize(); k++) {
		for (SparseMatrix<float, RowMajor>::InnerIterator it(oriGraph.A[7], k); it; ++it) {
			int row = it.row();
			int col = it.col();
			int depthLayer1 = row % nVertLevels;
			int depthLayer2 = col % nVertLevels;
			float weight;
			if (depthLayer1 == depthLayer2) {
				float value = it.value();
				weight = exp(-value / (4.0 * oriGraph.avgSqrDistH));
			}
			it.valueRef() = weight;
		}
	}

	// add vertical edges
	oriGraph.avgSqrDistV = 0.0;
	vector<float> sqrDistV(nVertLevels);
	for (int j = 0; j < nVertLevels; j++) {
		oriGraph.avgSqrDistV += maxThickness[j] * maxThickness[j];
	}
	oriGraph.avgSqrDistV /= nVertLevels;

	for (int j = 0; j < nVertLevels - 1; j++) {
		float weight = exp(-maxThickness[j] * maxThickness[j] / (4.0 * oriGraph.avgSqrDistV));
		for (int i = 0; i < nCells; i++) {
			int mtIdx1 = i * nVertLevels + j;
			int mtIdx2 = i * nVertLevels + j + 1;
			if (cells[mtIdx1].real && cells[mtIdx2].real) {
				Edge edge1 = Edge(cells[mtIdx1], cells[mtIdx2], weight);
				oriGraph.A[0].insert(mtIdx1, mtIdx2) = edge1.deltaHighLat;
				oriGraph.A[1].insert(mtIdx1, mtIdx2) = edge1.deltaLowLat;
				oriGraph.A[2].insert(mtIdx1, mtIdx2) = edge1.deltaWest;
				oriGraph.A[3].insert(mtIdx1, mtIdx2) = edge1.deltaEast;
				oriGraph.A[4].insert(mtIdx1, mtIdx2) = edge1.isHorizontal;
				oriGraph.A[5].insert(mtIdx1, mtIdx2) = edge1.isUp;
				oriGraph.A[6].insert(mtIdx1, mtIdx2) = edge1.isDown;
				oriGraph.A[7].insert(mtIdx1, mtIdx2) = edge1.weight;

				Edge edge2 = Edge(cells[mtIdx2], cells[mtIdx1], weight);
				oriGraph.A[0].insert(mtIdx2, mtIdx1) = edge2.deltaHighLat;
				oriGraph.A[1].insert(mtIdx2, mtIdx1) = edge2.deltaLowLat;
				oriGraph.A[2].insert(mtIdx2, mtIdx1) = edge2.deltaWest;
				oriGraph.A[3].insert(mtIdx2, mtIdx1) = edge2.deltaEast;
				oriGraph.A[4].insert(mtIdx2, mtIdx1) = edge2.isHorizontal;
				oriGraph.A[5].insert(mtIdx2, mtIdx1) = edge2.isUp;
				oriGraph.A[6].insert(mtIdx2, mtIdx1) = edge2.isDown;
				oriGraph.A[7].insert(mtIdx2, mtIdx1) = edge2.weight;
			}
		}
	}
	for (int i = 0; i < oriGraph.edgeNumAttr; i++)
		assert(oriGraph.A[i].outerSize() == nCells * nVertLevels);

	//oriGraph.Serialize(oriGraph.A, "oriGraphAdjacency");
	//oriGraph.Deserialize(oriGraph.A, "oriGraphAdjacency");

	Coarsening coarsening = Coarsening();
	coarsening.coarsen(oriGraph, 9, 2);
	//coarsening.permTest();
	//cout << coarsening.graphs[6].A << endl;
	//cout << coarsening.graphs[6].upAsgn << endl;
	//cout << coarsening.graphs[5].avgPoolAsgn << endl;

	vector<int> perm(coarsening.numReals[0]);
	for (int i = 0; i < coarsening.numReals[0]; i++) {
		int ordInCells = coarsening.perms[0][i];
		int ordInBFS = ordInCells / nVertLevels;
		int ordRawH = indicesByBFS[ordInBFS];
		int ordRawV = ordInCells % nVertLevels;
		perm[i] = ordRawH * nVertLevels + ordRawV;
	}
	cnpy::npy_save("../res/perm.npy", &perm[0], { (size_t)(perm.size()) }, "w");
	fstream writeFile;
	writeFile.open("../res/perm.dat", ios::binary | ios::out);
	writeFile.write((const char *)&perm[0], sizeof(int) * perm.size());

	cnpy::npy_save("../res/graphSizes.npy", &coarsening.numReals[0], { (size_t)(coarsening.numReals.size()) }, "w");
}


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

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

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_SPLATTED_MINIMUM_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SPLATTED_MINIMUM_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-swar
    Function object implementing splatted_minimum capabilities

    Splatted minimum

    Computes the splatted minimum of the element of its argument.

    @par Semantic

    Depending on the type of its arguments, splatted_maximum exhibits different
    semantics. For any type @c T and value @c x of type @c T:

    @code
    T r = splatted_maximum(x);
    @endcode

    is similar to:

    @code
    for(int i=0;i<T::static_size;++i) x[i] = minimum(x);
    @endcode

  **/
  const boost::dispatch::functor<tag::splatted_minimum_> splatted_minimum = {};
} }
#endif

#include <boost/simd/function/scalar/splatted_minimum.hpp>
#include <boost/simd/function/simd/splatted_minimum.hpp>

#endif