 /**
  * The MIT License (MIT)
  *
  * Copyright (c) 2014 Behrouz Babaki, Tias Guns, Siegfried Nijssen
  *
  * 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.
  */

#include "pricer_optclust.h"
#include "SiegBnBCons.h"
#include "SiegBnB.h"
#include <iostream>
#include <vector>
#include <cmath>
#include <algorithm>
#include <utility>
#include <boost/date_time/posix_time/posix_time.hpp>

#include "scip/cons_linear.h"
#include "scip/pub_var.h"

using namespace std;
using namespace scip;
bool map_compare (SCIP* scip, const std::map<const char*, SCIP_Real>& lhs, const std::map<const char*, SCIP_Real>& rhs);

// print a list in ranges, is helper function
void print_subrange(int min, int max, ostream& out) {
    if (min == max)
        out << min << " ";
    else if (min+1 == max)
        out << min << " " << max << " ";
    else
        out << min << ".." << max << " ";
}
void print_range(const vector<int>& vals, ostream& out) {
    if (vals.size() == 0)
        return;
    int min = vals[0];
    int max = min;
    for (unsigned i=1; i!=vals.size(); i++) {
        if (max+1 == vals[i])
            max++;
        else {
            print_subrange(min, max, out);
            min = vals[i];
            max = vals[i];
        }
    }
    print_subrange(min, max, out);
}
void print_cluster(std::vector< bool > cluster, const char* msg) {
     cout << msg;
     vector<int> vals;
     for (unsigned i=0; i!=cluster.size(); i++) {
       if (cluster[i])
         vals.push_back(i);
     }
     print_range(vals, cout);
     cout << endl;
}

/** Constructs the pricer object with the data needed */
ObjPricerOptClust::ObjPricerOptClust(
   SCIP*                                scip,          /**< SCIP pointer */
   const char*                          p_name,        /**< name of pricer */
   const int                            p_num_instances,   /**< number of instances */
   const int                            p_num_clusters, /**< number of clusters in clustering problem*/
   const vector< vector<double> >&       p_distance,    /**< matrix of distances */
   const vector<SCIP_CONS*>&            p_setcover_con,     /**< matrix of arc constraints */
   SCIP_CONS*                           p_side_con,     /**< array of partitioning constraints */
   Stabilisation                        stab,           /**type of stabilisation to apply */
   bool                                 findAll,
   const char*                          coefFileName,
   const char*                          columnFileName
   ):
   ObjPricer(scip, p_name, "Finds the cluster with the smallest reduced cost", 0, TRUE),
   _num_instances(p_num_instances),
   _num_clusters(p_num_clusters),
   _distance(p_distance),
   _setcover_con(p_setcover_con),
   _side_con(p_side_con),
   _coefFileName(coefFileName),
   _columnFileName(columnFileName),
   _stab(stab),
   _findAll (findAll),
   _mu(0),
   _epsilon(0),
   _updateCount (0),
   _prev_colVals(),
   _prev_out_score(-SCIPinfinity(scip)),
   _piHat_score(-SCIPinfinity(scip)),
   _piHat_redcost(-SCIPinfinity(scip)),
   _piHat_lambdas(),
   _piHat_sigma(0),
   _count_mispricings(1) // should be always 1, is used as divisor
{
  this -> _lower_bound = -SCIPinfinity (scip);
  this -> _upper_bound = SCIPinfinity (scip);
  this -> _duality_gap = SCIPinfinity (scip);
  this -> _bestLambSum = SCIPinfinity (scip);

  if (_coefFileName != NULL)
    _outStream.open(_coefFileName , ios::out);

  if (_columnFileName != NULL)
    _colStream.open(_columnFileName, ios::out);

  if ((this->_stab == ST_Smooth || this->_stab == ST_Smooth2)) {
      if (SCIPisZero(scip, _epsilon))
          _epsilon = 0.4; // default alpha value
  } else if (this->_stab == ST_SmoothAuto) {
      _epsilon = 0.8; // auto changed
  }

}

/** Destructs the pricer object. */
ObjPricerOptClust::~ObjPricerOptClust()
{
   _outStream.close();
   _colStream.close();
}

/** initialization method of variable pricer (called after problem was transformed)
 *
 *  Because SCIP transformes the original problem in preprocessing, we need to get the references to
 *  the constraints in the transformed problem from the references in the original
 *  problem.
 */
SCIP_DECL_PRICERINIT(ObjPricerOptClust::scip_init)
{
   for (int i = 0; i < num_instances(); ++i)
         SCIP_CALL( SCIPgetTransformedCons(scip, _setcover_con[i], &_setcover_con[i]) );

   SCIP_CALL( SCIPgetTransformedCons(scip, _side_con, &_side_con) );

   return SCIP_OKAY;
}

SCIP_RETCODE ObjPricerOptClust::farkasPricing(
   SCIP*                 scip               /**< SCIP data structure */
   )
{
  /* Are we going to check the degeneracy checking also in Farkas pricing? */

  /* allocate array for reduced costs */
  vector<SCIP_Real>  lambdas(num_instances() , 0.0);
  SCIP_Real sigma;

  cout << "FARKAS" << endl;
  for (int i = 0; i < num_instances(); ++i)
    {
      assert( setcover_con(i) != 0 );
      lambdas[i] = SCIPgetDualfarkasLinear(scip, setcover_con(i));
    }
  sigma = SCIPgetDualfarkasLinear(scip, side_con());

  /* find the cluster with the minimum reduced cost*/
  (void) find_add_farkas_cluster(scip, lambdas, sigma);

  return SCIP_OKAY;
}

/** perform pricing*/

SCIP_RETCODE ObjPricerOptClust::pricing(
   SCIP*                 scip               /**< SCIP data structure */
   )
{
   int verbose = 0;

   if (verbose >= 1) {
      int nVars = SCIPgetNVars(scip);
      cout << "Best Sol so far ("<<nVars<<" vars):\n";

      if (verbose >= 6) {
         SCIP_VAR ** vars = SCIPgetVars(scip);
         for (int i=0; i!=nVars; i++)
	   SCIPprintVar(scip, vars[i], NULL);

         // print best real sol
         SCIP_CALL( SCIPprintBestSol(scip, NULL, FALSE) );
      }
   }

   // save values of columns
   map<const char*, SCIP_Real> colVals;
   bool was_degenerate = true;
   {
         int nCols;
         SCIP_COL** cols;
         SCIP_CALL( SCIPgetLPColsData(scip, &cols, &nCols) );
         for (int i=0; i!=nCols; i++) {
             SCIP_VAR* colVar = SCIPcolGetVar(cols[i]);
             SCIP_Real val = SCIPcolGetPrimsol(cols[i]);
             if (SCIPvarIsBinary(colVar) and not SCIPisZero(scip, val)) {
                // this ignores the continuous (perturbation) columns
                colVals[SCIPvarGetName(colVar)] = val;
             }
         }

         if (!map_compare(scip, _prev_colVals, colVals)) {
             _prev_colVals = colVals;
             was_degenerate = false;
         }
   }

   if (verbose >= 4) {
        if (was_degenerate)
            cout << "YES degen; ";
        else
            cout << "not degen; ";
        cout << "non-zero columns ("<<colVals.size()<<") -- ";
        if (verbose >= 6) {
            for (map<const char*, SCIP_Real>::iterator ii=colVals.begin(); ii!=colVals.end(); ++ii) {
                cout << (*ii).first << "(" << (*ii).second << ")\t";
            }
        }
        cout << endl;
   }

   /* allocate array for reduced costs */
   vector<SCIP_Real>  lambdas(num_instances() , 0.0);
   SCIP_Real sigma;

   /* store the reduced costs for setcover constraints */
   for (int i = 0; i < num_instances(); ++i)
   {
       assert( setcover_con(i) != 0 );
       lambdas[i] = SCIPgetDualsolLinear(scip, setcover_con(i));
       if (SCIPisZero(scip, lambdas[i]))
           lambdas[i] = 0;
   }
   sigma = SCIPgetDualsolLinear(scip, side_con());
   if (SCIPisZero(scip, sigma))
       sigma = 0;

   /* When smoothing, try to cut the speration point */
   // out point (in point is _piHat_*
   SCIP_Real out_score = getCurSol(scip);
   vector<SCIP_Real> out_lambdas = lambdas;
   SCIP_Real out_sigma = sigma;
   // sep point
   SCIP_Real sep_score = -SCIPinfinity(scip);
   vector<SCIP_Real> sep_lambdas(lambdas.size());
   SCIP_Real sep_sigma;
   if ((this->_stab == ST_Smooth || this->_stab == ST_Smooth2 || this->_stab == ST_SmoothAuto)) {
       // safeguard (unfortunately it happens)
       if (SCIPisGT(scip, _piHat_score, out_score)) {
           _piHat_score = -SCIPinfinity(scip);
           _piHat_redcost  = -SCIPinfinity(scip);
           if (this->_stab == ST_SmoothAuto)
               _epsilon = 0.8; // try to reset eps as well
       }

       _prev_out_score = out_score;

       // smooth the duals
       SCIP_Real alpha = _epsilon/_count_mispricings; // mispricings counted lower
       if (!SCIPisInfinity(scip, -_piHat_score)) {

           if (!SCIPisEQ(scip, out_score, _piHat_score)) {
               assert(_piHat_lambdas.size() == out_lambdas.size());

               sep_score = alpha*_piHat_score + (1-alpha)*out_score;
               sep_sigma = alpha*_piHat_sigma + (1-alpha)*out_sigma;
               for (unsigned i=0; i!=out_lambdas.size(); i++)
                   sep_lambdas[i] = alpha*_piHat_lambdas[i] + (1-alpha)*out_lambdas[i];
               // cut sep point
               sigma = sep_sigma;
               lambdas = sep_lambdas;

           }
       }

   }

   /* find the cluster with the minimum reduced cost*/
   vector<bool> cluster;
   SCIP_Real reduced_cost;
   reduced_cost = find_add_clusters(scip, lambdas, sigma);

     /* update duality gap (only used when stabilizing, but interesting to print anyways)*/
     update_duality_gap(scip, out_lambdas, out_sigma, reduced_cost);
     if (verbose >= 2)
       cout << "Duality gap: "<<this->_duality_gap<< "\t(" << this->_lower_bound << " -- " << this->_upper_bound << ")\n";

     /* update the bounds */
     if (this->_stab == ST_Bound || this->_stab == ST_Both){
         if (verbose >= 5)
            cout << "Stabilize (not farkas)\n";

         if (SCIPisLT(scip, (this -> _duality_gap), (this -> _epsilon))) {
	     SCIP_CALL(updatePertVarsBounds(scip));
	     if (this->_stab == ST_Both)
	       SCIP_CALL(updatePertVarsCoefs(scip, lambdas));
         }
     }

     /* when smoothing, update in/out point */
     if (SCIPisFeasNegative(scip, reduced_cost) &&
         (this->_stab == ST_Smooth || this->_stab == ST_Smooth2 || this->_stab == ST_SmoothAuto)) {
         SCIP_Real sep_observed_score = this->bigL(out_lambdas,out_sigma,reduced_cost);

         if (!SCIPisLT(scip, sep_observed_score, sep_score) || _stab == ST_Smooth2) {

             if (this->_stab == ST_SmoothAuto && !SCIPisInfinity(scip, -_piHat_score)) {
                 _count_mispricings++;
             }

         }

             if (SCIPisGT(scip, reduced_cost, _piHat_redcost)) {
                 _piHat_score = sep_observed_score;
                 _piHat_redcost = reduced_cost;
                 _piHat_lambdas = out_lambdas;
                 _piHat_sigma = out_sigma;
             }
     }

   return SCIP_OKAY;
}

/** Pricing of additional variables if LP is feasible.
 *
 *  - get the values of the dual variables you need
 *  - find the best cluster with respect to these dual values
 *  - if this cluster has negative reduced cost, add it to the LP
 *
 *  possible return values for *result:
 *  - SCIP_SUCCESS    : at least one improving variable was found, or it is ensured that no such variable exists
 *  - SCIP_DIDNOTRUN  : the pricing process was aborted by the pricer, there is no guarantee that the current LP solution is optimal
 */
SCIP_DECL_PRICERREDCOST(ObjPricerOptClust::scip_redcost)
{
   SCIPdebugMessage("call scip_redcost ...\n");

   /* set result pointer, see above */
   *result = SCIP_SUCCESS;

   /* call pricing routine */
   SCIP_CALL( pricing(scip) );

   return SCIP_OKAY;
}

/** Pricing of additional variables if LP is infeasible.
 *
 *  - get the values of the dual Farks multipliers you need
 *  - find the best cluster with respect to these values
 *  - if this tour has negative reduced cost, add it to the LP
 */
SCIP_DECL_PRICERFARKAS(ObjPricerOptClust::scip_farkas)
{
   SCIPdebugMessage("call scip_farkas ...\n");

   /* call pricing routine */
   SCIP_CALL( farkasPricing(scip) );

   return SCIP_OKAY;
}

/** add cluster variable to problem */
SCIP_RETCODE ObjPricerOptClust::add_cluster_variable(
   SCIP*                 scip,               /**< SCIP data structure */
   const vector<bool>&      cluster,        /**< the best cluster found by subproblem */
   const vector<SCIP_Real>& lambdas,
   const SCIP_Real sigma
   )
{
   assert(cluster.size() == (unsigned)num_instances());
   bool verbose = false;

   char var_name[255];
   SCIPsnprintf(var_name, 255, "X_%d", SCIPgetNVars(scip));
   SCIPdebugMessage("new variable <%s>\n", var_name);
   if (verbose)
    cout << "creating new variable " << var_name << endl;

   if (verbose)
     print_cluster(cluster, "\tNew column: ");

   SCIP_Real cluster_cost = ObjPricerOptClust::getClusterCost(this->_distance, cluster);

   /* create the new variable */
   SCIP_VAR* var;
   SCIP_CALL( SCIPcreateVar(scip, &var, var_name,
                            0.0,                     // lower bound
                            1.0,      // upper bound
                            cluster_cost,                       // objective
                            SCIP_VARTYPE_BINARY, // variable type
                            false, false, 0, 0, 0, 0, 0) );

   /* add new variable to the list of variables to price into LP */
   SCIP_CALL( SCIPaddPricedVar(scip, var, 1.0) );

   /* add the coefficients of this variable to setcovering and side constraints */
   for (int i= 0; i < num_instances(); ++i)
      if (cluster[i])
         SCIP_CALL( SCIPaddCoefLinear(scip, setcover_con(i), var, 1.0) );

   SCIP_CALL( SCIPaddCoefLinear(scip, side_con(), var, 1.0 ) );

   /* cleanup */
   SCIP_CALL( SCIPreleaseVar(scip, &var) );

   return SCIP_OKAY;
}

/** find and add a cluster when in farkas
 *
 *  uses straightforward calculation
 */
SCIP_Real ObjPricerOptClust::find_add_farkas_cluster(
   SCIP* scip,
   const  vector<SCIP_Real> & lambdas,   /**< matrix of lengths */
   const  SCIP_Real& sigma
   )
{
   int verbose = 0;
   bool found = false;
   SCIP_Real sol = 0;
   vector<bool> cluster;
   SCIP_Real reduced_cost = SCIPinfinity (scip);

   SCIP_CONSHDLR* conshdlr = SCIPfindConshdlr(scip, "MLCL");
   int nconss = SCIPconshdlrGetNActiveConss(conshdlr);

   /* If there are no constraints, add all data points with a positive lambda */
   if (nconss == 0)
     {
       for (vector<SCIP_Real>::const_iterator itr = lambdas.begin(); itr != lambdas.end() ; itr++)
	 {
	   if (SCIPisPositive(scip,*itr)){
	     found = true;
	     cluster.push_back(true);
	     sol -= *itr;
	   }
	   else
	     cluster.push_back(false);
	 }
     }

   /* If there are constraints, use constrained B&B subsolver with zero distances */
   else{

     vector<unsigned int> mlBlocks;
     vector<pair <unsigned int, unsigned int> > clBlocks;
     getLinkConstriants(scip, mlBlocks, clBlocks);

     if (verbose >= 3){
       //printing constraints
       SCIP_CONSDATA* consdata;
       SCIP_CONS** conss = SCIPconshdlrGetConss(conshdlr);

       for (int i=0; i!=nconss; i++) {
	 consdata = SCIPconsGetData(conss[i]);
	 assert(consdata != NULL);
	 unsigned int first = SCIPgetMlClFirst(consdata);
	 unsigned int second = SCIPgetMlClSecond(consdata);
	 bool isML = SCIPgetIsML(consdata);
	 cout << (isML ? "ML" : "CL") << " : " << first << "-" << second << endl;
       }

     }

     vector<vector<double> > zeroDistance(_num_instances, vector<double> (_num_instances, 0));
     SiegBnBCons solver(mlBlocks, clBlocks, zeroDistance , lambdas , sigma, lambdas.size());
     solver.solve();
     if (!solver.hasSolution())
       solver.solvePositive();

     if (solver.hasSolution()){
       sol = solver.getSolution(cluster);
       found = true;
     }

     if (verbose >= 2){
       if (found) {
	 //printing the cluster found by farkas
	 for (auto itr = cluster.begin(), endItr = cluster.end(); itr != endItr; itr++)
	   cout  << (*itr ? "*" : "-");
	 cout << endl;
       }
       else
	 cout << "Farkas could not find an initial solution" << endl;
     }
   }

   if (found){
     /* verifying obtained cluster */
     if (!ConshdlrMLCL::clusterSatisfied(scip, cluster)) {
       cout << "ERROR\n";
       cout << "The above cluster does not satisfy the following set of constraints:\n";
       ConshdlrMLCL::print_constraints(scip);
       exit(1);
     }

     reduced_cost = sol - sigma;
   }

   /* add cluster variable */
   if ( found && SCIPisFeasNegative(scip, reduced_cost) )
   {
     cout << "reduced cost: " << reduced_cost << " sigma: " << sigma  << " sol: " << sol << endl;
     (void) add_cluster_variable(scip, cluster, lambdas, sigma);
   }

   else
     cout << "reduced cost not negative!" << endl;

   return reduced_cost;
}

/** find and add clusters with negative reduced cost
 *  uses a subsolver
 */
//TODO check the effect of returning one reduced cost for a set of added columns
SCIP_Real ObjPricerOptClust::find_add_clusters(
   SCIP* scip,
   const  vector<SCIP_Real> & lambdas,   /**< matrix of lengths */
   const  SCIP_Real& sigma)
{
   // local verbosity
   int verbose = 2;
   bool only_best = !_findAll;

   if (_coefFileName != NULL)
       saveCoef(lambdas, sigma);

   //saveLp(scip, true);

   if (verbose >= 1) {
      int nonzero = 0;
      for (vector<SCIP_Real>::const_iterator itr = lambdas.begin();
            itr != lambdas.end(); itr++){
	if (SCIPisPositive(scip, *itr))
            nonzero++;
      }

      cout << "Sigma: " << sigma;
      cout << " Lambda's ("<<lambdas.size()<<", "<<nonzero<<" non-zero): ";
      vector<int> vals;
      for (unsigned i=0; i != lambdas.size(); i++) {
	if (!SCIPisZero(scip, lambdas[i])) {
            vals.push_back(i);
	    if (verbose >= 3)
	      cout << i << "(" << lambdas[i] << ") ";
	 }
      }
      if (verbose >= 3)
         cout << endl;
      print_range(vals, cout);
      cout << endl;
   }

   boost::posix_time::ptime time1,time2;
   time1 = boost::posix_time::microsec_clock::local_time();

   if (verbose >= 1)
     cout << endl << "Solving subproblem ..." << flush;

   // initialise solver (with ml/cl blocks from the constraint handler)
   vector<unsigned int> mlblocks;
   vector<pair<unsigned int, unsigned int> > clblocks;
   getLinkConstriants(scip, mlblocks, clblocks);
   SiegBnBCons solver(mlblocks, clblocks, _distance , lambdas , sigma, lambdas.size());

   solver.solve();

   if (verbose >= 1) {
       time2 = boost::posix_time::microsec_clock::local_time();
       boost::posix_time::time_duration diff = time2 - time1;
       cout << " runtime: " << boost::posix_time::to_simple_string(diff) << endl;
   }
   SCIP_Real reduced_cost = SCIPinfinity(scip);

   if (only_best) {
      /* get the cluster with the minimum reduced cost*/
      vector<bool> cluster;
      SCIP_Real sol;
      if (!solver.hasSolution()) {
	  if (verbose >= 2)
	    cout << "No solution with negative reduced cost. Now searching for minimum solution with positive reduced cost. " << endl;

	solver.solvePositive();
      }

      sol = solver.getSolution(cluster);
      // sigma was constant in the optimisation and was not yet substracted
      reduced_cost = sol - sigma;

     /* verifying obtained cluster */
     if (!ConshdlrMLCL::clusterSatisfied(scip, cluster)) {
       cout << "ERROR\n";
       cout << "The above cluster does not satisfy the following set of constraints:\n";
       ConshdlrMLCL::print_constraints(scip);
       exit(1);
     }

      /* add cluster variable */
      if ( SCIPisFeasNegative(scip, reduced_cost) || !_pertVars.empty() ) {
	cout << "reduced cost: " << reduced_cost << " sigma: " << sigma  << " sol: " << sol << endl;
	(void) add_cluster_variable(scip, cluster, lambdas, sigma);
      } else
         cout << "reduced cost not negative!" << endl;

   } else {
     /* get and add ALL clusters (if neg. reduced cost) */
     vector< vector<bool> > clusters;

     vector< SCIP_Real > quals = solver.getSolutions(clusters);
     assert(quals.size() == clusters.size());

     int nrAdded = 0;
     for (unsigned i=0; i!=quals.size(); i++) {
       SCIP_Real redcost = quals[i] - sigma;
       if (SCIPisLT(scip, redcost, reduced_cost))
         reduced_cost = redcost;
       if ( SCIPisFeasNegative(scip, redcost) || !_pertVars.empty()) {
         (void) add_cluster_variable(scip, clusters[i], lambdas, sigma);
	 nrAdded++;
	 if (verbose >= 3)
	    cout << "Sol["<<i<<"] reduced cost: " << redcost << " sigma: " << sigma  << " qual: " << quals[i] << endl;
       } else {
	 if (verbose >= 3)
            cout << "Sol["<<i<<"] reduced cost not negative! (" << redcost << ", sigma: " << sigma  << " qual: " << quals[i] << ")" << endl;
	 if (i+1 == quals.size())
	   cout << "No negative reduced cost clusters found.\n";
       }
     }
     if (verbose >= 1)
	cout << "Added " << nrAdded << " columns with smalles neg. red. cost of " << reduced_cost << " (+sigma="<<reduced_cost+sigma<<")" << endl;
   }

   return reduced_cost;
}

// recalculate the duality gap
void ObjPricerOptClust::update_duality_gap(
      SCIP* scip,
      const vector<SCIP_Real> & lambdas,     /**< matrix of lengths */
      const SCIP_Real& sigma,
      const SCIP_Real& reduced_cost
      )
{
	 int verbose = 0;

	 SCIP_Real lambSum = 0;
	 for (vector<SCIP_Real>::const_iterator itr = lambdas.begin(), endItr = lambdas.end(); itr != endItr; itr++)
	   lambSum += *itr;

	 SCIP_Real ub = (this -> _num_clusters) * sigma + lambSum;
         if (verbose >= 2)
           cout << "ub("<<ub<<") = num_clus("<<this -> _num_clusters<<") * sigma("<<sigma<<") + lambSum("<<lambSum<<") -- was: "<<this -> _upper_bound << endl;
	 if (SCIPisLT(scip, ub, (this -> _upper_bound))) {
	   this -> _upper_bound = ub;
	 }

	 SCIP_Real lb = ub + (this -> _num_clusters) * reduced_cost;
         if (verbose >= 2)
           cout << "lb("<<lb<<") = ub("<<ub<<") + num_clus("<<this -> _num_clusters<<") * redCost("<<reduced_cost<<") -- was: "<<this -> _lower_bound << endl;
	 if (SCIPisGT(scip, lb, (this -> _lower_bound))){
	   this -> _lower_bound = lb;
	 }

	 if (!SCIPisInfinity(scip, this -> _upper_bound) &&
	     !SCIPisInfinity(scip, - this -> _lower_bound)){
	   SCIP_Real denom = ((this -> _upper_bound) > 1 ? (this -> _upper_bound) : 1);
	   this -> _duality_gap = ((this -> _upper_bound) - (this -> _lower_bound)) / denom;
           if (verbose >= 2)
               cout << "gap("<<this -> _duality_gap<<") = ( ub("<<this -> _upper_bound<<") - lb("<<this -> _lower_bound<<") ) / denom("<<denom<<")"<<endl;
	 }
         // for plotting UB/LB evolution:
         //cout << "oub;olb;"<<ub<<";"<<lb<<"\n";
}

void ObjPricerOptClust::saveCoef(
   const  vector<SCIP_Real> & lambdas,   /**< matrix of lengths */
   const  SCIP_Real& sigma
   )
{
  _outStream << sigma;

   for (vector<SCIP_Real>::const_iterator itr = lambdas.begin();
        itr != lambdas.end(); itr++){
     _outStream << "\t" << *itr;
   }
   _outStream << endl;

}

void ObjPricerOptClust::saveLp(
   SCIP* scip, bool onlyBasic){
  //   _outStream << endl;

   int nrows = SCIPgetNLPRows(scip);
   int ncols = SCIPgetNLPCols(scip);

   SCIP_COL ** cols = SCIPgetLPCols(scip);
   SCIP_ROW ** rows = SCIPgetLPRows(scip);

   for (int counter = 0; counter < ncols; counter ++){

      //SCIP_BASESTAT stat = (SCIP_BASESTAT) SCIPcolGetBasisStatus (cols[counter]);
      bool isBasic = (SCIPcolGetPrimsol(cols[counter]) > 0);

      if (!onlyBasic || isBasic){

      int nNonZero = SCIPcolGetNLPNonz(cols[counter]);
      SCIP_ROW ** currentColRows = SCIPcolGetRows (cols[counter]);

      int index = 0;
      for (int counter2 = 0; counter2 < nNonZero; counter2++){
         SCIP_ROW * currentRow = currentColRows[counter2];
        int position = SCIProwGetLPPos(currentRow);
        // cout << position << ";";

         while (index < position ){
            cout << "-\t";
            index++;
         }

         cout << position + 1<< "\t";
         index++;
      }

      while (index < nrows - 1){
         cout << "-\t";
         index ++;
      }

      cout << "\tObjVal:" << SCIPcolGetObj(cols[counter]);
      cout << "\tSol:" << SCIPcolGetPrimsol(cols[counter]);
      cout << endl;
      }
   }

   for (int counter = 0; counter < nrows; counter++)
      cout << SCIProwGetDualsol (rows[counter]) << "\t";
   cout << endl;

   //   _outStream << endl;
}

void ObjPricerOptClust::setPertVars(std::vector<SCIP_VAR*> pertVars, SCIP_Real mu, SCIP_Real epsilon)
{
    this -> _pertVars = pertVars;
    this -> _mu = mu;
    this -> _epsilon = epsilon;
}
void ObjPricerOptClust::setEpsilon(SCIP_Real epsilon)
{
    this -> _epsilon = epsilon;
}

SCIP_RETCODE ObjPricerOptClust::updatePertVarsBounds (SCIP* scip){
  _mu /= pow (2, ++ _updateCount);

  if (SCIPisFeasPositive(scip, _mu))
    cout << "Updating bounds of perturbed vars (gap="<<this -> _duality_gap<<" < "<<this -> _epsilon<<"=eps) -- new bound: " << _mu << "\n";

  SCIP_Bool infeasible = false, fixed = true;
    for (std::vector<SCIP_VAR *>::iterator itr = (this -> _pertVars).begin(), endItr = (this -> _pertVars).end();itr != endItr; itr++){
      /* If the bound is not too tight, shrink it */
      if (SCIPisFeasPositive(scip, _mu)){
	SCIP_CALL (SCIPchgVarLb(scip, *itr, -_mu));
	SCIP_CALL (SCIPchgVarUb(scip, *itr, _mu));
      }
      /* otherwise, fix the variables to zero  */
      else {
        SCIP_CALL (SCIPfixVar (scip,*itr,0,&infeasible,&fixed ) );
	_pertVars.clear();
      }
    }

    this -> _lower_bound = -SCIPinfinity (scip);
    this -> _upper_bound = SCIPinfinity (scip);
    this -> _duality_gap = SCIPinfinity (scip);

    return SCIP_OKAY;
}

SCIP_RETCODE ObjPricerOptClust::updatePertVarsCoefs (SCIP * scip, std::vector<SCIP_Real> lambdas)
{
  /* Verify that the bound is not set to zero */
  if (!SCIPisFeasPositive(scip, _mu)) {
    cout << "updatePerVarsCoefs: _mu("<<_mu<<") set to zero, nothing left to do...\n";
    return SCIP_OKAY;
  }

  // TODO!! Check that updated coefficient is not the same as previous
  // if so: no need to fix/recreate (this happens in experiments)

    int verbose=1;

    SCIP_Bool infeasible = false, fixed = true;
    // remove all pertvars for which the lambda changed (and set them to NULL to re-fill later)
    for (unsigned i=0; i!= _pertVars.size(); i++) {
      SCIP_VAR* var = _pertVars[i];

      if (!SCIPisEQ(scip, lambdas[i], SCIPvarGetObj(var))) {
          if (verbose >= 3)
            cout << "var " << i << "new lambda: " << lambdas[i] << " old one: " << SCIPvarGetObj(var) << "\n";

          SCIP_CALL (SCIPfixVar (scip,var,0,&infeasible,&fixed ) );
          _pertVars[i] = NULL;
          if (!fixed)
            return SCIP_ERROR;
      }
    }

    if (verbose >= 3)
      std::cout << "Continueing (num inst) " << _num_instances << "\n";
    for (int counter = 0; counter < _num_instances; counter++){
      if (_pertVars[counter] != NULL)
        continue;

      if (verbose >= 3)
	std::cout << "Creating a perturb var ("<<counter<<")\n";

      SCIP_VAR* var;
      char pert_var_name [255];
      SCIPsnprintf(pert_var_name, 255, "PV%d", counter);
      SCIP_CALL( SCIPcreateVar(scip, &var, pert_var_name,
			       -_mu,                     // lower bound
			       _mu,      // upper bound
			       lambdas[counter],                       // objective
			       SCIP_VARTYPE_CONTINUOUS, // variable type
			       true, //initial
			       true, //removable
			       0, 0, 0, 0, 0) );

      /* add new variable to the list of variables to price into LP */
      SCIP_CALL( SCIPaddPricedVar(scip, var, 1.0) );

      /* add the coefficients of this variable to setcovering and side constraints */
      SCIP_CALL( SCIPaddCoefLinear(scip, setcover_con(counter), var, 1.0) );

      /* add to the vector of purturbation variables */
      _pertVars[counter] = var;

      /* cleanup */
      SCIP_CALL( SCIPreleaseVar(scip, &var) );
    }

    this -> _lower_bound = -SCIPinfinity (scip);
    this -> _upper_bound = SCIPinfinity (scip);
    this -> _duality_gap = SCIPinfinity (scip);

    return SCIP_OKAY;
}

SCIP_Real ObjPricerOptClust::getCurSol(SCIP* scip)
{
    SCIP_Real ret = 0;

    int nVars = SCIPgetNVars(scip);

    SCIP_VAR ** vars = SCIPgetVars(scip);
    for (int i=0; i!=nVars; i++) {
        ret += (SCIPgetVarSol(scip,vars[i]) * SCIPvarGetObj(vars[i]));
    }

    return ret;
}

// The bound at each iteration would still be calculated by lambSum + sigmaK + (reduced cost computed for that iteration)
SCIP_Real ObjPricerOptClust::bigL(
      const vector<SCIP_Real> & lambdas,
      const SCIP_Real& sigma,
      const SCIP_Real& reduced_cost
                             )
{
    SCIP_Real lambSum = 0;
    for (vector<SCIP_Real>::const_iterator itr = lambdas.begin(), endItr = lambdas.end(); itr != endItr; itr++)
        lambSum += *itr;

    SCIP_Real sigmaK = sigma * this->_num_clusters;

    SCIP_Real ret = lambSum + sigmaK + reduced_cost;

    return ret;
}
SCIP_Real ObjPricerOptClust::bigL(
      const vector<SCIP_Real> & lambdas,
      const SCIP_Real& sigma,
      const vector<bool>& cluster
                             )
{
    assert(lambdas.size() == cluster.size());

    SCIP_Real lambSum = 0;
    for (vector<SCIP_Real>::const_iterator itr = lambdas.begin(), endItr = lambdas.end(); itr != endItr; itr++)
        lambSum += *itr;

    SCIP_Real sigmaK = sigma * this->_num_clusters;

    SCIP_Real cx = ObjPricerOptClust::getClusterCost(this->_distance, cluster);

    SCIP_Real piAx = sigma;
    for (unsigned i=0; i!=cluster.size(); i++) {
        if (cluster[i])
            piAx += lambdas[i];
    }

    SCIP_Real ret = lambSum + sigmaK + cx - piAx;
    return ret;
}

SCIP_Real ObjPricerOptClust::bigL_t(SCIP* scip,
      const vector<SCIP_Real> & lambdas,
      const SCIP_Real& sigma
                             )
{
         SCIP_Real ret_min = SCIPinfinity(scip);

         // calculate for each column
         int nCols;
         SCIP_COL** cols;
         SCIP_CALL( SCIPgetLPColsData(scip, &cols, &nCols) );
         for (int i=0; i!=nCols; i++) {
             SCIP_VAR* colVar = SCIPcolGetVar(cols[i]);
             if (SCIPvarIsBinary(colVar)) {
                 vector<bool> cluster = getColCluster(scip, colVar);
                 SCIP_Real valL = bigL(lambdas, sigma, cluster);

                 if (SCIPisLT(scip, valL, ret_min)) {
                     ret_min = valL;
                 }
             }
         }

         return ret_min;
}

std::vector< bool > ObjPricerOptClust::getColCluster(SCIP* scip, SCIP_VAR* colVar)
{
    SCIP_VAR** vars = (SCIP_Var**) malloc (SCIPgetNVars(scip) * sizeof(SCIP_VAR*));

    vector<bool> cluster(num_instances());
    int varssize = SCIPgetNVars(scip);
    SCIP_Bool success;

    for (int i = 0; i < num_instances(); ++i) {
         SCIPgetConsNVars ( scip, _setcover_con[i], &varssize, &success);

         bool inCluster = false;
         if (success) {
             SCIPgetConsVars(scip, _setcover_con[i], vars, varssize, &success);
             if (success) {
                 for (int j=0; j!=varssize; j++) {
                     if (SCIPvarGetIndex(vars[j]) == SCIPvarGetIndex(colVar)) {
                         inCluster = true;
                         break;
                     }
                 }
             }
         }
         cluster[i] = inCluster;
    }

    free(vars);
    return cluster;
}

SCIP_Real ObjPricerOptClust::getClusterCost(const vector< std::vector< double > >& distance, const std::vector< bool >& cluster)
{
   /* compute the cost of a cluster */
   SCIP_Real cluster_cost = 0;

   int cluster_size = 0;
   /* calculate the size of cluster */
   for (unsigned i = 0 ; i < cluster.size(); i++)
      if (cluster[i])
        cluster_size++;

   for (unsigned i = 0 ; i < cluster.size(); i++)
      for (unsigned j = i + 1; j < cluster.size(); j++)
         if (cluster[i] && cluster[j])
            cluster_cost += distance[i][j];

   if (cluster_size > 0)
     cluster_cost /= cluster_size;

   return cluster_cost;
}

struct equalityChecker{
  SCIP* scip;
  bool operator() (pair<const char*, SCIP_Real> const &lhs, pair<const char*, SCIP_Real> const &rhs) {
    return (lhs.first == rhs.first) &&
      SCIPisEQ(scip, lhs.second , rhs.second);
      }
};

bool map_compare (SCIP* scip, const std::map<const char*, SCIP_Real>& lhs, const std::map<const char*, SCIP_Real>& rhs) {
  equalityChecker eqCheck;
  eqCheck.scip = scip;
  return lhs.size() == rhs.size()
    && std::equal(lhs.begin(), lhs.end(),
		  rhs.begin(), eqCheck);
}

void ObjPricerOptClust::getLinkConstriants(SCIP* scip, vector<unsigned int>& mlBlocks, vector<pair<unsigned int, unsigned int> >& clBlocks){
  clBlocks.clear();

  vector<int> tempMBs (_num_instances, -1);
  int blockNum = 0;

  SCIP_CONSHDLR* conshdlr = SCIPfindConshdlr(scip, "MLCL");

  int nconss = SCIPconshdlrGetNActiveConss(conshdlr); // only these first ones are active
  SCIP_CONS** conss = SCIPconshdlrGetConss(conshdlr);
  assert(nconss == 0 || conss != NULL);

  SCIP_CONSDATA* consdata;
  for (int i=0; i!=nconss; i++) {
    consdata = SCIPconsGetData(conss[i]);
    assert(consdata != NULL);

    unsigned int first = SCIPgetMlClFirst(consdata);
    unsigned int second = SCIPgetMlClSecond(consdata);

    if (!SCIPgetIsML(consdata))
      clBlocks.push_back(make_pair(first, second));
    else{
      int firstInd = tempMBs[first] , secondInd = tempMBs[second];
      if (firstInd == -1 && secondInd == -1){
	tempMBs[first] = blockNum;
	tempMBs[second] = blockNum;
	blockNum++;
      }
      else if (firstInd > -1 && secondInd > -1){
	for (auto itr = tempMBs.begin(), endItr = tempMBs.end(); itr != endItr; itr++)
	  if (*itr == firstInd)
	    *itr = secondInd;
      }
      else{
	if (firstInd == -1)
	  tempMBs[first] = secondInd;
	else
	  tempMBs[second] = firstInd;
      }
    }
  }

  mlBlocks.clear();
  for (auto itr = tempMBs.begin(), endItr = tempMBs.end(); itr != endItr; itr++){
    if (*itr == -1){
      *itr = blockNum++;
    }
    mlBlocks.push_back(*itr);
  }
}


//////////////////////////////////////////////////////////////////////////////
///   Copyright 2003 and onward LASMEA UMR 6602 CNRS/U.B.P Clermont-Ferrand
//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   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
//==============================================================================
#define NT2_UNIT_MODULE "nt2 boost.simd.predicates toolbox - is_imag/simd Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of boost.simd.predicates components in simd mode
//////////////////////////////////////////////////////////////////////////////
/// created  by jt the 21/02/2011
///
#include <nt2/include/functions/is_imag.hpp>

#include <boost/simd/sdk/simd/native.hpp>
#include <nt2/sdk/simd/logical.hpp>

#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/mzero.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/i.hpp>
#include <nt2/include/constants/true.hpp>
#include <nt2/include/constants/false.hpp>

#include <nt2/sdk/complex/complex.hpp>

NT2_TEST_CASE_TPL ( is_imag_real__1_0,  BOOST_SIMD_SIMD_REAL_TYPES)
{
  using nt2::is_imag;
  using nt2::tag::is_imag_;
  using boost::simd::native;

  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<std::complex<T>, ext_t> cvT;
  typedef typename boost::dispatch::meta::call<is_imag_(cvT)>::type r_t;
  typedef typename nt2::meta::scalar_of<r_t>::type sr_t;

  // specific values tests
  NT2_TEST_EQUAL(is_imag(nt2::Mzero<cvT>())[0], nt2::True<sr_t>());
  NT2_TEST_EQUAL(is_imag(nt2::Inf<cvT>())[0], nt2::False<sr_t>());
  NT2_TEST_EQUAL(is_imag(nt2::Minf<cvT>())[0], nt2::False<sr_t>());
  NT2_TEST_EQUAL(is_imag(nt2::Mone<cvT>())[0], nt2::False<sr_t>());
  NT2_TEST_EQUAL(is_imag(nt2::Nan<cvT>())[0], nt2::False<sr_t>());
  NT2_TEST_EQUAL(is_imag(nt2::One<cvT>())[0], nt2::False<sr_t>());
  NT2_TEST_EQUAL(is_imag(nt2::Zero<cvT>())[0], nt2::True<sr_t>());
  NT2_TEST_EQUAL(is_imag(nt2::I<cvT>())[0], nt2::True<sr_t>());
} // end of test for floating_


#ifndef _PARAMETERS_HPP
#define _PARAMETERS_HPP

#include <memory>
#include <vector>
#include <string>
#include <opencv2/opencv.hpp>
#include <Eigen/Dense>

#define INF 0x7f800000
// #define USE_CNN

#ifdef USE_CNN
#include <caffe/caffe.hpp>
#include <caffe/util/io.hpp>
#include "net.h"
#endif

using std::string;
using std::vector;

typedef cv::Rect_<float> Rect2f;
typedef std::vector<std::vector<Eigen::MatrixXcf> > EcoFeats;

#ifdef USE_CNN
struct CnnParameters {
	string proto;
	string model;
	string mean_file;

	string ncnn_param;
	string ncnn_bin;

    boost::shared_ptr<ncnn::Net> ncnn_net;
	boost::shared_ptr<caffe::Net<float>> net;
	cv::Mat deep_mean_mat, deep_mean_mean_mat;

	vector<int> stride = {2, 16};			// stride in total
	vector<int> cell_size = {4, 16};		// downsample_factor
	vector<int> output_layer = {3, 14};		// Which layers to use
	vector<int> downsample_factor = {2, 1}; // How much to downsample each output layer
	int input_size_scale = 1;				// Extra scale factor of the input samples to the network (1 is no scaling)
	vector<int> nDim = {96, 512};			// Original dimension of features (ECO Paper Table 1)
	vector<int> compressed_dim = {16, 64};  // Compressed dimensionality of each output layer (ECO Paper Table 1)
	vector<float> penalty = {0, 0};

	vector<int> start_ind = {3, 3, 1, 1};	 // sample feature start index
	vector<int> end_ind = {106, 106, 13, 13}; // sample feature end index
};
struct CnnFeatures {
	CnnParameters fparams;
	cv::Size img_input_sz = cv::Size(224, 224); // VGG default input sample size
	cv::Size img_sample_sz;						// the size of sample
	cv::Size data_sz_block0, data_sz_block1;
	cv::Mat mean;
};
#endif

struct HogParameters {
	int cell_size = 6;
	int compressed_dim = 10;
	int nOrients = 9;
	size_t nDim = 31;
};
struct HogFeatures {
	HogParameters fparams;
	cv::Size img_input_sz;
	cv::Size img_sample_sz;
	cv::Size data_sz_block0;
};

struct CgOpts {
	bool debug;
	bool CG_use_FR;
	float tol;
	bool CG_standard_alpha;
	float init_forget_factor;
	int maxit;
};

struct EcoParameters {
	// Features
	bool useDeepFeature;
	bool useHogFeature;

	HogFeatures hog_features;

	// extra parameters
	CgOpts CG_opts;
	float max_score_threshhold;

	// img sample parameters
	float search_area_scale;
	int min_image_sample_size;
	int max_image_sample_size;

	// Detection parameters
	int newton_iterations;

	// Learning parameters
	float output_sigma_factor;
	float learning_rate;
	size_t nSamples;
	int train_gap;

	// Factorized convolution parameters
	float projection_reg;

	// Conjugate Gradient parameters
	int CG_iter;
	int init_CG_iter;
	int init_GN_iter;
	bool CG_use_FR;
	bool CG_standard_alpha;
	int CG_forgetting_rate;
	float precond_data_param;
	float precond_reg_param;
	int precond_proj_param;

	// Regularization window parameters
	bool use_reg_window;
	double reg_window_min;
	double reg_window_edge;
	size_t reg_window_power;
	float reg_sparsity_threshold;

	float interpolation_bicubic_a;
	size_t number_of_scales;
	float scale_step;
	float min_scale_factor;
	float max_scale_factor;
};
#endif


#include <boost/version.hpp>
#if ((BOOST_VERSION / 100) % 1000) >= 53
#include <boost/thread/lock_guard.hpp>
#endif

#include <ros/ros.h>
#include <iostream>
#include <math.h>

#include <opencv2/core/core.hpp>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/highgui.hpp>
#include "opencv2/ximgproc.hpp"

#include "opencv2/calib3d.hpp"
#include "opencv2/imgproc.hpp"
#include <string>

#include "darknet_ros_msgs/BoundingBoxes.h"
#include "darknet_ros_msgs/BoundingBox.h"
#include <sensor_msgs/CameraInfo.h>
#include <image_geometry/stereo_camera_model.h>
#include <stereo_msgs/DisparityImage.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <geometry_msgs/Point.h>
#include <custom_msgs/DetectedObjects.h>
#include <custom_msgs/Object.h>
#include <custom_msgs/NorthEastHeading.h>
#include <custom_msgs/gnssGGA.h>

#include <custom_libraries/convert_to_NED.hpp>

#include <fstream>

double heading_MA;

using namespace message_filters;

ros::Publisher detected_objects_pub;

float medianMat (cv::Mat image){
  double m=(image.rows*image.cols)/2;
  int bin=0;
  int med = -1;
  int histSize = 256;
  float range[] = { 0, 256 } ;
  const float* histRange = { range };
  bool uniform = true;
  bool accumulate = false;
  cv::Mat hist;
  cv::calcHist( &image, 1, 0, cv::Mat(), hist, 1, &histSize, &histRange, uniform, accumulate );

    for (int i=0; i<256 && ( med<0);i++){
        bin=bin+cvRound(hist.at<float>(i));
        if (bin>m && med<0)
            med=i;
    }

    return med;
}

double constrainAngle(double x){
    x = x - 90;
    x = fmod(x + 180,360);
    if (x < 0)
        x += 360;
    return x - 180;
}

void headingCallback(const custom_msgs::NorthEastHeading::ConstPtr& heading){
  heading_MA = heading->heading;

}

void callback(const stereo_msgs::DisparityImage::ConstPtr& disp, const darknet_ros_msgs::BoundingBoxes::ConstPtr& objects, const sensor_msgs::CameraInfo::ConstPtr& l_cam_info, const sensor_msgs::CameraInfo::ConstPtr& r_cam_info, const custom_msgs::gnssGGA::ConstPtr& pos){
    custom_msgs::DetectedObjects detected_objects;

    std::vector<double> position_MA = lla2nedPiren(pos->latitude, pos->longitude, pos->altitude);

    image_geometry::StereoCameraModel model;
    model.fromCameraInfo(l_cam_info, r_cam_info);

    const sensor_msgs::Image& dimage = disp->image;
    cv::Mat_<float> dmat(dimage.height, dimage.width, (float*)&dimage.data[0], dimage.step);
    cv::Mat disp_u(dmat.size(), 0);
    dmat.convertTo(disp_u, disp_u.type());
    cv::normalize(disp_u, disp_u, 0, 255, cv::NORM_MINMAX );
    applyColorMap(disp_u, disp_u, cv::COLORMAP_JET);

    for(int i = 0; i < objects->bounding_boxes.size() ; i++){

      int width = objects->bounding_boxes[i].xmax - objects->bounding_boxes[i].xmin;
      int height = objects->bounding_boxes[i].ymax - objects->bounding_boxes[i].ymin;
      cv::Mat roi(dmat, cv::Rect(objects->bounding_boxes[i].xmin, objects->bounding_boxes[i].ymin, width, height));

      float med = medianMat(roi);

      float middle_x = objects->bounding_boxes[i].xmin + (width/2);
      float middle_y = objects->bounding_boxes[i].ymin + (height/2);
      cv::Point2d point_uv(middle_x, middle_y);
      cv::Point3d point_xyz;

      model.projectDisparityTo3d(point_uv, med, point_xyz);

      geometry_msgs::Point coord;
      coord.x = (point_xyz.x/1000);
      coord.y = point_xyz.y/1000;
      coord.z = (point_xyz.z/1000);

      cv::Point3d xmin_xyz, xmax_xyz;
      model.projectDisparityTo3d(cv::Point2d(objects->bounding_boxes[i].xmin, objects->bounding_boxes[i].ymin), dmat.at<float>(objects->bounding_boxes[i].ymin, objects->bounding_boxes[i].xmin),xmin_xyz);
      model.projectDisparityTo3d(cv::Point2d(objects->bounding_boxes[i].xmax, objects->bounding_boxes[i].ymin), dmat.at<float>(objects->bounding_boxes[i].ymin, objects->bounding_boxes[i].xmax),xmax_xyz);

      float obj_width = (xmax_xyz.x/1000) - (xmin_xyz.x/1000);

      std::vector<double> obj_NED = objectCoord2NED(coord.x, coord.y, coord.z, position_MA[0], position_MA[1], position_MA[2], heading_MA);

      custom_msgs::Object object;
      object.north = obj_NED[0];
      object.east  = obj_NED[1];
      object.down  = obj_NED[2];
      object.width = obj_width;
      object.Class = objects->bounding_boxes[i].Class;

      if (coord.z  < 250){
        detected_objects.objects.push_back(object);
      }
    }
    detected_objects.header = objects->header;
    detected_objects_pub.publish(detected_objects);
}

int main(int argc, char** argv){
  ros::init(argc, argv, "clustering_cnn");
  ros::NodeHandle nh;

  heading_MA = 0;
  detected_objects_pub = nh.advertise<custom_msgs::DetectedObjects>("detected_objects", 10);

  message_filters::Subscriber<sensor_msgs::CameraInfo> l_camInfo_sub(nh, "/camera_array/left/camera_info", 1);
  message_filters::Subscriber<sensor_msgs::CameraInfo> r_camInfo_sub(nh, "/camera_array/right/camera_info", 1);
  message_filters::Subscriber<stereo_msgs::DisparityImage> disp_sub(nh, "/camera_array/disparity", 100);
  message_filters::Subscriber<darknet_ros_msgs::BoundingBoxes> objects_sub(nh, "/darknet_ros/bounding_boxes", 1);
  message_filters::Subscriber<custom_msgs::gnssGGA> position_sub(nh, "/vectorVS330/fix", 1);
  TimeSynchronizer<stereo_msgs::DisparityImage,darknet_ros_msgs::BoundingBoxes, sensor_msgs::CameraInfo,sensor_msgs::CameraInfo, custom_msgs::gnssGGA> sync(disp_sub, objects_sub, l_camInfo_sub, r_camInfo_sub, position_sub, 10);
  sync.registerCallback(boost::bind(&callback, _1, _2, _3, _4, _5));

  ros::Subscriber heading_sub = nh.subscribe("/navigation/eta", 1000, headingCallback);

  ros::spin();
  return 0;

}


//
// metatest.cpp: This program produces a nice set of
//				 errors to flex the muscles of STLFilt's
//				 template metaprogramming-specific features.
//
// Note: You must have a recent version of the Boost libs installed.

#include <boost/mpl/range_c.hpp>
#include <boost/mpl/reverse_fold.hpp>
#include <boost/mpl/plus.hpp>

namespace mpl = boost::mpl;

mpl::reverse_fold<
        mpl::range_c<long, 0, 50>
      , void
      , mpl::plus<>
>::type x;


// Stdlib
#include <stdlib.h>
#include <cmath>
#include <limits.h>
#include <iterator>
#include <vector>
#include <boost/format.hpp>
#include <stdint.h>

// PCL input/output
#include <pcl/console/parse.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/point_cloud_color_handlers.h>

//PCL other
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/supervoxel_clustering.h>
#include <pcl/features/normal_3d_omp.h>

// The segmentation class this example is for
#include <pcl/segmentation/lccp_segmentation.h>

typedef pcl::LCCPSegmentation<pcl::PointXYZ>::SupervoxelAdjacencyList SuperVoxelAdjacencyList;

void
compute_surface_normals (pcl::PointCloud<pcl::PointXYZ>::Ptr &points, float normal_radius,
                         pcl::PointCloud<pcl::Normal>::Ptr &normals_out)
{
  pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> norm_est;

  // Use a FLANN-based KdTree to perform neighborhood searches
  //norm_est.setSearchMethod (pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr (new pcl::KdTreeFLANN<pcl::PointXYZ>));
  norm_est.setSearchMethod (pcl::search::KdTree<pcl::PointXYZ>::Ptr (new pcl::search::KdTree<pcl::PointXYZ>));

  // Specify the size of the local neighborhood to use when computing the surface normals
  norm_est.setRadiusSearch (normal_radius);

  // Set the input points
  norm_est.setInputCloud (points);

  // Estimate the surface normals and store the result in "normals_out"
  norm_est.compute (*normals_out);
}

int
main (int argc, char** argv)
{
  // Fetch point cloud filename in arguments | Works with PCD and PLY files
  std::vector<int> filenames;
  bool file_is_pcd = false;

  filenames = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");

  pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud_ptr (new pcl::PointCloud<pcl::PointXYZ> ());

  if (pcl::io::loadPCDFile (argv[filenames[0]], *input_cloud_ptr) < 0)
  {
      std::cout << "Error loading point cloud " << argv[filenames[0]] << std::endl << std::endl;
      return -1;
  }

  std::cout<< "NUmber of input points" << input_cloud_ptr->size () << std::endl;

  /// -----------------------------------|  Preparations  |-----------------------------------

  /// Create variables needed for preparations
  std::string outputname ("");
  pcl::PointCloud<pcl::Normal>::Ptr input_normals_ptr (new pcl::PointCloud<pcl::Normal>);
  float normal_radius = 5.0 ;
  compute_surface_normals (input_cloud_ptr, normal_radius, input_normals_ptr);

/// -----------------------------------|  Main Computation  |-----------------------------------

  ///  Default values of parameters before parsing
  // Supervoxel Stuff
  float voxel_resolution = 4.0f;
  float seed_resolution = 10.0f;
  float color_importance = 0.0f;
  float spatial_importance = 1.0f;
  float normal_importance = 4.0f;
  bool use_single_cam_transform = false;
  bool use_supervoxel_refinement = true;

  // LCCPSegmentation Stuff
  float concavity_tolerance_threshold = 10;
  float smoothness_threshold = 0.4;
  uint32_t min_segment_size = 4.0;
  bool use_extended_convexity = true;
  bool use_sanity_criterion = false;

  ///  Parse Arguments needed for computation
  //Supervoxel Stuff

  //  normals_scale = seed_resolution / 2.0;

  // Segmentation Stuff
  unsigned int k_factor = 0;
  if (use_extended_convexity)
    k_factor = 1;

  /// Preparation of Input: Supervoxel Oversegmentation

  pcl::SupervoxelClustering<pcl::PointXYZ> super (voxel_resolution, seed_resolution);
  super.setUseSingleCameraTransform (use_single_cam_transform);
  super.setInputCloud (input_cloud_ptr);

  super.setNormalCloud (input_normals_ptr);
  super.setColorImportance (color_importance);
  super.setSpatialImportance (spatial_importance);
  super.setNormalImportance (normal_importance);
  std::map<uint32_t, pcl::Supervoxel<pcl::PointXYZ>::Ptr> supervoxel_clusters;

  PCL_INFO ("Extracting supervoxels\n");
  super.extract (supervoxel_clusters);

  if (use_supervoxel_refinement)
  {
    PCL_INFO ("Refining supervoxels\n");
    super.refineSupervoxels (2, supervoxel_clusters);
  }
  std::stringstream temp;
  temp << "  Nr. Supervoxels: " << supervoxel_clusters.size () << "\n";
  PCL_INFO (temp.str ().c_str ());

  PCL_INFO ("Getting supervoxel adjacency\n");
  std::multimap<uint32_t, uint32_t> supervoxel_adjacency;
  super.getSupervoxelAdjacency (supervoxel_adjacency);

  /// Get the cloud of supervoxel centroid with normals and the colored cloud with supervoxel coloring (this is used for visulization)
  pcl::PointCloud<pcl::PointNormal>::Ptr sv_centroid_normal_cloud = pcl::SupervoxelClustering<pcl::PointXYZ>::makeSupervoxelNormalCloud (supervoxel_clusters);

  /// The Main Step: Perform LCCPSegmentation

  PCL_INFO ("Starting Segmentation\n");
  pcl::LCCPSegmentation<pcl::PointXYZ> lccp;
  lccp.setConcavityToleranceThreshold (concavity_tolerance_threshold);
  lccp.setSanityCheck (use_sanity_criterion);
  lccp.setSmoothnessCheck (true, voxel_resolution, seed_resolution, smoothness_threshold);
  lccp.setKFactor (k_factor);
  lccp.setInputSupervoxels (supervoxel_clusters, supervoxel_adjacency);
  lccp.setMinSegmentSize (min_segment_size);
  lccp.segment ();

  PCL_INFO ("Interpolation voxel cloud -> input cloud and relabeling\n");
  pcl::PointCloud<pcl::PointXYZL>::Ptr sv_labeled_cloud = super.getLabeledCloud ();
  pcl::PointCloud<pcl::PointXYZL>::Ptr lccp_labeled_cloud = sv_labeled_cloud->makeShared ();
  lccp.relabelCloud (*lccp_labeled_cloud);
  SuperVoxelAdjacencyList sv_adjacency_list;
  lccp.getSVAdjacencyList (sv_adjacency_list);  // Needed for visualization
  std::cout<< "number of lccp points : " << lccp_labeled_cloud->size () << std::endl;

  // storing all the indices of the LCCP groups

  std::vector<uint32_t> index_group;
  for (size_t i = 0; i < lccp_labeled_cloud->size (); ++i)
  {
    std::cout<<"It's round "<< i << ": lable is "<< lccp_labeled_cloud-> at (i).label<< std::endl;
    if (i == 0)  index_group.push_back(lccp_labeled_cloud-> at (0).label);
    else
    {
      bool flag = false;
      for (auto pit : index_group)
      {
        std::cout<< pit <<std::endl;
        if ( pit == lccp_labeled_cloud-> at (i).label)  flag = true;
      }

      if (!flag)  index_group.push_back(lccp_labeled_cloud-> at (i).label);
    }

  }

  std::cout << "start!!!!" << std::endl;

  for (auto pit : index_group)
  {
    std::cout<< pit << std::endl;
  }

  // storing every group of cloud points into the vector

  //  std::vector<pcl::PointCloud<pcl::PointXYZ>> points_group;
  int count = 1;
  for (auto pit : index_group)
  {
    pcl::PointCloud<pcl::PointXYZL>::Ptr cloud_ (new pcl::PointCloud<pcl::PointXYZL> ());
    for (size_t i = 0; i < lccp_labeled_cloud->size (); ++i)
    {
      if (lccp_labeled_cloud-> at (i).label == pit)
      {
        cloud_->push_back(lccp_labeled_cloud->at(i));
      }
    }
    if (cloud_->size()>10000)
    {
      std::stringstream name_point;
      name_point<<"cloud_group"<< count<<".pcd";
      pcl::io::savePCDFileASCII(name_point.str(), *cloud_);
      count ++;
    }
//    points_group.push_back(cloud_);
  }

/*
  size_t number_max =2000;
//
  int count =1;
  for (auto pit : points_group)
  {
    std::stringstream name_point;
    name_point<<"cloud_group"<< count;
    if (points_group.size() > number_max)
    {
      pcl::io::savePCDFileASCII(name_point.str(), pit);
    }
    count ++;
  }
*/
/*
typedef pcl::LCCPSegmentation<pcl::PointXYZ>::VertexIterator VertexIterator;

std::pair<VertexIterator, VertexIterator> vertex_iterator_range;
vertex_iterator_range = boost::vertices (sv_adjacency_list);

for (VertexIterator itr = vertex_iterator_range.first; itr != vertex_iterator_range.second; ++itr)
{
	const uint32_t sv_label = sv_adjacency_list[*itr];
	pcl::PointXYZL group_label = lccp_labeled_cloud->points[ sv_label ];
        std::cout << "sv_label: " << sv_label << "; group_label.label: " << group_label.label <<std::endl;
//	printf(“%d %d\n”, sv_label, group_label.label); // supervoxel label, segmentation group label

}

/*
  /// Creating Colored Clouds and Output
  if (lccp_labeled_cloud->size () == input_cloud_ptr->size ())
  {
    if (output_specified)
    {
      PCL_INFO ("Saving output\n");
      if (add_label_field)
      {
        if (pcl::getFieldIndex (input_pointcloud2, "label") >= 0)
          PCL_WARN ("Input cloud already has a label field. It will be overwritten by the lccp segmentation output.\n");
        pcl::PCLPointCloud2 output_label_cloud2, output_concat_cloud2;
        pcl::toPCLPointCloud2 (*lccp_labeled_cloud, output_label_cloud2);
        pcl::concatenateFields (input_pointcloud2, output_label_cloud2, output_concat_cloud2);
        pcl::io::savePCDFile (outputname + "_out.pcd", output_concat_cloud2, Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), save_binary_pcd);
      }
      else
        pcl::io::savePCDFile (outputname + "_out.pcd", *lccp_labeled_cloud, save_binary_pcd);

      if (sv_output_specified)
      {
        pcl::io::savePCDFile (outputname + "_svcloud.pcd", *sv_centroid_normal_cloud, save_binary_pcd);
      }
    }
  }
  else
  {
    PCL_ERROR ("ERROR:: Sizes of input cloud and labeled supervoxel cloud do not match. No output is produced.\n");
  }
*/
  return (0);
}


#include <vector>
#include <Eigen/Dense>
#include <cassert>
#include <numeric>
#include <math.h>
#include <errno.h>
#include <fstream>
#include <iostream>
#include <map>
#include <vector>
#include <algorithm>
#include <regex>
#include "MorrisMethod.h"
#include "Transformable.h"
#include "RunManagerAbstract.h"
#include "ParamTransformSeq.h"
#include "ModelRunPP.h"
#include "utilities.h"
#include "FileManager.h"
#include "Stats.h"
//#include "Ensemble.h"

using namespace std;
using namespace pest_utils;
using Eigen::MatrixXd;
using Eigen::VectorXd;

void MorrisObsSenFile::initialize(const vector<string> &_par_names_vec, const vector<string> &_obs_names_vec, double _no_data, const GsaAbstractBase *_gsa_abstract_base)
{
	par_names_vec =_par_names_vec;
	obs_names_vec = _obs_names_vec;
	gsa_abstract_base = _gsa_abstract_base;
	no_data = _no_data;
}

void MorrisObsSenFile::add_sen_run_pair(const std::string &par_name, double p1, Observations &obs1, double p2, Observations &obs2)
{
	assert (obs1.size() == obs2.size());

	// compute sensitivities of individual observations
	double isen;
	double del_par = p2 - p1;

	for (const auto &iobs : obs_names_vec)
	{
		isen = (obs2[iobs] - obs1[iobs]) / del_par;
		const auto id = make_pair(par_name, iobs);
		if (map_obs_stats.find(id) == map_obs_stats.end())
		{
			map_obs_stats[id] = RunningStats();
		}
		map_obs_stats[make_pair(par_name, iobs)].add(isen);
	}
}

void MorrisObsSenFile::calc_pooled_obs_sen(ofstream &fout_obs_sen, map<string, double> &obs_2_sen_weight,
	map<string, double> &par_2_sen_weight)
{
	fout_obs_sen << "par_name, n_samples, obs_name, mean, abs_mean, sigma, scaled_sen" << endl;
	for (const auto &ip : par_names_vec)
	{
		string ipar = ip;
		for (const auto &iobs : obs_names_vec)
		{
			double mean = no_data;
			double abs_mean = no_data;
			double sigma = no_data;
			int n_samples = 0;
			const auto id = make_pair(ipar, iobs);
			auto sen_itr = map_obs_stats.find(id);
			if (sen_itr != map_obs_stats.end() && sen_itr->second.comp_nsamples() > 0)
			{
				mean = sen_itr->second.comp_mean();
				abs_mean = sen_itr->second.comp_abs_mean();
				sigma = sen_itr->second.comp_sigma();
				n_samples = sen_itr->second.comp_nsamples();
			}
			string weighted_sen = "N/A";
			auto it_obs = obs_2_sen_weight.find(iobs);
			auto it_par = par_2_sen_weight.find(ipar);
			if (it_obs != obs_2_sen_weight.end() && it_par != par_2_sen_weight.end() && abs_mean != no_data)
			{
				stringstream sstr;
				double value = abs_mean * it_par->second / it_obs->second;
				sstr << value;
				weighted_sen = sstr.str();
			}
			fout_obs_sen << ipar << ", " << n_samples << ", " << iobs << ", " << mean << ", " << abs_mean << ", " << sigma << ", " << weighted_sen << endl;
		}
	}
}

void MorrisMethod::process_pooled_var_file()
{
	if (calc_obs_sen == true)
	{
		ifstream &fin = file_manager_ptr->open_ifile_ext("pgp");
		std::set<string> obs_group_names;
		for (const auto &imap : obs_info_ptr->groups)
			obs_group_names.insert(imap.first);

		string line;
		string cur_pool_grp;
		regex reg_reg("regex\\s*\\(\"(.+)\"\\)", regex_constants::icase);
		regex reg_grp("pool_group\\s*\\((.+)\\)", regex_constants::icase);
		cmatch mr;
		while (getline(fin, line))
		{
			if (regex_match(line.c_str(), mr, reg_reg))
			{
				regex inp_reg = regex(mr[1].str(), regex_constants::icase);

				for (auto itr = obs_group_names.begin(); itr != obs_group_names.end();)
				{
					auto  here = itr++;
					if (regex_match(*here, inp_reg))
					{
						group_2_pool_group_map[*here] = cur_pool_grp;
						obs_group_names.erase(here);
					}
				}

			}
			if (regex_match(line.c_str(), mr, reg_grp))
			{
				cur_pool_grp = mr[1];
			}
		}
	}
	file_manager_ptr->close_file("pgp");
}

MatrixXd MorrisMethod::create_P_star_mat(int k)
{
	MatrixXd b_mat = create_B_mat(k);
	MatrixXd j_mat = create_J_mat(k);
	MatrixXd d_mat = create_D_mat(k);
	MatrixXd x_vec = create_x_vec(k);
	MatrixXd p_mat = create_P_mat(k);
	b_star_mat = j_mat.col(0) * x_vec.transpose() + (delta/2.0)*((2.0 * b_mat - j_mat) * d_mat + j_mat) ;
	return b_star_mat;
}

MorrisMethod::MorrisMethod(Pest &_pest_scenario,
	FileManager &_file_manager, ObjectiveFunc *_obj_func_ptr,
	const ParamTransformSeq &_par_transform,
	int _p, int _r, double _delta,
	bool _calc_pooled_obs, bool _calc_morris_obs_sen, PARAM_DIST _par_dist, unsigned int _seed)
	: GsaAbstractBase(_pest_scenario, _file_manager, _obj_func_ptr, _par_transform,
		_par_dist, _seed),
	calc_obs_sen(_calc_pooled_obs), calc_morris_obs_sen(_calc_morris_obs_sen)
{
	initialize(_p, _r, _delta);
}

void MorrisMethod::initialize(int _p, int _r, double _delta)
{
	p = _p;
	r = _r;
	delta = _delta;
	//delta = p / (2.0 * (p - 1));
}

MatrixXd MorrisMethod::create_B_mat(int k)
{
	MatrixXd b_mat = MatrixXd::Constant(k+1, k, 0.0);
	int nrow = k+1;
	int ncol = k;
	for (int icol=0; icol<ncol; ++icol)
	{
		for (int irow=icol+1; irow<nrow; ++irow)
		{
			b_mat(irow, icol) = 1.0;
		}
	}
	return b_mat;
}

MatrixXd MorrisMethod::create_J_mat(int k)
{
	MatrixXd j_mat = MatrixXd::Constant(k+1, k, 1.0);
	return j_mat;
}

MatrixXd MorrisMethod::create_D_mat(int k)
{
	MatrixXd d_mat=MatrixXd::Constant(k, k, 0.0);
	for (int i=0; i<k; ++i) {
		d_mat(i,i) = rand_plus_minus_1();
	}
	return d_mat;
}

MatrixXd MorrisMethod::create_P_mat(int k)
{
	// generate and return a random permutation matrix
	MatrixXd p_mat=MatrixXd::Constant(k, k, 0.0);
	vector<int> rand_idx;
	rand_idx.reserve(k);
	for (int i=0; i<k; ++i) {
		rand_idx.push_back(i);
	}
	// Shuffle random index vector
	shuffle(rand_idx.begin(), rand_idx.end(), rand_engine);

	for(int irow=0; irow<k; ++irow)
	{
	  p_mat(irow, rand_idx[irow]) = 1;
	}

	return p_mat;
}

VectorXd MorrisMethod::create_x_vec(int k)
{
	// Warning Satelli's book is wrong.  Need to use Morris's original paper
	// to compute x.  Maximim value of any xi shoud be 1.0 - delta.
	VectorXd x(k);
	double rnum;
	// Used with the randon number generator below, max_num will produce a
	// random interger which varies between 0 and (p-2)/2.  This will in turn
	// produce the correct xi's which vary from {0, 1/(p-1), 2/(p-1), .... 1 - delta)
	// when divided by (p-1).  See Morris's paper for the derivaition.
	// Satelli's book has this equation wrong!
	int max_num = 1+(p-2)/2;
	for (int i=0; i<k; ++i)
	{
		rnum = rand_engine() % max_num;
		x(i) = rnum / (p - 1);
	}
	return x;
}

int MorrisMethod::rand_plus_minus_1(void)
{
	return (rand_engine() % 2 * 2) - 1;
}

Parameters MorrisMethod::get_numeric_parameters(int row)
{
	Parameters numeric_pars;
	auto e=adj_par_name_vec.end();
	size_t n_cols = b_star_mat.cols();
	for (int j=0; j<n_cols; ++j)
	{
		const string &p = adj_par_name_vec[j];
		auto it_lbnd = min_numeric_pars.find(p);
		assert(it_lbnd != min_numeric_pars.end());
		auto it_ubnd = max_numeric_pars.find(p);
		assert(it_ubnd != max_numeric_pars.end());
		numeric_pars[p] =  it_lbnd->second + (it_ubnd->second - it_lbnd->second) * b_star_mat(row, j);
	}
	return numeric_pars;
}

void MorrisMethod::assemble_runs(RunManagerAbstract &run_manager)
{
	map<int, Parameters> par_map;
	for (int tmp_r=0; tmp_r<r; ++tmp_r)
	{
		b_star_mat = create_P_star_mat(adj_par_name_vec.size());
		auto n_rows = b_star_mat.rows();
		int run_id;
		string par_name = "";
		for (int i=0; i<n_rows; ++i)
		{
			par_name.clear();
			//get control parameters
			Parameters pars = get_numeric_parameters(i);
			// convert control parameters to model parameters
			base_partran_seq_ptr->numeric2model_ip(pars);
			// convert control parameters to model parameters
			base_partran_seq_ptr->ctl2model_ip(pars);
			if (i>0)
			{
				par_name = adj_par_name_vec[i-1];
			}
			run_id = run_manager.add_run(pars, par_name, Parameters::no_data);
			base_partran_seq_ptr->model2ctl_ip(pars);
			par_map[run_id] =  pars;

		}
	}
	//write the parameter sequence to a csv file
	string filename = file_manager_ptr->get_base_filename() + ".sen.par.csv";
	ofstream fout(filename);
	vector<string> var_names = pest_scenario_ptr->get_ctl_ordered_par_names();
	fout << "run_id";
	for (auto pname : var_names)
		fout << "," << pname;
	fout << endl;
	for (auto &p : par_map)
	{
		fout << p.first;
		for (auto &pname : var_names)
			fout << "," << p.second.get_rec(pname);
		fout << endl;
	}
	/*ParameterEnsemble pe(pest_scenario_ptr);
	vector<string> real_names,var_names=pest_scenario_ptr->get_ctl_ordered_par_names();
	stringstream ss;
	map<int, string> real_name_map;
	for (auto &p : par_map)
	{
		ss.str("");
		ss << p.first;
		real_names.push_back(ss.str());
		real_name_map[p.first] = ss.str();
	}
	pe.set_trans_status(ParameterEnsemble::transStatus::CTL);
	pe.reserve(real_names, var_names);
	for (auto &p : par_map)
	{
		pe.update_real_ip(real_name_map[p.first], p.second.get_data_eigen_vec(var_names));
	}
	string filename = file_manager_ptr->get_base_filename() + ".sen.par.csv";
	pe.to_csv(filename);*/

}

void  MorrisMethod::calc_sen(RunManagerAbstract &run_manager, ModelRun model_run)
{
	ofstream &fout_morris = file_manager_ptr->open_ofile_ext("msn");
	ofstream &fout_raw = file_manager_ptr->open_ofile_ext("raw.csv");

	ModelRun run0 = model_run;
	ModelRun run1 = model_run;
	Parameters pars0;
	Observations obs0;
	Parameters pars1;
	Observations obs1;

	map<string, RunningStats > sen_map;
	map<string, RunningStats> obs_stats_map;

	for (auto &it_p : adj_par_name_vec)
	{
		sen_map[it_p] = RunningStats();
	}

	for (auto &it_obs : obs_name_vec)
	{
		obs_stats_map[it_obs] = RunningStats();
	}

	const vector<string> &run_mngr_obs_name_vec = run_manager.get_obs_name_vec();
	obs_sen_file.initialize(adj_par_name_vec, run_mngr_obs_name_vec, Observations::no_data, this);

	fout_raw << "parameter_name, phi_0, phi_1, par_0, par_1, elem_effect" << endl;
	int n_runs = run_manager.get_nruns();
	bool run0_ok = false;
	bool run1_ok = false;
	string par_name_1;
	double null_value;
	stringstream message;
	cout << endl;
	run1_ok = run_manager.get_run(0, pars1, obs1);
	base_partran_seq_ptr->model2numeric_ip(pars1);
	for (int i_run=1; i_run<n_runs; ++i_run)
	{
		std::cout << string(message.str().size(), '\b');
		message.str("");
		message << "processing run " << i_run+1 << " / " << n_runs;
		std::cout << message.str();

		run0_ok = run1_ok;
		pars0 = pars1;
		obs0 = obs1;
		run1_ok = run_manager.get_run(i_run, pars1, obs1, par_name_1, null_value);
		base_partran_seq_ptr->model2numeric_ip(pars1);
		// Add run0 to obs_stats
		if (run0_ok)
		{
			for (const auto &i_obs : run_mngr_obs_name_vec)
			{
				auto it = obs0.find(i_obs);
				if (it != obs0.end() && it->second != Observations::no_data)
				{
					obs_stats_map[i_obs].add(obs0[i_obs]);
				}
			}
		}

		if (run0_ok && run1_ok && !par_name_1.empty())
		{
			Parameters tmp_ctl_par = base_partran_seq_ptr->numeric2ctl_cp(pars0);
			run0.update_ctl(tmp_ctl_par, obs0);
			double phi0 = run0.get_phi(DynamicRegularization::get_zero_reg_instance());
			tmp_ctl_par = base_partran_seq_ptr->numeric2ctl_cp(pars1);
			run1.update_ctl(tmp_ctl_par, obs1);
			double phi1 = run1.get_phi(DynamicRegularization::get_zero_reg_instance());
			double p0 = pars0[par_name_1];
			double p1 = pars1[par_name_1];
			// compute standard Morris Sensitivity on the global objective function
			double sen = (phi1 - phi0) / delta;
			fout_raw << par_name_1 << ",  " << phi1 << ",  " << phi0 << ",  " << p1 << ",  " << p0 << ", " << sen << endl;

			const auto &it_senmap = sen_map.find(par_name_1);
			if (it_senmap != sen_map.end())
			{
				it_senmap->second.add(sen);
			}

			//Compute sensitvities of indiviual observations
			obs_sen_file.add_sen_run_pair(par_name_1, p0, obs0, p1, obs1);
		}
	}
	// Add final run to obs_stats
	if (run1_ok)
	{
		for (const auto &i_obs : run_mngr_obs_name_vec)
		{
			auto it = obs1.find(i_obs);
			if (it != obs1.end() && it->second != Observations::no_data)
			{
				obs_stats_map[i_obs].add(obs0[i_obs]);
			}
		}
	}
	cout << endl;
	cout << "writing output files" << endl;
	// write standard Morris Sensitivity for the global objective function
	fout_morris << "parameter_name, n_samples, sen_mean, sen_mean_abs, sen_std_dev" << endl;
	for (const auto &it_par : adj_par_name_vec)
	{
		const auto &it_senmap = sen_map.find(it_par);
		if (it_senmap != sen_map.end())
		{
			fout_morris << it_par << ", " << it_senmap->second.comp_nsamples() << ", " << it_senmap->second.comp_mean() << ", " << it_senmap->second.comp_abs_mean() << ", " << sqrt(it_senmap->second.comp_var()) << endl;
		}
	}
	if (calc_morris_obs_sen)
	{
		// write standard Morris Sensitivity for individual observations
		ofstream &fout_mis = file_manager_ptr->open_ofile_ext("mio");
		calc_morris_obs(fout_mis, obs_sen_file);
		file_manager_ptr->close_file("mio");
	}

	if (calc_obs_sen)
	{
		ofstream &fout_mos = file_manager_ptr->open_ofile_ext("mos");
		////compute pooled standard deviations
		map<string, double> obs_2_sen_weight;
		{
			//Compute Pooled Standard Deviations
			map<string, vector<RunningStats> > tmp_pool_grps;
			for (const auto &it : obs_stats_map)
			{
				const string &obs_name = it.first;
				const string &obs_group = obs_info_ptr->get_group(obs_name);
				auto it_pg = group_2_pool_group_map.find(obs_group);
				if (it_pg != group_2_pool_group_map.end())
				{
					const string &pool_group = it_pg->second;
					if (tmp_pool_grps.find(pool_group) == tmp_pool_grps.end())
					{
						tmp_pool_grps[pool_group] = vector<RunningStats>();
					}
					tmp_pool_grps[pool_group].push_back(it.second);
				}
			}
			////compute pooled standard deviations
			map<string, double> obs_pooled_grp_std_dev;
			for (const auto &i_pgrp : tmp_pool_grps)
			{
				const string &pool_group = i_pgrp.first;
				double var_sum = 0;
				long int weight_sum = 0;
				for (const auto &istat : i_pgrp.second)
				{
					long int weight = istat.comp_nsamples() - 1;
					var_sum += weight * istat.comp_var();
					weight_sum += weight;
				}
				if (weight_sum > 0)
				{
					obs_pooled_grp_std_dev[pool_group] = sqrt(var_sum / weight_sum);
				}
			}
			for (const auto &iobs : obs_name_vec)
			{
				const string &obs_name = iobs;
				const string &obs_group = obs_info_ptr->get_group(obs_name);
				auto it_pg = group_2_pool_group_map.find(obs_group);
				if (it_pg != group_2_pool_group_map.end())
				{
					const string &pool_group = it_pg->second;
					if (obs_pooled_grp_std_dev.find(pool_group) != obs_pooled_grp_std_dev.end())
					{
						obs_2_sen_weight[iobs] = obs_pooled_grp_std_dev[pool_group];
					}
				}
			}
		}

		//compute parameter standard deviations
		map<string, double> par_std_dev;
		par_std_dev = calc_parameter_unif_std_dev();
		obs_sen_file.calc_pooled_obs_sen(fout_mos, obs_2_sen_weight, par_std_dev);
		file_manager_ptr->close_file("mos");
	}
	file_manager_ptr->close_file("msn");
	file_manager_ptr->close_file("raw");
}

void MorrisMethod::calc_morris_obs(ostream &fout, MorrisObsSenFile &morris_sen_file)
{
	// write standard Morris Sensitivity
	fout << "observation_name,parameter_name,n_samples,sen_mean,sen_mean_abs,sen_std_dev" << endl;

	for (const auto &i_obs : morris_sen_file.obs_names_vec)
	{
		//fout << "Method of Morris for observation: " << i_obs << endl;
		//fout << "parameter_name, n_samples, sen_mean, sen_mean_abs, sen_std_dev" << endl;
		//fout << i_obs;
		for (const auto &i_par : morris_sen_file.par_names_vec)
		{
			const auto &it_senmap = morris_sen_file.map_obs_stats.find(make_pair(i_par, i_obs));
			if (it_senmap != morris_sen_file.map_obs_stats.end())
			{
				fout << i_obs <<"," << i_par << "," << it_senmap->second.comp_nsamples() << "," << it_senmap->second.comp_mean() << "," << it_senmap->second.comp_abs_mean() << "," << sqrt(it_senmap->second.comp_var()) << endl;
			}
		}
		//fout << endl;
	}
}

MorrisMethod::~MorrisMethod(void)
{
}


/*
 * Copyright 2010, 2011, 2012
 * François Bleibel,
 * Olivier Stasse,
 * Florent Lamiraux
 * Nicolas Mansard
 *
 * CNRS/AIST
 *
 */

/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */

/* --- SOT --- */
//#define VP_DEBUG
//#define VP_DEBUG_MODE 45
#include <dynamic-graph/command-bind.h>
#include <dynamic-graph/command-getter.h>
#include <dynamic-graph/command-setter.h>
#include <dynamic-graph/command.h>

#include <Eigen/LU>

#include <sot/core/debug.hh>
#include <sot/core/exception-feature.hh>
#include <sot/core/feature-point6d.hh>

using namespace std;
using namespace dynamicgraph;
using namespace dynamicgraph::sot;

#include <sot/core/factory.hh>
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePoint6d, "FeaturePoint6d");

/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */

const FeaturePoint6d::ComputationFrameType
    FeaturePoint6d::COMPUTATION_FRAME_DEFAULT = FRAME_DESIRED;

FeaturePoint6d::FeaturePoint6d(const string &pointName)
    : FeatureAbstract(pointName), computationFrame_(COMPUTATION_FRAME_DEFAULT),
      positionSIN(NULL, "sotFeaturePoint6d(" + name +
                            ")::input(matrixHomo)::position"),
      velocitySIN(NULL,
                  "sotFeaturePoint6d(" + name + ")::input(vector)::velocity"),
      articularJacobianSIN(NULL, "sotFeaturePoint6d(" + name +
                                     ")::input(matrix)::Jq"),
      error_th_(), R_(), Rref_(), Rt_(), Rreft_(), P_(3, 3), Pinv_(3, 3),
      accuracy_(1e-8) {
  jacobianSOUT.addDependency(positionSIN);
  jacobianSOUT.addDependency(articularJacobianSIN);

  errorSOUT.addDependency(positionSIN);

  signalRegistration(positionSIN << articularJacobianSIN);
  signalRegistration(errordotSOUT << velocitySIN);
  errordotSOUT.setFunction(
      boost::bind(&FeaturePoint6d::computeErrordot, this, _1, _2));
  errordotSOUT.addDependency(velocitySIN);
  errordotSOUT.addDependency(positionSIN);
  errordotSOUT.addDependency(errorSOUT);

  // Commands
  //
  {
    using namespace dynamicgraph::command;
    std::string docstring;
    // Set computation frame
    docstring = "Set computation frame\n"
                "\n"
                "  Input:\n"
                "    a string: 'current' or 'desired'.\n"
                "      If 'current', the error is defined as the rotation "
                "vector (VectorUTheta)\n"
                "      corresponding to the position of the reference in the "
                "current frame:\n"
                "                         -1 *\n"
                "        error = utheta (M  M )\n"
                "      If 'desired',      *-1\n"
                "        error = utheta (M   M)\n";
    addCommand("frame",
               new dynamicgraph::command::Setter<FeaturePoint6d, std::string>(
                   *this, &FeaturePoint6d::computationFrame, docstring));
    docstring = "Get frame of computation of the error\n"
                "\n"
                "  See command 'frame' for definition.\n";
    addCommand("getFrame",
               new dynamicgraph::command::Getter<FeaturePoint6d, std::string>(
                   *this, &FeaturePoint6d::computationFrame, docstring));
    addCommand(
        "keep",
        makeCommandVoid0(
            *this, &FeaturePoint6d::servoCurrentPosition,
            docCommandVoid0(
                "modify the desired position to servo at current pos.")));
  }
}

void FeaturePoint6d::addDependenciesFromReference(void) {
  assert(isReferenceSet());
  errorSOUT.addDependency(getReference()->positionSIN);
  jacobianSOUT.addDependency(getReference()->positionSIN);
}

void FeaturePoint6d::removeDependenciesFromReference(void) {
  assert(isReferenceSet());
  errorSOUT.removeDependency(getReference()->positionSIN);
  jacobianSOUT.removeDependency(getReference()->positionSIN);
}

/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
void FeaturePoint6d::computationFrame(const std::string &inFrame) {
  if (inFrame == "current")
    computationFrame_ = FRAME_CURRENT;
  else if (inFrame == "desired")
    computationFrame_ = FRAME_DESIRED;
  else {
    std::string msg("FeaturePoint6d::computationFrame: " + inFrame +
                    ": invalid argument,\n"
                    "expecting 'current' or 'desired'");
    throw ExceptionFeature(ExceptionFeature::GENERIC, msg);
  }
}

/// \brief Get computation frame
std::string FeaturePoint6d::computationFrame() const {
  switch (computationFrame_) {
  case FRAME_CURRENT:
    return "current";
  case FRAME_DESIRED:
    return "desired";
  }
  assert(false && "Case not handled");
  return "Case not handled";
}
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */

unsigned int &FeaturePoint6d::getDimension(unsigned int &dim, int time) {
  sotDEBUG(25) << "# In {" << endl;

  const Flags &fl = selectionSIN.access(time);

  dim = 0;
  for (int i = 0; i < 6; ++i)
    if (fl(i))
      dim++;

  sotDEBUG(25) << "# Out }" << endl;
  return dim;
}

/** Compute the interaction matrix from a subset of
 * the possible features.
 */
Matrix &FeaturePoint6d::computeJacobian(Matrix &J, int time) {
  sotDEBUG(15) << "# In {" << endl;

  const Matrix &Jq = articularJacobianSIN(time);
  const int &dim = dimensionSOUT(time);
  const Flags &fl = selectionSIN(time);

  sotDEBUG(25) << "dim = " << dimensionSOUT(time) << " time:" << time << " "
               << dimensionSOUT.getTime() << " " << dimensionSOUT.getReady()
               << endl;
  sotDEBUG(25) << "selec = " << selectionSIN(time) << " time:" << time << " "
               << selectionSIN.getTime() << " " << selectionSIN.getReady()
               << endl;

  sotDEBUG(15) << "Dimension=" << dim << std::endl;

  const Matrix::Index cJ = Jq.cols();
  J.resize(dim, cJ);
  Matrix LJq(6, cJ);

  if (FRAME_CURRENT == computationFrame_) {
    /* The Jacobian on rotation is equal to Jr = - hdRh Jr6d.
     * The Jacobian in translation is equalt to Jt = [hRw(wthd-wth)]x Jr - Jt.
     */
    const MatrixHomogeneous &wMh = positionSIN(time);
    MatrixRotation wRh;
    wRh = wMh.linear();
    MatrixRotation wRhd;
    Vector hdth(3), Rhdth(3);

    if (isReferenceSet()) {
      const MatrixHomogeneous &wMhd = getReference()->positionSIN(time);
      wRhd = wMhd.linear();
      for (unsigned int i = 0; i < 3; ++i)
        hdth(i) = wMhd(i, 3) - wMh(i, 3);
    } else {
      wRhd.setIdentity();
      for (unsigned int i = 0; i < 3; ++i)
        hdth(i) = -wMh(i, 3);
    }
    Rhdth = (wRh.inverse()) * hdth;
    MatrixRotation hdRh;
    hdRh = (wRhd.inverse()) * wRh;

    Matrix Lx(6, 6);
    for (unsigned int i = 0; i < 3; i++) {
      for (unsigned int j = 0; j < 3; j++) {
        if (i == j) {
          Lx(i, j) = -1;
        } else {
          Lx(i, j) = 0;
        }
        Lx(i + 3, j) = 0;
        Lx(i + 3, j + 3) = -hdRh(i, j);
      }
    }
    const double &X = Rhdth(0), &Y = Rhdth(1), &Z = Rhdth(2);
    Lx(0, 4) = -Z;
    Lx(0, 5) = Y;
    Lx(1, 3) = Z;
    Lx(1, 5) = -X;
    Lx(2, 3) = -Y;
    Lx(2, 4) = X;
    Lx(0, 3) = 0;
    Lx(1, 4) = 0;
    Lx(2, 5) = 0;
    sotDEBUG(15) << "Lx= " << Lx << endl;

    LJq = Lx * Jq;
  } else {
    /* The Jacobian in rotation is equal to Jr = hdJ = hdRh Jr.
     * The Jacobian in translation is equal to Jr = hdJ = hdRh Jr. */
    const MatrixHomogeneous &wMh = positionSIN(time);
    MatrixRotation wRh;
    wRh = wMh.linear();
    MatrixRotation hdRh;

    if (isReferenceSet()) {
      const MatrixHomogeneous &wMhd = getReference()->positionSIN(time);
      MatrixRotation wRhd;
      wRhd = wMhd.linear();
      hdRh = (wRhd.inverse()) * wRh;
    } else {
      hdRh = wRh;
    }

    LJq.fill(0);
    for (unsigned int i = 0; i < 3; i++)
      for (unsigned int j = 0; j < cJ; j++) {
        for (unsigned int k = 0; k < 3; k++) {
          LJq(i, j) += hdRh(i, k) * Jq(k, j);
          LJq(i + 3, j) += hdRh(i, k) * Jq(k + 3, j);
        }
      }
  }

  /* Select the active line of Jq. */
  unsigned int rJ = 0;
  for (unsigned int r = 0; r < 6; ++r)
    if (fl(r)) {
      for (unsigned int c = 0; c < cJ; ++c)
        J(rJ, c) = LJq(r, c);
      rJ++;
    }

  sotDEBUG(15) << "# Out }" << endl;
  return J;
}

#define SOT_COMPUTE_H1MH2(wMh, wMhd, hMhd)                                     \
  {                                                                            \
    MatrixHomogeneous hMw;                                                     \
    hMw = wMh.inverse(Eigen::Affine);                                          \
    sotDEBUG(15) << "hMw = " << hMw << endl;                                   \
    hMhd = hMw * wMhd;                                                         \
    sotDEBUG(15) << "hMhd = " << hMhd << endl;                                 \
  }

/** Compute the error between two visual features from a subset
 * a the possible features.
 */
Vector &FeaturePoint6d::computeError(Vector &error, int time) {
  sotDEBUGIN(15);

  const Flags &fl = selectionSIN(time);
  const MatrixHomogeneous &wMh = positionSIN(time);
  sotDEBUG(15) << "wMh = " << wMh << endl;

  /* Computing only translation:                                        *
   * trans( hMw wMhd ) = htw + hRw wthd                                 *
   *                   = -hRw wth + hrW wthd                            *
   *                   = hRw ( wthd - wth )                             *
   * The second line is obtained by writting hMw as the inverse of wMh. */

  MatrixHomogeneous hMhd;
  if (isReferenceSet()) {
    const MatrixHomogeneous &wMhd = getReference()->positionSIN(time);
    sotDEBUG(15) << "wMhd = " << wMhd << endl;
    switch (computationFrame_) {
    case FRAME_CURRENT:
      SOT_COMPUTE_H1MH2(wMh, wMhd, hMhd);
      break;
    case FRAME_DESIRED:
      SOT_COMPUTE_H1MH2(wMhd, wMh, hMhd);
      break; // Compute hdMh indeed.
    };
  } else {
    switch (computationFrame_) {
    case FRAME_CURRENT:
      hMhd = wMh.inverse();
      break;
    case FRAME_DESIRED:
      hMhd = wMh;
      break; // Compute hdMh indeed.
    };
  }

  sotDEBUG(25) << "dim = " << dimensionSOUT(time) << " time:" << time << " "
               << dimensionSOUT.getTime() << " " << dimensionSOUT.getReady()
               << endl;
  sotDEBUG(25) << "selec = " << selectionSIN(time) << " time:" << time << " "
               << selectionSIN.getTime() << " " << selectionSIN.getReady()
               << endl;

  error.resize(dimensionSOUT(time));
  unsigned int cursor = 0;
  for (unsigned int i = 0; i < 3; ++i) {
    if (fl(i))
      error(cursor++) = hMhd(i, 3);
  }

  if (fl(3) || fl(4) || fl(5)) {
    MatrixRotation hRhd;
    hRhd = hMhd.linear();
    error_th_.fromRotationMatrix(hRhd);
    for (unsigned int i = 0; i < 3; ++i) {
      if (fl(i + 3))
        error(cursor++) = error_th_.angle() * error_th_.axis()(i);
    }
  }

  sotDEBUGOUT(15);
  return error;
}

void FeaturePoint6d::inverseJacobianRodrigues() {
  const double &r1 = error_th_.angle() * error_th_.axis()(0);
  const double &r2 = error_th_.angle() * error_th_.axis()(1);
  const double &r3 = error_th_.angle() * error_th_.axis()(2);
  double r1_2 = r1 * r1;
  double r2_2 = r2 * r2;
  double r3_2 = r3 * r3;
  double r1_3 = r1 * r1_2;
  double r2_3 = r2 * r2_2;
  double r3_3 = r3 * r3_2;
  double r1_4 = r1_2 * r1_2;
  double r2_4 = r2_2 * r2_2;
  double r3_4 = r3_2 * r3_2;
  double norm_2 = r3_2 + r2_2 + r1_2;

  if (norm_2 < accuracy_) {
    P_.setIdentity();
  } else {
    // This code has been generated by maxima software
    P_(0, 0) =
        ((r3_2 + r2_2) * sqrt(norm_2) * sin(sqrt(norm_2)) + r1_2 * r3_2 +
         r1_2 * r2_2 + r1_4) /
        (r3_4 + (2 * r2_2 + 2 * r1_2) * r3_2 + r2_4 + 2 * r1_2 * r2_2 + r1_4);
    P_(0, 1) =
        -(r1 * r2 * sqrt(norm_2) * sin(sqrt(norm_2)) +
          (r3_3 + (r2_2 + r1_2) * r3) * cos(sqrt(norm_2)) - r3_3 -
          r1 * r2 * r3_2 + (-r2_2 - r1_2) * r3 - r1 * r2_3 - r1_3 * r2) /
        (r3_4 + (2 * r2_2 + 2 * r1_2) * r3_2 + r2_4 + 2 * r1_2 * r2_2 + r1_4);
    P_(0, 2) =
        -(r1 * r3 * sqrt(norm_2) * sin(sqrt(norm_2)) +
          (-r2 * r3_2 - r2_3 - r1_2 * r2) * cos(sqrt(norm_2)) - r1 * r3_3 +
          r2 * r3_2 + (-r1 * r2_2 - r1_3) * r3 + r2_3 + r1_2 * r2) /
        (r3_4 + (2 * r2_2 + 2 * r1_2) * r3_2 + r2_4 + 2 * r1_2 * r2_2 + r1_4);
    P_(1, 0) =
        -(r1 * r2 * sqrt(norm_2) * sin(sqrt(norm_2)) +
          ((-r2_2 - r1_2) * r3 - r3_3) * cos(sqrt(norm_2)) + r3_3 -
          r1 * r2 * r3_2 + (r2_2 + r1_2) * r3 - r1 * r2_3 - r1_3 * r2) /
        (r3_4 + (2 * r2_2 + 2 * r1_2) * r3_2 + r2_4 + 2 * r1_2 * r2_2 + r1_4);
    P_(1, 1) =
        ((r3_2 + r1_2) * sqrt(norm_2) * sin(sqrt(norm_2)) + r2_2 * r3_2 + r2_4 +
         r1_2 * r2_2) /
        (r3_4 + (2 * r2_2 + 2 * r1_2) * r3_2 + r2_4 + 2 * r1_2 * r2_2 + r1_4);
    P_(1, 2) =
        -(r2 * r3 * sqrt(norm_2) * sin(sqrt(norm_2)) +
          (r1 * r3_2 + r1 * r2_2 + r1_3) * cos(sqrt(norm_2)) - r2 * r3_3 -
          r1 * r3_2 + (-r2_3 - r1_2 * r2) * r3 - r1 * r2_2 - r1_3) /
        (r3_4 + (2 * r2_2 + 2 * r1_2) * r3_2 + r2_4 + 2 * r1_2 * r2_2 + r1_4);
    P_(2, 0) =
        -(r1 * r3 * sqrt(norm_2) * sin(sqrt(norm_2)) +
          (r2 * r3_2 + r2_3 + r1_2 * r2) * cos(sqrt(norm_2)) - r1 * r3_3 -
          r2 * r3_2 + (-r1 * r2_2 - r1_3) * r3 - r2_3 - r1_2 * r2) /
        (r3_4 + (2 * r2_2 + 2 * r1_2) * r3_2 + r2_4 + 2 * r1_2 * r2_2 + r1_4);
    P_(2, 1) =
        -(r2 * r3 * sqrt(norm_2) * sin(sqrt(norm_2)) +
          (-r1 * r3_2 - r1 * r2_2 - r1_3) * cos(sqrt(norm_2)) - r2 * r3_3 +
          r1 * r3_2 + (-r2_3 - r1_2 * r2) * r3 + r1 * r2_2 + r1_3) /
        (r3_4 + (2 * r2_2 + 2 * r1_2) * r3_2 + r2_4 + 2 * r1_2 * r2_2 + r1_4);
    P_(2, 2) =
        ((r2_2 + r1_2) * sqrt(norm_2) * sin(sqrt(norm_2)) + r3_4 +
         (r2_2 + r1_2) * r3_2) /
        (r3_4 + (2 * r2_2 + 2 * r1_2) * r3_2 + r2_4 + 2 * r1_2 * r2_2 + r1_4);
  }
  Pinv_ = P_.inverse();
}

Vector &FeaturePoint6d::computeErrordot(Vector &errordot, int time) {
  if (isReferenceSet()) {
    const Vector &velocity = getReference()->velocitySIN(time);
    const MatrixHomogeneous &M = positionSIN(time);
    const MatrixHomogeneous &Mref = getReference()->positionSIN(time);
    // Linear velocity if the reference frame
    v_(0) = velocity(0);
    v_(1) = velocity(1);
    v_(2) = velocity(2);
    // Angular velocity if the reference frame
    omega_(0) = velocity(3);
    omega_(1) = velocity(4);
    omega_(2) = velocity(5);
    R_ = M.linear();
    t_ = M.translation();
    Rt_ = R_.transpose();
    Rref_ = Mref.linear();
    tref_ = Mref.translation();
    Rreft_ = Rref_.transpose();
    errorSOUT.recompute(time);
    inverseJacobianRodrigues();
    switch (computationFrame_) {
    case FRAME_CURRENT:
      // \dot{e}_{t} = R^{T} v
      errordot_t_ = Rt_ * v_;
      // \dot{e}_{\theta} = P^{-1}(e_{theta})R^{*T}\omega
      Rreftomega_ = Rreft_ * omega_;
      errordot_th_ = Pinv_ * Rreftomega_;
      break;
    case FRAME_DESIRED:
      errordot_t_ = Rreft_ * (omega_.cross(tref_ - t_) - v_);
      errordot_th_ = -Pinv_ * (Rt_ * omega_);
      break;
    }
  } else {
    errordot_t_.setZero();
    errordot_th_.setZero();
  }

  const Flags &fl = selectionSIN(time);
  errordot.resize(dimensionSOUT(time));
  unsigned int cursor = 0;
  for (unsigned int i = 0; i < 3; ++i) {
    if (fl(i)) {
      errordot(cursor++) = errordot_t_(i);
    }
  }

  if (fl(3) || fl(4) || fl(5)) {
    for (unsigned int i = 0; i < 3; ++i) {
      if (fl(i + 3)) {
        errordot(cursor++) = errordot_th_(i);
      }
    }
  }

  return errordot;
}

/* Modify the value of the reference (sdes) so that it corresponds
 * to the current position. The effect on the servo is to maintain the
 * current position and correct any drift. */
void FeaturePoint6d::servoCurrentPosition(void) {
  sotDEBUGIN(15);

  if (!isReferenceSet()) {
    sotERROR << "The reference is not set, this function should not be called"
             << std::endl;
    throw ExceptionFeature(
        ExceptionFeature::GENERIC,
        "The reference is not set, this function should not be called");
  }
  getReference()->positionSIN = positionSIN.accessCopy();

  sotDEBUGOUT(15);
}

static const char *featureNames[] = {"X ", "Y ", "Z ", "RX", "RY", "RZ"};
void FeaturePoint6d::display(std::ostream &os) const {
  os << "Point6d <" << name << ">: (";

  try {
    const Flags &fl = selectionSIN.accessCopy();
    bool first = true;
    for (int i = 0; i < 6; ++i)
      if (fl(i)) {
        if (first) {
          first = false;
        } else {
          os << ",";
        }
        os << featureNames[i];
      }
    os << ") ";
  } catch (ExceptionAbstract e) {
    os << " selectSIN not set.";
  }
}


#include <vector>
#include <typeinfo>
// For External Library
#include <torch/torch.h>
#include <boost/program_options.hpp>
// For Original Header
#include "networks.hpp"

// Define Namespace
namespace nn = torch::nn;
namespace F = torch::nn::functional;
namespace po = boost::program_options;

// ----------------------------------------------------------------------
// struct{VariationalAutoEncoderImpl}(nn::Module) -> constructor
// ----------------------------------------------------------------------
VariationalAutoEncoderImpl::VariationalAutoEncoderImpl(po::variables_map &vm){

    size_t feature = vm["nf"].as<size_t>();

    DownSampling(this->encoder, vm["nc"].as<size_t>(), feature, /*BN=*/false, /*ReLU=*/true);     // {C,256,256} ===> {F,128,128}
    DownSampling(this->encoder, feature, feature*2, /*BN=*/true, /*ReLU=*/true);                  // {F,128,128} ===> {2F,64,64}
    DownSampling(this->encoder, feature*2, feature*4, /*BN=*/true, /*ReLU=*/true);                // {2F,64,64}  ===> {4F,32,32}
    DownSampling(this->encoder, feature*4, feature*8, /*BN=*/true, /*ReLU=*/true);                // {4F,32,32}  ===> {8F,16,16}
    DownSampling(this->encoder, feature*8, feature*8, /*BN=*/true, /*ReLU=*/true);                // {8F,16,16}  ===> {8F,8,8}
    register_module("encoder", this->encoder);

    DownSampling(this->encoder_mean, feature*8, vm["nz"].as<size_t>(), /*BN=*/false, /*ReLU=*/false);  // {8F,8,8}    ===> {Z,4,4}
    DownSampling(this->encoder_var, feature*8, vm["nz"].as<size_t>(), /*BN=*/false, /*ReLU=*/false);   // {8F,8,8}    ===> {Z,4,4}
    register_module("encoder_mean", this->encoder_mean);
    register_module("encoder_var", this->encoder_var);

    UpSampling(this->decoder, vm["nz"].as<size_t>(), feature*8, /*BN=*/true, /*ReLU=*/true);      // {Z,4,4}     ===> {8F,8,8}
    UpSampling(this->decoder, feature*8, feature*8, /*BN=*/true, /*ReLU=*/true);                  // {8F,8,8}    ===> {8F,16,16}
    UpSampling(this->decoder, feature*8, feature*4, /*BN=*/true, /*ReLU=*/true);                  // {8F,16,16}  ===> {4F,32,32}
    UpSampling(this->decoder, feature*4, feature*2, /*BN=*/true, /*ReLU=*/true);                  // {4F,32,32}  ===> {2F,64,64}
    UpSampling(this->decoder, feature*2, feature, /*BN=*/true, /*ReLU=*/true);                    // {2F,64,64}  ===> {F,128,128}
    UpSampling(this->decoder, feature, vm["nc"].as<size_t>(), /*BN=*/false, /*ReLU=*/false);      // {F,128,128} ===> {C,256,256}
    this->decoder->push_back(nn::Tanh());                                                         // [-inf,+inf] ===> [-1,1]
    register_module("decoder", this->decoder);

}

// ----------------------------------------------------------------------
// struct{VariationalAutoEncoderImpl}(nn::Module) -> function{sampling}
// ----------------------------------------------------------------------
torch::Tensor VariationalAutoEncoderImpl::sampling(torch::Tensor &mean, torch::Tensor &var){
    torch::Tensor eps = torch::randn({mean.size(0), mean.size(1), mean.size(2), mean.size(3)}).to(mean.device());
    torch::Tensor z = mean + torch::sqrt(var) * eps;
    return z;
}

// ----------------------------------------------------------------------
// struct{VariationalAutoEncoderImpl}(nn::Module) -> function{forward}
// ----------------------------------------------------------------------
torch::Tensor VariationalAutoEncoderImpl::forward(torch::Tensor x){
    torch::Tensor mid = this->encoder->forward(x);    // {C,256,256} ===> {Z,8,8}
    this->mean_keep = this->encoder_mean->forward(mid);
    this->var_keep = F::softplus(this->encoder_var->forward(mid));
    this->z_keep = this->sampling(this->mean_keep, this->var_keep);
    torch::Tensor out = this->decoder->forward(this->z_keep);  // {Z,4,4} ===> {C,256,256}
    return out;
}

// ----------------------------------------------------------------------
// struct{VariationalAutoEncoderImpl}(nn::Module) -> function{forward_z}
// ----------------------------------------------------------------------
torch::Tensor VariationalAutoEncoderImpl::forward_z(torch::Tensor z){
    torch::Tensor out = this->decoder->forward(z);  // {Z,4,4} ===> {C,256,256}
    return out;
}

// -----------------------------------------------------------------------------
// struct{VariationalAutoEncoderImpl}(nn::Module) -> function{kld_just_before}
// -----------------------------------------------------------------------------
torch::Tensor VariationalAutoEncoderImpl::kld_just_before(){
    torch::Tensor kld = - 0.5 * torch::mean(1.0 + torch::log(this->var_keep) - this->mean_keep * this->mean_keep - this->var_keep);
    return kld;
}

// -----------------------------------------------------------------------------
// struct{VariationalAutoEncoderImpl}(nn::Module) -> function{get_z_shape}
// -----------------------------------------------------------------------------
std::vector<long int> VariationalAutoEncoderImpl::get_z_shape(const std::vector<long int> x_shape, torch::Device &device){
    torch::Tensor x = torch::full(x_shape, /*value=*/0.0, torch::TensorOptions().dtype(torch::kFloat)).to(device);
    torch::Tensor mid = this->encoder->forward(x);    // {C,256,256} ===> {Z,8,8}
    torch::Tensor mean = this->encoder_mean->forward(mid);
    torch::Tensor var = F::softplus(this->encoder_var->forward(mid));
    torch::Tensor z = this->sampling(mean, var);
    std::vector<long int> z_shape = {z.size(0), z.size(1), z.size(2), z.size(3)};
    return z_shape;
}

// ----------------------------
// function{weights_init}
// ----------------------------
void weights_init(nn::Module &m){
    if ((typeid(m) == typeid(nn::Conv2d)) || (typeid(m) == typeid(nn::Conv2dImpl)) || (typeid(m) == typeid(nn::ConvTranspose2d)) || (typeid(m) == typeid(nn::ConvTranspose2dImpl))) {
        auto p = m.named_parameters(false);
        auto w = p.find("weight");
        auto b = p.find("bias");
        if (w != nullptr) nn::init::normal_(*w, /*mean=*/0.0, /*std=*/0.02);
        if (b != nullptr) nn::init::constant_(*b, /*bias=*/0.0);
    }
    else if ((typeid(m) == typeid(nn::BatchNorm2d)) || (typeid(m) == typeid(nn::BatchNorm2dImpl))){
        auto p = m.named_parameters(false);
        auto w = p.find("weight");
        auto b = p.find("bias");
        if (w != nullptr) nn::init::normal_(*w, /*mean=*/1.0, /*std=*/0.02);
        if (b != nullptr) nn::init::constant_(*b, /*bias=*/0.0);
    }
    return;
}

// ----------------------------
// function{DownSampling}
// ----------------------------
void DownSampling(nn::Sequential &sq, const size_t in_nc, const size_t out_nc, const bool BN, const bool ReLU, const bool bias){
    sq->push_back(nn::Conv2d(nn::Conv2dOptions(in_nc, out_nc, 4).stride(2).padding(1).bias(bias)));
    if (BN){
        sq->push_back(nn::BatchNorm2d(out_nc));
    }
    if (ReLU){
        sq->push_back(nn::ReLU(nn::ReLUOptions().inplace(true)));
    }
    return;
}

// ----------------------------
// function{UpSampling}
// ----------------------------
void UpSampling(nn::Sequential &sq, const size_t in_nc, const size_t out_nc, const bool BN, const bool ReLU, const bool bias){
    sq->push_back(nn::ConvTranspose2d(nn::ConvTranspose2dOptions(in_nc, out_nc, 4).stride(2).padding(1).bias(bias)));
    if (BN){
        sq->push_back(nn::BatchNorm2d(out_nc));
    }
    if (ReLU){
        sq->push_back(nn::ReLU(nn::ReLUOptions().inplace(true)));
    }
    return;
}