/* ----------------------------------------------------------------------------

* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)

* See LICENSE for the license information
* -------------------------------------------------------------------------- */

/**
* @file    SolverComparer.cpp
* @brief   Incremental and batch solving, timing, and accuracy comparisons
* @author  Richard Roberts
* @date    August, 2013
*
* Here is an example. Below, to run in batch mode, we first generate an initialization in incremental mode.
*
* Solve in incremental and write to file w_inc:
* ./SolverComparer --incremental -d w10000 -o w_inc
*
* You can then perturb that initialization to get batch something to optimize.
* Read in w_inc, perturb it with noise of stddev 0.6, and write to w_pert:
* ./SolverComparer --perturb 0.6 -i w_inc -o w_pert
*
* Then optimize with batch, read in w_pert, solve in batch, and write to w_batch:
* ./SolverComparer --batch -d w10000 -i w_pert -o w_batch
*
* And finally compare solutions in w_inc and w_batch to check that batch converged to the global minimum
* ./SolverComparer --compare w_inc w_batch
*
*/

#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/sam/BearingRangeFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/linear/GaussianJunctionTree.h>
#include <gtsam/linear/GaussianEliminationTree.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/timing.h>
#include <gtsam/base/treeTraversal-inst.h>
#include <gtsam/config.h> // for GTSAM_USE_TBB

#include <boost/archive/binary_iarchive.hpp>
#include <boost/archive/binary_oarchive.hpp>
#include <boost/program_options.hpp>
#include <boost/range/algorithm/set_algorithm.hpp>
#include <boost/range/adaptor/reversed.hpp>
#include <boost/random.hpp>
#include <boost/serialization/export.hpp>

#include <fstream>
#include <iostream>

#ifdef GTSAM_USE_TBB
#include <tbb/tbb.h>
#undef max // TBB seems to include windows.h and we don't want these macros
#undef min
#endif

using namespace std;
using namespace gtsam;
using namespace gtsam::symbol_shorthand;
namespace po = boost::program_options;
namespace br = boost::range;

typedef Pose2 Pose;

typedef NoiseModelFactor1<Pose> NM1;
typedef NoiseModelFactor2<Pose,Pose> NM2;
typedef BearingRangeFactor<Pose,Point2> BR;

double chi2_red(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config) {
  // Compute degrees of freedom (observations - variables)
  // In ocaml, +1 was added to the observations to account for the prior, but
  // the factor graph already includes a factor for the prior/equality constraint.
  //  double dof = graph.size() - config.size();
  int graph_dim = 0;
  for(const boost::shared_ptr<gtsam::NonlinearFactor>& nlf: graph) {
    graph_dim += (int)nlf->dim();
  }
  double dof = double(graph_dim) - double(config.dim()); // kaess: changed to dim
  return 2. * graph.error(config) / dof; // kaess: added factor 2, graph.error returns half of actual error
}

// Global variables (these are only set once at program startup and never modified after)
string outputFile;
string inputFile;
string datasetName;
int firstStep;
int lastStep;
int nThreads;
int relinSkip;
bool incremental;
bool dogleg;
bool batch;
bool compare;
bool perturb;
bool stats;
double perturbationNoise;
string compareFile1, compareFile2;

Values initial;
NonlinearFactorGraph datasetMeasurements;

// Run functions for each mode
void runIncremental();
void runBatch();
void runCompare();
void runPerturb();
void runStats();

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

  po::options_description desc("Available options");
  desc.add_options()
    ("help", "Print help message")
    ("write-solution,o", po::value<string>(&outputFile)->default_value(""), "Write graph and solution to the specified file")
    ("read-solution,i", po::value<string>(&inputFile)->default_value(""), "Read graph and solution from the specified file")
    ("dataset,d", po::value<string>(&datasetName)->default_value(""), "Read a dataset file (if and only if --incremental is used)")
    ("first-step,f", po::value<int>(&firstStep)->default_value(0), "First step to process from the dataset file")
    ("last-step,l", po::value<int>(&lastStep)->default_value(-1), "Last step to process, or -1 to process until the end of the dataset")
    ("threads", po::value<int>(&nThreads)->default_value(-1), "Number of threads, or -1 to use all processors")
    ("relinSkip", po::value<int>(&relinSkip)->default_value(10), "Fluid relinearization check every arg steps")
    ("incremental", "Run in incremental mode using ISAM2 (default)")
    ("dogleg", "When in incremental mode, solve with Dogleg instead of Gauss-Newton in iSAM2")
    ("batch", "Run in batch mode, requires an initialization from --read-solution")
    ("compare", po::value<vector<string> >()->multitoken(), "Compare two solution files")
    ("perturb", po::value<double>(&perturbationNoise), "Perturb a solution file with the specified noise")
    ("stats", "Gather factorization statistics about the dataset, writes text-file histograms")
    ;
  po::variables_map vm;
  po::store(po::command_line_parser(argc, argv).options(desc).run(), vm);
  po::notify(vm);

  batch = (vm.count("batch") > 0);
  compare = (vm.count("compare") > 0);
  perturb = (vm.count("perturb") > 0);
  stats = (vm.count("stats") > 0);
  const int modesSpecified = int(batch) + int(compare) + int(perturb) + int(stats);
  incremental = (vm.count("incremental") > 0 || modesSpecified == 0);
  dogleg = (vm.count("dogleg") > 0);
  if(compare) {
    const vector<string>& compareFiles = vm["compare"].as<vector<string> >();
    if(compareFiles.size() != 2) {
      cout << "Must specify two files with --compare";
      exit(1);
    }
    compareFile1 = compareFiles[0];
    compareFile2 = compareFiles[1];
  }

  if(modesSpecified > 1) {
    cout << "Only one of --incremental, --batch, --compare, --perturb, and --stats may be specified\n" << desc << endl;
    exit(1);
  }
  if((incremental || batch) && datasetName.empty()) {
    cout << "In incremental and batch modes, a dataset must be specified\n" << desc << endl;
    exit(1);
  }
  if(!(incremental || batch || stats) && !datasetName.empty()) {
    cout << "A dataset may only be specified in incremental or batch modes\n" << desc << endl;
    exit(1);
  }
  if(batch && inputFile.empty()) {
    cout << "In batch model, an input file must be specified\n" << desc << endl;
    exit(1);
  }
  if(perturb && (inputFile.empty() || outputFile.empty())) {
    cout << "In perturb mode, specify input and output files\n" << desc << endl;
    exit(1);
  }
  if(stats && (datasetName.empty() || inputFile.empty())) {
    cout << "In stats mode, specify dataset and input file\n" << desc << endl;
    exit(1);
  }

  // Read input file
  if(!inputFile.empty())
  {
    cout << "Reading input file " << inputFile << endl;
    std::ifstream readerStream(inputFile.c_str(), ios::binary);
    boost::archive::binary_iarchive reader(readerStream);
    reader >> initial;
  }

  // Read dataset
  if(!datasetName.empty())
  {
    cout << "Loading dataset " << datasetName << endl;
    try {
      string datasetFile = findExampleDataFile(datasetName);
      std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> data =
        load2D(datasetFile);
      datasetMeasurements = *data.first;
    } catch(std::exception& e) {
      cout << e.what() << endl;
      exit(1);
    }
  }

#ifdef GTSAM_USE_TBB
  std::unique_ptr<tbb::task_scheduler_init> init;
  if(nThreads > 0) {
    cout << "Using " << nThreads << " threads" << endl;
    init.reset(new tbb::task_scheduler_init(nThreads));
  } else
    cout << "Using threads for all processors" << endl;
#else
  if(nThreads > 0) {
    std::cout << "GTSAM is not compiled with TBB, so threading is disabled and the --threads option cannot be used." << endl;
    exit(1);
  }
#endif

  // Run mode
  if(incremental)
    runIncremental();
  else if(batch)
    runBatch();
  else if(compare)
    runCompare();
  else if(perturb)
    runPerturb();
  else if(stats)
    runStats();

  return 0;
}

/* ************************************************************************* */
void runIncremental()
{
  ISAM2Params params;
  if(dogleg)
    params.optimizationParams = ISAM2DoglegParams();
  params.relinearizeSkip = relinSkip;
  params.enablePartialRelinearizationCheck = true;
  ISAM2 isam2(params);

  // Look for the first measurement to use
  cout << "Looking for first measurement from step " << firstStep << endl;
  size_t nextMeasurement = 0;
  bool havePreviousPose = false;
  Key firstPose;
  while(nextMeasurement < datasetMeasurements.size())
  {
    if(BetweenFactor<Pose>::shared_ptr factor =
      boost::dynamic_pointer_cast<BetweenFactor<Pose> >(datasetMeasurements[nextMeasurement]))
    {
      Key key1 = factor->key1(), key2 = factor->key2();
      if(((int)key1 >= firstStep && key1 < key2) || ((int)key2 >= firstStep && key2 < key1)) {
        // We found an odometry starting at firstStep
        firstPose = std::min(key1, key2);
        break;
      }
      if(((int)key2 >= firstStep && key1 < key2) || ((int)key1 >= firstStep && key2 < key1)) {
        // We found an odometry joining firstStep with a previous pose
        havePreviousPose = true;
        firstPose = std::max(key1, key2);
        break;
      }
    }
    ++ nextMeasurement;
  }

  if(nextMeasurement == datasetMeasurements.size()) {
    cout << "The supplied first step is past the end of the dataset" << endl;
    exit(1);
  }

  // If we didn't find an odometry linking to a previous pose, create a first pose and a prior
  if(!havePreviousPose) {
    cout << "Looks like " << firstPose << " is the first time step, so adding a prior on it" << endl;
    NonlinearFactorGraph newFactors;
    Values newVariables;

    newFactors.push_back(boost::make_shared<PriorFactor<Pose> >(firstPose, Pose(), noiseModel::Unit::Create(3)));
    newVariables.insert(firstPose, Pose());

    isam2.update(newFactors, newVariables);
  }

  cout << "Playing forward time steps..." << endl;

  for (size_t step = firstPose;
      nextMeasurement < datasetMeasurements.size() && (lastStep == -1 || (int)step <= lastStep);
      ++step)
  {
    Values newVariables;
    NonlinearFactorGraph newFactors;

    // Collect measurements and new variables for the current step
    gttic_(Collect_measurements);
    while(nextMeasurement < datasetMeasurements.size()) {

      NonlinearFactor::shared_ptr measurementf = datasetMeasurements[nextMeasurement];

      if(BetweenFactor<Pose>::shared_ptr factor =
        boost::dynamic_pointer_cast<BetweenFactor<Pose> >(measurementf))
      {
        // Stop collecting measurements that are for future steps
        if(factor->key1() > step || factor->key2() > step)
          break;

        // Require that one of the nodes is the current one
        if(factor->key1() != step && factor->key2() != step)
          throw runtime_error("Problem in data file, out-of-sequence measurements");

        // Add a new factor
        newFactors.push_back(factor);
        const auto& measured = factor->measured();

        // Initialize the new variable
        if(factor->key1() > factor->key2()) {
          if(!newVariables.exists(factor->key1())) { // Only need to check newVariables since loop closures come after odometry
            if(step == 1)
              newVariables.insert(factor->key1(), measured.inverse());
            else {
              Pose prevPose = isam2.calculateEstimate<Pose>(factor->key2());
              newVariables.insert(factor->key1(), prevPose * measured.inverse());
            }
          }
        } else {
          if(!newVariables.exists(factor->key2())) { // Only need to check newVariables since loop closures come after odometry
            if(step == 1)
              newVariables.insert(factor->key2(), measured);
            else {
              Pose prevPose = isam2.calculateEstimate<Pose>(factor->key1());
              newVariables.insert(factor->key2(), prevPose * measured);
            }
          }
        }
      }
      else if(BearingRangeFactor<Pose, Point2>::shared_ptr factor =
        boost::dynamic_pointer_cast<BearingRangeFactor<Pose, Point2> >(measurementf))
      {
        Key poseKey = factor->keys()[0], lmKey = factor->keys()[1];

        // Stop collecting measurements that are for future steps
        if(poseKey > step)
          throw runtime_error("Problem in data file, out-of-sequence measurements");

        // Add new factor
        newFactors.push_back(factor);

        // Initialize new landmark
        if(!isam2.getLinearizationPoint().exists(lmKey))
        {
          Pose pose;
          if(isam2.getLinearizationPoint().exists(poseKey))
            pose = isam2.calculateEstimate<Pose>(poseKey);
          else
            pose = newVariables.at<Pose>(poseKey);
          const auto& measured = factor->measured();
          Rot2 measuredBearing = measured.bearing();
          double measuredRange = measured.range();
          newVariables.insert(lmKey,
            pose.transform_from(measuredBearing.rotate(Point2(measuredRange, 0.0))));
        }
      }
      else
      {
        throw std::runtime_error("Unknown factor type read from data file");
      }
      ++ nextMeasurement;
    }
    gttoc_(Collect_measurements);

    // Update iSAM2
    try {
      gttic_(Update_ISAM2);
      isam2.update(newFactors, newVariables);
      gttoc_(Update_ISAM2);
    } catch(std::exception& e) {
      cout << e.what() << endl;
      exit(1);
    }

    if((step - firstPose) % 1000 == 0) {
      try {
        gttic_(chi2);
        Values estimate(isam2.calculateEstimate());
        double chi2 = chi2_red(isam2.getFactorsUnsafe(), estimate);
        cout << "chi2 = " << chi2 << endl;
        gttoc_(chi2);
      } catch(std::exception& e) {
        cout << e.what() << endl;
        exit(1);
      }
    }

    tictoc_finishedIteration_();

    if((step - firstPose) % 1000 == 0) {
      cout << "Step " << step << endl;
      tictoc_print_();
    }
  }

  if(!outputFile.empty())
  {
    try {
      cout << "Writing output file " << outputFile << endl;
      std::ofstream writerStream(outputFile.c_str(), ios::binary);
      boost::archive::binary_oarchive writer(writerStream);
      Values estimates = isam2.calculateEstimate();
      writer << estimates;
    } catch(std::exception& e) {
      cout << e.what() << endl;
      exit(1);
    }
  }

  tictoc_print_();

  // Compute marginals
  //try {
  //  Marginals marginals(graph, values);
  //  int i=0;
  //  for (Key key1: boost::adaptors::reverse(values.keys())) {
  //    int j=0;
  //    for (Key key12: boost::adaptors::reverse(values.keys())) {
  //      if(i != j) {
  //        gttic_(jointMarginalInformation);
  //        KeyVector keys(2);
  //        keys[0] = key1;
  //        keys[1] = key2;
  //        JointMarginal info = marginals.jointMarginalInformation(keys);
  //        gttoc_(jointMarginalInformation);
  //        tictoc_finishedIteration_();
  //      }
  //      ++j;
  //      if(j >= 50)
  //        break;
  //    }
  //    ++i;
  //    if(i >= 50)
  //      break;
  //  }
  //  tictoc_print_();
  //  for(Key key: values.keys()) {
  //    gttic_(marginalInformation);
  //    Matrix info = marginals.marginalInformation(key);
  //    gttoc_(marginalInformation);
  //    tictoc_finishedIteration_();
  //    ++i;
  //  }
  //} catch(std::exception& e) {
  //  cout << e.what() << endl;
  //}
  //tictoc_print_();
}

/* ************************************************************************* */
void runBatch()
{
  cout << "Creating batch optimizer..." << endl;

  NonlinearFactorGraph measurements = datasetMeasurements;
  measurements.push_back(boost::make_shared<PriorFactor<Pose> >(0, Pose(), noiseModel::Unit::Create(3)));

  gttic_(Create_optimizer);
  GaussNewtonParams params;
  params.linearSolverType = NonlinearOptimizerParams::MULTIFRONTAL_CHOLESKY;
  GaussNewtonOptimizer optimizer(measurements, initial, params);
  gttoc_(Create_optimizer);
  double lastError;
  do {
    lastError = optimizer.error();
    gttic_(Iterate_optimizer);
    optimizer.iterate();
    gttoc_(Iterate_optimizer);
    cout << "Error: " << lastError << " -> " << optimizer.error() /*<< ", lambda: " << optimizer.lambda()*/ << endl;
    gttic_(chi2);
    double chi2 = chi2_red(measurements, optimizer.values());
    cout << "chi2 = " << chi2 << endl;
    gttoc_(chi2);
  } while(!checkConvergence(optimizer.params().relativeErrorTol,
    optimizer.params().absoluteErrorTol, optimizer.params().errorTol,
    lastError, optimizer.error(), optimizer.params().verbosity));

  tictoc_finishedIteration_();
  tictoc_print_();

  if(!outputFile.empty())
  {
    try {
      cout << "Writing output file " << outputFile << endl;
      std::ofstream writerStream(outputFile.c_str(), ios::binary);
      boost::archive::binary_oarchive writer(writerStream);
      writer << optimizer.values();
    } catch(std::exception& e) {
      cout << e.what() << endl;
      exit(1);
    }
  }
}

/* ************************************************************************* */
void runCompare()
{
  Values soln1, soln2;

  cout << "Reading solution file " << compareFile1 << endl;
  {
    std::ifstream readerStream(compareFile1.c_str(), ios::binary);
    boost::archive::binary_iarchive reader(readerStream);
    reader >> soln1;
  }

  cout << "Reading solution file " << compareFile2 << endl;
  {
    std::ifstream readerStream(compareFile2.c_str(), ios::binary);
    boost::archive::binary_iarchive reader(readerStream);
    reader >> soln2;
  }

  // Check solution for equality
  cout << "Comparing solutions..." << endl;
  KeyVector missingKeys;
  br::set_symmetric_difference(soln1.keys(), soln2.keys(), std::back_inserter(missingKeys));
  if(!missingKeys.empty()) {
    cout << "  Keys unique to one solution file: ";
    for(size_t i = 0; i < missingKeys.size(); ++i) {
      cout << DefaultKeyFormatter(missingKeys[i]);
      if(i != missingKeys.size() - 1)
        cout << ", ";
    }
    cout << endl;
  }
  KeyVector commonKeys;
  br::set_intersection(soln1.keys(), soln2.keys(), std::back_inserter(commonKeys));
  double maxDiff = 0.0;
  for(Key j: commonKeys)
    maxDiff = std::max(maxDiff, soln1.at(j).localCoordinates_(soln2.at(j)).norm());
  cout << "  Maximum solution difference (norm of logmap): " << maxDiff << endl;
}

/* ************************************************************************* */
void runPerturb()
{
  // Set up random number generator
  boost::mt19937 rng;
  boost::normal_distribution<double> normal(0.0, perturbationNoise);

  // Perturb values
  VectorValues noise;
  for(const Values::KeyValuePair& key_val: initial)
  {
    Vector noisev(key_val.value.dim());
    for(Vector::Index i = 0; i < noisev.size(); ++i)
      noisev(i) = normal(rng);
    noise.insert(key_val.key, noisev);
  }
  Values perturbed = initial.retract(noise);

  // Write results
  try {
    cout << "Writing output file " << outputFile << endl;
    std::ofstream writerStream(outputFile.c_str(), ios::binary);
    boost::archive::binary_oarchive writer(writerStream);
    writer << perturbed;
  } catch(std::exception& e) {
    cout << e.what() << endl;
    exit(1);
  }

}

/* ************************************************************************* */
void runStats()
{
  cout << "Gathering statistics..." << endl;
  GaussianFactorGraph linear = *datasetMeasurements.linearize(initial);
  GaussianJunctionTree jt(GaussianEliminationTree(linear, Ordering::Colamd(linear)));
  treeTraversal::ForestStatistics statistics = treeTraversal::GatherStatistics(jt);

  ofstream file;

  cout << "Writing SolverComparer_Stats_problemSizeHistogram.txt..." << endl;
  file.open("SolverComparer_Stats_problemSizeHistogram.txt");
  treeTraversal::ForestStatistics::Write(file, statistics.problemSizeHistogram);
  file.close();

  cout << "Writing SolverComparer_Stats_numberOfChildrenHistogram.txt..." << endl;
  file.open("SolverComparer_Stats_numberOfChildrenHistogram.txt");
  treeTraversal::ForestStatistics::Write(file, statistics.numberOfChildrenHistogram);
  file.close();

  cout << "Writing SolverComparer_Stats_problemSizeOfSecondLargestChildHistogram.txt..." << endl;
  file.open("SolverComparer_Stats_problemSizeOfSecondLargestChildHistogram.txt");
  treeTraversal::ForestStatistics::Write(file, statistics.problemSizeOfSecondLargestChildHistogram);
  file.close();
}


/**
 * TuningPotential.cc
 * Computes tuning potential for all regions
 * @ param1:
 */

#include <iostream>
#include <string>
#include <sstream>
#include <fstream>
#include <unistd.h>
#include <algorithm>
#include <numeric>
#include <boost/format.hpp>
#include <vector>
#include <list>
#include <map>
#include <iomanip>

#include "../include/TuningPotential.h"
#include "../include/ProcessingIntensity.h"
#include "../../common_incl/helper.h"

using namespace std;
using namespace cube;
using namespace services;
using namespace boost;

/**
 * @Brief: Computes and detect dynamism for all significant regions
 * @param phase_node
 * @param cnodes list of significant call nodes
 * @param rgion_names list of significant region names
 * @param in_cube name of the cube file
 * @param threshold Threshold of Dynamism depending on avg of phase
 * @return
 */
list< SignificantRegion* >
ComputeIntraPhaseTP( Cnode*            phase_node,
                     vector< Cnode* >& cnodes,
                     vector< string >& region_names,
                     Cube*             in_cube ) {
    vector< Cnode* >           sig_nodes;
    list< SignificantRegion* > sig_region_list;
    const vector< Process* >&  processes = in_cube->get_procv();
    Metric*                    met_any   = in_cube->get_met( "time" );

    for( const auto& cnode : cnodes )
    {
        string c_name = cnode->get_callee()->get_name();
        for( const auto& region : region_names )
        {
            if( strcasecmp( region.c_str(), c_name.c_str() ) == 0 )
                sig_nodes.push_back( cnode );
        }
    }

    //double avg_phase_transfer_intnsty = getPhaseNodeAvgProcIntnsty( phase_node, in_cube, processes[0] );

    vector< Cnode* > variant_nodes;

    for( const auto& sig_node : sig_nodes )
    {
        Region *region = sig_node->get_callee();

        Value* met_tau_val = in_cube->get_sev_adv( met_any,
                                                   CUBE_CALCULATE_INCLUSIVE,
                                                   sig_node,
                                                   CUBE_CALCULATE_EXCLUSIVE,
                                                   processes[ 0 ],
                                                   CUBE_CALCULATE_INCLUSIVE );

        if( met_tau_val->myDataType() != CUBE_DATA_TYPE_TAU_ATOMIC )
        {
            std::cout << "Please instrument the application with CUBE_TUPLE profiling format( Intra-phase )" << endl;

            return sig_region_list;
        }

        /* Casting from Value to TauAtomicValue */
        TauAtomicValue* tau_atomic_tuple = ( TauAtomicValue* ) met_tau_val;
        string stat_tuple                = tau_atomic_tuple->getString();
        vector< string > tau_atomic_val  = ValueParser( stat_tuple );
        double stand_deviation           = tau_atomic_val.at( 4 ) == "-nan" ? 0.0 : stod( tau_atomic_val.at( 4 ) );
        uint64_t N                       = stod( tau_atomic_val.at( 0 ) );
        double mean                      = stod( tau_atomic_val.at( 3 ) );
        double minValue                  = tau_atomic_tuple->getMinValue().getDouble();
        double maxValue                  = tau_atomic_tuple->getMaxValue().getDouble();

        if ( N != 0 )
        {
            double phasetime = getPhaseNodeTime( phase_node, in_cube, met_any, processes[ 0 ] );
            /*std::cout << "REGION NAME: " << sig_node->get_callee()->get_name() << endl;
            std::cout << "Exec_time: " << mean * N << endl;
            std::cout << "Exec_time of Phase: " << phasetime << endl;*/
            double dyn_avg_phase = ( mean ) * N / getPhaseNodeTime( phase_node, in_cube, met_any, processes[ 0 ] ) * 100; // weight
            double dev_perc_reg  = 0.0;

            if ( mean != 0.0 )
            {
                dev_perc_reg = ( stand_deviation / mean ) * 100 ;  // coefficient of variation OR degree of spreadness
            }
            double dev_perc_phase = ( stand_deviation / getPhaseNodeAvgValue( phase_node, in_cube, met_any, processes[ 0 ] ) ) * 100;

            DynamismMetric dyn_ti = DynamismMetric( met_any->get_uniq_name(), minValue, maxValue, ( mean ) * N, dev_perc_reg, dev_perc_phase, dyn_avg_phase );
            vector< DynamismMetric > dyn_metrics;
            dyn_metrics.push_back( dyn_ti );
            double avg_compute_intensity = getProcessingIntensity( sig_node, in_cube );

            if( avg_compute_intensity != -1 )
            {
                double avg_phase_compute_intensity = getPhaseNodeAvgProcIntnsty( phase_node, in_cube, processes[ 0 ] );
                double variation_compute_intensity = ( avg_compute_intensity / avg_phase_compute_intensity ) * 100;
                std::string str                    = "Transfer Intensity";
                DynamismMetric dyn_t_i             = DynamismMetric( str, 0.0, 0.0, 0.0, 0.0, 0.0, variation_compute_intensity );
                dyn_metrics.push_back( dyn_t_i );
            }
            else
            {
                std::cout << " Please run the application with PAPI Metric plugin to get intensity information" << endl;
                std::string str        = "Transfer Intensity";
                DynamismMetric dyn_t_i = DynamismMetric( str, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 );
                dyn_metrics.push_back( dyn_t_i );
            }

            if( !dyn_metrics.empty() )
            {
                SignificantRegion* sig_region = new SignificantRegion;

                sig_region->name            = region->get_name();
                //sig_region->source_location = region->get_mod();
                //sig_region->line_no         = region->get_begn_ln();
                sig_region->granularity     = computeGranularity( sig_node, in_cube );

                sig_region->dynamism_metrics.assign( dyn_metrics.begin(), dyn_metrics.end() );
                sig_region_list.push_back( sig_region );
                variant_nodes.push_back( sig_node );
            }
        }
    }

    if ( !variant_nodes.empty() )
    {
        printPotentialResults( sig_region_list );
    }
    else
    {
        std::cout << endl << " There is no intra-phase dynamism" << endl;
    }

    return sig_region_list;
}

/**
 *
 * @Brief: Print the tuning potential results
 * @param
 * @s_regions: pointer to dnyamism metric
 * @size: no of significant regions
 * @no_m: # of metrices
 */
void
printPotentialResults( list< SignificantRegion* >& regions )
{
    std::cout << "Significant region information\n";
    std::cout << "==============================" << endl;

    std::cout << format( "%-28s %-15s %-12s %-15s %-15s %-15s %-15s\n" )
            % "Region name"  %  "Min(t)" % "Max(t)" % "Time" % "Time Dev.(%Reg)" % "Ops/L3miss" %  "Weight(%Phase)"     ;
    for ( const auto& region : regions )
    {
        if ( region->dynamism_metrics.size() > 0 )
        {
            std::cout << format( "%-25s %8.3f          %8.3f     %8.3f         %5.1f       %7.0f            %5.0f \n" ) %
                         region->name.substr( 0, 25 ) %
                         region->dynamism_metrics[ 0 ].min %
                         region->dynamism_metrics[ 0 ].max %
                         region->dynamism_metrics[ 0 ].execTime %
                         region->dynamism_metrics[ 0 ].dev_perc_reg %
                         region->dynamism_metrics[ 1 ].dyn_perc_phase %
                         region->dynamism_metrics[ 0 ].dyn_perc_phase;
        }
    }
}

/**
 * @brief Computes and detect dynamism across all phases
 * @param phase_node
 * @param in_cube name of the cube file
 * @param threshold  Threshold of Dynamism
 * @return
 */
PhaseRegion*
ComputeInterPhaseTP( Cnode* phase_node,
                     Cube*  in_cube,
                     double threshold )
{
    Metric* met_t                       = in_cube->get_met( "time" );
    const vector< Process* >& processes = in_cube->get_procv();

    Value* complex_value = in_cube->get_sev_adv( met_t,
                                                 CUBE_CALCULATE_INCLUSIVE,
                                                 phase_node,
                                                 CUBE_CALCULATE_EXCLUSIVE,
                                                 processes[ 0 ],
                                                 CUBE_CALCULATE_INCLUSIVE );

    if( complex_value->myDataType() != CUBE_DATA_TYPE_TAU_ATOMIC )
    {
        std::cout << "Please instrument the application with CUBE_TUPLE profiling format( Inter-phase ) " << endl;
        return NULL;
    }

    /* Casting from Value to TauAtomicValue */
    TauAtomicValue* tau_atomic_tuple = ( TauAtomicValue* )complex_value;
    vector< string > tau_atomic_val  = ValueParser( tau_atomic_tuple->getString() );
    double standard_deviation        = stod( tau_atomic_val.at( 4 ) );
    double mean                      = stod( tau_atomic_val.at( 3 ) );
    double minValue                  = tau_atomic_tuple->getMinValue().getDouble();
    double maxValue                  = tau_atomic_tuple->getMaxValue().getDouble();
    uint64_t N                       = stod( tau_atomic_val.at( 0 ) );
    if( std::isnan( standard_deviation ) )
        standard_deviation = 0.0;

    double variation_percentage = ( ( maxValue - minValue ) / mean ) * 100;
    double dev_percentage       = ( standard_deviation / mean ) * 100;

    std::cout << endl << "Phase information" << endl;
    std::cout <<         "=================" << endl;
    std::cout << format( "%-20s %-20s %-20s %-20s %-20s %-20s \n" )
                 % "Min" % "Max" % "Mean"% "Time"% "Dev.(% Phase)" % "Dyn.(% Phase)";

    std::cout << format( "%-20s %-20s %-20s %-20s %-20s %-20s  \n" ) %
                 minValue % maxValue %  mean % ( N * mean ) % dev_percentage % variation_percentage;

    PhaseRegion* phase = new PhaseRegion;

    phase->name         = phase_node->get_callee()->get_name();
    phase->granularity  = computeGranularity( phase_node, in_cube );
    phase->min          = minValue;
    phase->max          = maxValue;
    phase->mean         = mean;
    phase->absTime      = N * mean;
    phase->dev_perc     = dev_percentage;
    phase->var_perc     = variation_percentage;
    phase->has_dynamism = variation_percentage > threshold;

    return phase;
}

/**
 * @brief Get the avaergae value of phase node
 * @param pnode
 * @param input
 * @param met_any
 * @param proc
 * @return
 */
double
getPhaseNodeAvgValue( Cnode*   pnode,
                      Cube *   input,
                      Metric*  met_any,
                      Process* proc )
{
    Value* complex_value = input->get_sev_adv( met_any,
                                               CUBE_CALCULATE_INCLUSIVE,
                                               pnode,
                                               CUBE_CALCULATE_EXCLUSIVE,
                                               proc,
                                               CUBE_CALCULATE_INCLUSIVE );

    /* Casting from Value to TauAtomicValue */
    TauAtomicValue* tau_atomic_tuple = ( TauAtomicValue* )complex_value;
    vector< string > tau_atomic_val  = ValueParser( tau_atomic_tuple->getString() );
    double average                   = stod( tau_atomic_val.at( 3 ) );

    return average;
}

/**
 * @brief Get the absolute value of phase node
 * @param pnode
 * @param input
 * @param met_any
 * @param proc
 * @return
 */
double
getPhaseNodeTime( Cnode*   pnode,
                  Cube*    input,
                  Metric*  met_any,
                  Process* proc )
{

    Value* complex_value = input->get_sev_adv( met_any,
                                               CUBE_CALCULATE_INCLUSIVE,
                                               pnode,
                                               CUBE_CALCULATE_EXCLUSIVE,
                                               proc,
                                               CUBE_CALCULATE_INCLUSIVE );

    /* Casting from Value to TauAtomicValue */
    TauAtomicValue* tau_atomic_tuple = ( TauAtomicValue* )complex_value;
    vector< string > tau_atomic_val  = ValueParser( tau_atomic_tuple->getString() );
    double average                   = stod( tau_atomic_val.at( 3 ) );
    uint64_t N                       = stod( tau_atomic_val.at( 0 ) );

    return average * N;
}


// Copyright 2020 Peter Dimov
// Distributed under the Boost Software License, Version 1.0.
// https://www.boost.org/LICENSE_1_0.txt

#include <boost/lambda2.hpp>
#include <boost/core/lightweight_test.hpp>

int main()
{
    using namespace boost::lambda2;

    BOOST_TEST_EQ( (_1 + _2 * _3)( 1, 2, 3), 1 + 2 * 3 );

    return boost::report_errors();
}


#include <rct_optimizations/dh_chain_kinematic_calibration.h>
#include <rct_optimizations/local_parameterization.h>
#include <rct_optimizations_tests/dh_chain_observation_creator.h>
#include <rct_optimizations_tests/utilities.h>
#include <rct_optimizations_tests/observation_creator.h>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>
#include <gtest/gtest.h>

using namespace rct_optimizations;

class DHChainMeasurementTest : public ::testing::Test
{
public:
  /**
   * @brief Constructor
   * Note: the calibration problem is initialized with a DH chain with modified values to be
   * more representative of a "real-life" use-case
   */
  DHChainMeasurementTest()
    : camera_chain_truth(test::perturbDHChain(test::createABBIRB2400(), 1.0e-3))
    , target_chain_truth({})
    , problem(camera_chain_truth, target_chain_truth)
    , orientation_weight(100.0)
  {
    // Set a few specific Ceres solver parameters
    options.max_num_iterations = 500;
    options.num_threads = 4;
    options.minimizer_progress_to_stdout = true;
    options.use_nonmonotonic_steps = true;
  }

  void SetUp() override
  {
    setActualData();
    setInitialGuess();
    setObservations();
    applyMasks();
  }

  virtual void setActualData()
  {
    n_observations = 150;

    position_diff_mean_threshold = 0.005;
    position_diff_std_dev_threshold = 0.003;
    orientation_diff_mean_threshold = 0.008;
    orientation_diff_std_dev_threshold = 0.004;

    // Create the transform from the camera mount (i.e. robot wrist) to the camera
    camera_mount_to_camera_truth = Eigen::Isometry3d::Identity();
    camera_mount_to_camera_truth.rotate(Eigen::AngleAxisd(M_PI / 2.0, Eigen::Vector3d::UnitY()));

    // Create the transform from the target mount (in this case an empty kinematic chain) to the target
    target_mount_to_target_truth = Eigen::Isometry3d::Identity();

    // Create the transform from the base of the camera kinematic chain to the base of the target kinematic chain
    camera_base_to_target_base_truth = Eigen::Isometry3d::Identity();
    camera_base_to_target_base_truth.translate(Eigen::Vector3d(0.940, 0.0, 0.0));
  }

  virtual void setInitialGuess()
  {
    // set initial guess to true values
    problem.camera_mount_to_camera_guess = camera_mount_to_camera_truth;
    problem.target_mount_to_target_guess = target_mount_to_target_truth;
    problem.camera_base_to_target_base_guess = camera_base_to_target_base_truth;
  }

  virtual void setObservations()
  {
    // generate observations using true kinematics
    EXPECT_NO_THROW(problem.observations = test::createKinematicMeasurements(camera_chain_truth,
                                                                             target_chain_truth,
                                                                             camera_mount_to_camera_truth,
                                                                             target_mount_to_target_truth,
                                                                             camera_base_to_target_base_truth,
                                                                             n_observations));
  }

  virtual void applyMasks()
  {
  }

  virtual void analyzeResults(const KinematicCalibrationResult& result)
  {
    EXPECT_TRUE(result.converged);

    // Expect the resulting DH parameter offsets to be the same shape as the input
    // This is an important check of the case where the cost function modifies for 0 DoF chains
    EXPECT_EQ(problem.camera_chain.dof(), result.camera_chain_dh_offsets.rows());
    EXPECT_EQ(problem.target_chain.dof(), result.target_chain_dh_offsets.rows());

    // Test the result by moving the robot around to a lot of positions and seeing of the results match
    DHChain optimized_chain(problem.camera_chain, result.camera_chain_dh_offsets);

    std::cout << "true chain:\n" << camera_chain_truth.getDHTable().matrix() << std::endl << std::endl;
    std::cout << "optimized chain:\n" << optimized_chain.getDHTable().matrix() << std::endl << std::endl;

    namespace ba = boost::accumulators;
    ba::accumulator_set<double, ba::stats<ba::tag::mean, ba::tag::variance>> pos_acc;
    ba::accumulator_set<double, ba::stats<ba::tag::mean, ba::tag::variance>> ori_acc;

    const std::size_t n = 1000;
    for (std::size_t i = 0; i < n; ++i)
    {
      Eigen::VectorXd random_pose = camera_chain_truth.createUniformlyRandomPose();

      // True chain
      Eigen::Isometry3d true_fk = camera_chain_truth.getFK(random_pose) * camera_mount_to_camera_truth;

      // Optimized chain
      Eigen::Isometry3d optimized_fk = optimized_chain.getFK(random_pose)
                                       * result.camera_mount_to_camera;

      // Compare
      Eigen::Isometry3d diff = true_fk.inverse() * optimized_fk;
      pos_acc(diff.translation().norm());
      ori_acc(Eigen::Quaterniond(true_fk.linear()).angularDistance(Eigen::Quaterniond(optimized_fk.linear())));
    }

    std::cout << "Position Difference Mean: " << ba::mean(pos_acc) << std::endl;
    std::cout << "Position Difference Std. Dev.: " << std::sqrt(ba::variance(pos_acc)) << std::endl;

    std::cout << "Orientation Difference Mean: " << ba::mean(ori_acc) << std::endl;
    std::cout << "Orientation difference Std. Dev.: " << std::sqrt(ba::variance(ori_acc)) << std::endl;

    EXPECT_LT(ba::mean(pos_acc), position_diff_mean_threshold);
    EXPECT_LT(std::sqrt(ba::variance(pos_acc)), position_diff_std_dev_threshold);
    EXPECT_LT(ba::mean(ori_acc), orientation_diff_mean_threshold);
    EXPECT_LT(std::sqrt(ba::variance(ori_acc)), orientation_diff_std_dev_threshold);
  }

  double position_diff_mean_threshold;
  double position_diff_std_dev_threshold;
  double orientation_diff_mean_threshold;
  double orientation_diff_std_dev_threshold;

  std::size_t n_observations;

  KinematicMeasurement::Set observations;

  const DHChain camera_chain_truth;
  const DHChain target_chain_truth;
  Eigen::Isometry3d camera_mount_to_camera_truth;
  Eigen::Isometry3d target_mount_to_target_truth;
  Eigen::Isometry3d camera_base_to_target_base_truth;

  KinematicCalibrationProblemPose6D problem;
  ceres::Solver::Options options;
  double orientation_weight;
};

/**
 * @brief Tests the Dual DH Chain kinematic calibration algorithm with perfect initial conditions
 */
class DHChainMeasurementTest_PerfectInitial : public DHChainMeasurementTest
{
public:
  using DHChainMeasurementTest::DHChainMeasurementTest;

  virtual void analyzeResults(const KinematicCalibrationResult& result) override
  {
    // Check the nominal expectations
    DHChainMeasurementTest::analyzeResults(result);

    EXPECT_LT(result.initial_cost_per_obs, 1.0e-15);

    EXPECT_TRUE(result.camera_mount_to_camera.isApprox(camera_mount_to_camera_truth));
    EXPECT_TRUE(result.target_mount_to_target.isApprox(target_mount_to_target_truth));
    EXPECT_TRUE(result.camera_base_to_target_base.isApprox(camera_base_to_target_base_truth));

    // Expect the resulting DH parameter offsets to be the same shape as the input
    // This is an important check of the case where the cost function modifies for 0 DoF chains
    EXPECT_EQ(problem.camera_chain.dof(), result.camera_chain_dh_offsets.rows());
    EXPECT_EQ(problem.target_chain.dof(), result.target_chain_dh_offsets.rows());

    // Expect the camera DH offsets to remain the same as the input (i.e. all zeros)
    for (Eigen::Index row = 0; row < result.camera_chain_dh_offsets.rows(); ++row)
    {
      for (Eigen::Index col = 0; col < result.camera_chain_dh_offsets.cols(); ++col)
      {
        EXPECT_LT(result.camera_chain_dh_offsets(row, col), 1.0e-15);
      }
    }
  }
};

/**
 * @brief Tests the Dual DH Chain kinematic calibration algorithm with
 * initial guesses for DH params that are slightly different from the true values
 */
class DHChainMeasurementTest_PerturbedDH : public DHChainMeasurementTest
{
public:
  using DHChainMeasurementTest::DHChainMeasurementTest;

  virtual void setInitialGuess() override
  {
    DHChainMeasurementTest::setInitialGuess();

    problem.camera_chain = test::createABBIRB2400();
  }

  virtual void applyMasks() override
  {
    // Mask a few DH parameters
    {
      Eigen::Matrix<bool, Eigen::Dynamic, 4> mask =
          Eigen::Matrix<bool, Eigen::Dynamic, 4>::Constant(problem.camera_chain.dof(), 4, false);

      // Mask the last row because they duplicate the camera mount to camera transform
      mask.bottomRows(1) << true, true, true, true;

      // Mask the joint 2 "r" parameter
      // It seems to have a high correlation with target_mount_to_target_y
      mask(1, 2) = true;

      // Add the mask to the problem
      problem.mask.at(0) = createDHMask(mask);
    }

    /* Mask the camera base to target base transform (duplicated by target mount to target transform when
     * the target chain has no joints */
    problem.mask.at(6) = { 0, 1, 2 };
    problem.mask.at(7) = { 0, 1, 2 };
  }

  virtual void analyzeResults(const KinematicCalibrationResult& result) override
  {
    DHChainMeasurementTest::analyzeResults(result);

    // Expect the masked variables not to have changed
    EXPECT_TRUE(result.camera_chain_dh_offsets.bottomRows(1).isApproxToConstant(0.0));
    EXPECT_DOUBLE_EQ(result.camera_chain_dh_offsets(1, 2), 0.0);
    EXPECT_TRUE(result.camera_base_to_target_base.isApprox(problem.camera_base_to_target_base_guess));
  }
};

/**
 * @brief Tests the Dual DH Chain kinematic calibration algorithm with
 * initial guesses for DH params and transforms that are slightly different from the true values
 */
class DHChainMeasurementTest_PerturbedDH_PertubedGuess : public DHChainMeasurementTest_PerturbedDH
{
public:
  using DHChainMeasurementTest_PerturbedDH::DHChainMeasurementTest_PerturbedDH;

  virtual void setInitialGuess() override
  {
    DHChainMeasurementTest_PerturbedDH::setInitialGuess();

    problem.camera_mount_to_camera_guess = test::perturbPose(camera_mount_to_camera_truth, 0.05, 0.05);
    problem.target_mount_to_target_guess = test::perturbPose(target_mount_to_target_truth, 0.05, 0.05);
    problem.camera_base_to_target_base_guess = test::perturbPose(camera_base_to_target_base_truth, 0.05, 0.05);
  }
};

TEST_F(DHChainMeasurementTest, TestCostFunction)
{
  // Initialize the optimization variables
  // Camera mount to camera (cm_to_c) quaternion and translation
  Eigen::Vector3d t_cm_to_c(camera_mount_to_camera_truth.translation());
  Eigen::AngleAxisd rot_cm_to_c(camera_mount_to_camera_truth.rotation());
  Eigen::Vector3d aa_cm_to_c(rot_cm_to_c.angle() * rot_cm_to_c.axis());

  // Target mount to target (cm_to_c) quaternion and translation
  Eigen::Vector3d t_tm_to_t(target_mount_to_target_truth.translation());
  Eigen::AngleAxisd rot_tm_to_t(target_mount_to_target_truth.rotation());
  Eigen::Vector3d aa_tm_to_t(rot_tm_to_t.angle() * rot_tm_to_t.axis());

  // Camera chain base to target_chain_base (ccb_to_tcb) quaternion and translation
  Eigen::Vector3d t_ccb_to_tcb(camera_base_to_target_base_truth.translation());
  Eigen::AngleAxisd rot_ccb_to_tcb(camera_base_to_target_base_truth.rotation());
  Eigen::Vector3d aa_ccb_to_tcb(rot_ccb_to_tcb.angle() * rot_ccb_to_tcb.axis());

  // Create containers for the kinematic chain DH offsets
  // Ceres will not work with parameter blocks of size zero, so create a dummy set of DH offsets for chains with DoF == 0
  Eigen::MatrixX4d camera_chain_dh_offsets = Eigen::MatrixX4d::Zero(camera_chain_truth.dof(), 4);
  Eigen::MatrixX4d target_chain_dh_offsets = Eigen::MatrixX4d::Zero(target_chain_truth.dof(), 4);

  // Create a vector of the pointers to the optimization variables in the order that the cost function expects them
  std::vector<double *> parameters
      = DualDHChainCostPose6D::constructParameters(camera_chain_dh_offsets,
                                                 target_chain_dh_offsets,
                                                 t_cm_to_c,
                                                 aa_cm_to_c,
                                                 t_tm_to_t,
                                                 aa_tm_to_t,
                                                 t_ccb_to_tcb,
                                                 aa_ccb_to_tcb);

  // Create observations
  KinematicMeasurement::Set observations = test::createKinematicMeasurements(camera_chain_truth,
                                                                             target_chain_truth,
                                                                             camera_mount_to_camera_truth,
                                                                             target_mount_to_target_truth,
                                                                             camera_base_to_target_base_truth,
                                                                             n_observations);

  // Test the cost function
  for (const auto &obs : observations)
  {
    DualDHChainCostPose6D cost(obs, camera_chain_truth, target_chain_truth, orientation_weight);

    double residual[4];
    EXPECT_TRUE(cost(parameters.data(), residual));

    EXPECT_NEAR(residual[0], 0, 1.0e-12);  // x
    EXPECT_NEAR(residual[1], 0, 1.0e-12);  // y
    EXPECT_NEAR(residual[2], 0, 1.0e-12);  // z

    EXPECT_NEAR(residual[3], 0, 1.0e-12);  // rotation
  }
}

TEST_F(DHChainMeasurementTest_PerfectInitial, PerfectInitialConditions)
{
  KinematicCalibrationResult result = optimize(problem, orientation_weight, options);
  std::cout << result.covariance.printCorrelationCoeffAboveThreshold(0.5) << std::endl;
  analyzeResults(result);
}

TEST_F(DHChainMeasurementTest_PerturbedDH, PerfectGuessPerturbedDH)
{
  KinematicCalibrationResult result = optimize(problem, orientation_weight, options);
  std::cout << result.covariance.printCorrelationCoeffAboveThreshold(0.5) << std::endl;
  analyzeResults(result);
}

TEST_F(DHChainMeasurementTest_PerturbedDH_PertubedGuess, PerturbedDHPerturbedGuess)
{
  KinematicCalibrationResult result = optimize(problem, orientation_weight, options);
  std::cout << result.covariance.printCorrelationCoeffAboveThreshold(0.5) << std::endl;
  analyzeResults(result);
}

int main(int argc, char **argv)
{
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}