/*
 * (C) Copyright 2021 Met Office UK
 *
 * This software is licensed under the terms of the Apache Licence Version 2.0
 * which can be obtained at http://www.apache.org/licenses/LICENSE-2.0.
 */

#include "ufo/filters/TrackCheckShip.h"

#include <Eigen/Core>  // for easy array addition/subtraction

#include <algorithm>
#include <cassert>
#include <cmath>
#include <functional>
#include <map>
#include <string>
#include <tuple>
#include <utility>
#include <vector>

#include "ioda/ObsDataVector.h"
#include "ioda/ObsSpace.h"
#include "oops/base/Variables.h"
#include "oops/util/DateTime.h"
#include "oops/util/Duration.h"
#include "oops/util/Logger.h"
#include "ufo/filters/ObsAccessor.h"
#include "ufo/filters/TrackCheckShipDiagnostics.h"
#include "ufo/filters/TrackCheckShipParameters.h"
#include "ufo/utils/Constants.h"
#include "ufo/utils/RecursiveSplitter.h"

namespace ufo {

TrackCheckShip::TrackCheckShip(ioda::ObsSpace &obsdb, const Parameters_ &parameters,
                               std::shared_ptr<ioda::ObsDataVector<int> > flags,
                               std::shared_ptr<ioda::ObsDataVector<float> > obserr)
  : FilterBase(obsdb, parameters, flags, obserr), options_(parameters)
{
  oops::Log::debug() << "TrackCheckShip: config = " << options_ << std::endl;
  assert(options_.core.maxSpeed.value() > 0);
  if (options_.testingMode)
    diagnostics_.reset(new TrackCheckShipDiagnostics());
}

/// \brief Estimate of speed as calculated in Ops_CheckShipTrack.
///
/// Speed is calculated between \p obs1 and \p obs2, accounting for spatial/temporal
/// uncertainty using the resolution values stored in \p options.
double TrackCheckShip::speedEstimate(
    const TrackCheckShip::TrackObservation &obs1,
    const TrackCheckShip::TrackObservation &obs2,
    const TrackCheckShipParameters& options) {
  util::Duration temporalDistance = abs(obs1.getTime() -
                                        obs2.getTime());
  util::Duration tempRes = options.core.temporalResolution;
  auto dist = distance(obs1, obs2);
  auto spatialRes = options.core.spatialResolution;
  double speedEst = 0.0;
  if (dist > spatialRes) {
    speedEst = (dist - 0.5 * spatialRes) /
        std::max(temporalDistance.toSeconds(),
                 (tempRes.toSeconds()));
    speedEst *= 1000.0;  // convert units from km/s to m/s
  }
  return speedEst;
}

/// \brief Returns the angle in degrees (rounded to the nearest .1 degree)
/// between displacement vectors going from observations \p a to \p b
/// and \p b to \p c.
float TrackCheckShip::angle(const TrackCheckShip::TrackObservation &a,
                            const TrackCheckShip::TrackObservation &b,
                            const TrackCheckShip::TrackObservation &c) {
  auto locA = a.getLocation();
  auto locB = b.getLocation();
  auto locC = c.getLocation();
  Eigen::Vector3f disp1{locB[0]-locA[0], locB[1]-locA[1], locB[2]-locA[2]};
  Eigen::Vector3f disp2{locC[0]-locB[0], locC[1]-locB[1], locC[2]-locB[2]};
  auto retValue = std::acos(disp1.dot(disp2) / (disp1.norm() * disp2.norm()));
  retValue *= static_cast<float>(Constants::rad2deg);
  return std::round(retValue * 10.0f) * 0.1f;
}

const TrackCheckShipDiagnostics* TrackCheckShip::diagnostics() const
{
  return diagnostics_.get();
}

TrackCheckShip::TrackObservation::TrackObservation(
    double latitude, double longitude,
    const util::DateTime &time,
    std::shared_ptr<TrackStatistics> const& ts,
    std::shared_ptr<TrackCheckUtils::CheckCounter> const &checkCounter,
    size_t observationNumber)
  : obsLocationTime_(latitude, longitude, time), fullTrackStatistics_(ts),
    observationNumber_(observationNumber), rejected_(false) {}

size_t TrackCheckShip::TrackObservation::getObservationNumber() const
{
  return observationNumber_;
}

// Required for the correct destruction of options_.
TrackCheckShip::~TrackCheckShip()
{}

/// Detects whether each observation in a track has been rejected, and marks
/// the corresponding space in \p isRejected as true if so.
/// \param trackObsIndicesBegin the begin iterator of the track
/// \param trackObsIndicesEnd the end iterator
/// \param validObsIds the vector marking all of the observations' global
/// positions
/// \param trackObservations the full vector of observations within
/// the single track
/// \param isRejected the boolean vector whose indices correspond to all of
/// the rejected observations in the full input dataset
void TrackCheckShip::flagRejectedTrackObservations(
    std::vector<size_t>::const_iterator trackObsIndicesBegin,
    std::vector<size_t>::const_iterator trackObsIndicesEnd,
    const std::vector<size_t> &validObsIds,
    const std::vector<TrackObservation> &trackObservations,
    std::vector<bool> &isRejected) const {
  auto trackObsIndexIt = trackObsIndicesBegin;
  auto trackObsIt = trackObservations.begin();
  for (; trackObsIndexIt != trackObsIndicesEnd; ++trackObsIndexIt, ++trackObsIt)
    isRejected[validObsIds[*trackObsIndexIt]] = trackObsIt->rejected();
}

void TrackCheckShip::print(std::ostream & os) const {
  os << "TrackCheckShip: config = " << options_ << std::endl;
}

/// The filter begins with separating observations into tracks based on \p Station_Id. Various
/// calculations are then performed between consecutive observations: distances/speeds between
/// observation pairs, and angles between the displacement vectors formed by triplets of
/// consecutive observations. Track-specific counters are incremented for various metrics that
/// indicate an unexpected track path.
///
/// Based on these counter values, the filter may ignore the current track and move on to the next
/// one. Using the described calculations and additional ones if necessary, the observation
/// directly before and/or after the fastest segment is chosen for rejection. This will continue
/// until all remaining segments that link accepted observations are either:
/// 1. slower than a user-defined "max speed (m/s)" where the angles between the fastest accepted
/// segment's displacement vector and its two neighbouring segments' displacement vectors are
/// both less than 90 degrees.
/// 2. slower than 80 percent of this user-defined speed.
///
/// The full track will be rejected if the number of observations removed is more than the
/// user-defined "rejection threshold".

void TrackCheckShip::applyFilter(const std::vector<bool> & apply,
                                 const Variables & filtervars,
                                 std::vector<std::vector<bool>> & flagged) const {
  ObsAccessor obsAccessor = TrackCheckUtils::createObsAccessor(options_.stationIdVariable, obsdb_);

  const std::vector<size_t> validObsIds
                                   = obsAccessor.getValidObservationIds(apply, *flags_, filtervars);

  RecursiveSplitter splitter = obsAccessor.splitObservationsIntoIndependentGroups(validObsIds);
  TrackCheckUtils::sortTracksChronologically(validObsIds, obsAccessor, splitter);

  TrackCheckUtils::ObsGroupLocationTimes obsLocTime =
      TrackCheckUtils::collectObservationsLocations(obsAccessor);

  std::vector<bool> isRejected(obsLocTime.latitudes.size(), false);
  size_t trackNumber = 0;
  for (auto track : splitter.multiElementGroups()) {
    trackNumber++;
    std::string stationId = std::to_string(trackNumber);
    std::vector<TrackObservation> trackObservations = collectTrackObservations(
          track.begin(), track.end(), validObsIds, obsLocTime);
    std::vector<std::reference_wrapper<TrackObservation>> trackObservationsReferences;
    trackObservationsReferences.reserve(trackObservations.size());
    std::transform(trackObservations.begin(), trackObservations.end(),
                   std::back_inserter(trackObservationsReferences),
                   [](TrackObservation &obs) {
      return std::ref<TrackObservation>(obs); });
    calculateTrackSegmentProperties(trackObservationsReferences,
                                    CalculationMethod::FIRSTITERATION);
    if (!trackObservationsReferences.empty() &&
        this->options_.core.earlyBreakCheck &&
        TrackCheckShip::earlyBreak(trackObservationsReferences, stationId)) {
      continue;
    }
      bool firstIterativeRemoval = true;
      while (trackObservationsReferences.size() >= 3) {
        // Initial loop: fastest (as determined by set of comparisons) observation removed
        // until all segments show slower speed than max threshold
        auto maxSpeedReferenceIterator = std::max_element(
              trackObservationsReferences.begin(), trackObservationsReferences.end(),
              [](TrackObservation a, TrackObservation b) {
            return a.getObservationStatistics().speed <
            b.getObservationStatistics().speed;});
        auto maxSpeedValue = maxSpeedReferenceIterator->get().getObservationStatistics().speed;
        if (maxSpeedValue <= (0.8 * options_.core.maxSpeed.value())) {
          break;
        } else if (maxSpeedValue < options_.core.maxSpeed.value()) {
          auto maxSpeedAngle = std::max(
                (maxSpeedReferenceIterator - 1)->get().getObservationStatistics().angle,
                maxSpeedReferenceIterator->get().getObservationStatistics().angle);
          if (maxSpeedAngle <= 90.0) {
            break;
          }
        }
        removeFaultyObservation(
              trackObservationsReferences, maxSpeedReferenceIterator, firstIterativeRemoval,
              stationId);
        firstIterativeRemoval = false;
        calculateTrackSegmentProperties(trackObservationsReferences, CalculationMethod::MAINLOOP);
      }
      auto rejectedCount = std::count_if(trackObservations.begin(), trackObservations.end(),
                    [](const TrackObservation& a) {return a.rejected();});
      if (rejectedCount >= options_.core.rejectionThreshold.value() * trackObservations.size()) {
        oops::Log::trace() << "CheckShipTrack: track " << stationId << " NumRej " <<
                              rejectedCount << " out of " << trackObservations.size() <<
                              " reports rejected. *** Reject whole track ***\n";
        for (TrackObservation &obs : trackObservations)
          obs.setRejected(true);
      }
      flagRejectedTrackObservations(track.begin(), track.end(),
                                    validObsIds, trackObservations, isRejected);
  }
  obsAccessor.flagRejectedObservations(isRejected, flagged);
}

/// \returns a \p vector of \p TrackObservations that all hold a \p shared_ptr to an instance
/// of \p TrackStatistics, which holds all of the track-specific counters.
std::vector<TrackCheckShip::TrackObservation> TrackCheckShip::collectTrackObservations(
    std::vector<size_t>::const_iterator trackObsIndicesBegin,
    std::vector<size_t>::const_iterator trackObsIndicesEnd,
    const std::vector<size_t> &validObsIds,
    const TrackCheckUtils::ObsGroupLocationTimes &obsLocTime) const {
  std::vector<TrackObservation> trackObservations;
  trackObservations.reserve(trackObsIndicesEnd - trackObsIndicesBegin);
  std::shared_ptr<TrackStatistics> trackStatistics(new TrackStatistics());
  std::shared_ptr<TrackCheckUtils::CheckCounter> checkCounter(new TrackCheckUtils::CheckCounter);
  size_t observationNumber = 0;
  for (std::vector<size_t>::const_iterator it = trackObsIndicesBegin;
       it != trackObsIndicesEnd; ++it) {
    const size_t obsId = validObsIds[*it];
    trackObservations.push_back(TrackObservation(obsLocTime.latitudes[obsId],
                                                 obsLocTime.longitudes[obsId],
                                                 obsLocTime.datetimes[obsId], trackStatistics,
                                                 checkCounter,
                                                 observationNumber));
    observationNumber++;
  }
  return trackObservations;
}

/// \brief \returns true if at least half of the track segments have
/// incremented the relevant rejection counters
///
/// This filter is best for mostly-accurate observation tracks. If the track has
/// many "errors" (as indicated by the counters that are incremented before
/// any observations are removed), it will stop early by default.
///
/// Sometimes caused by two ships with same callsign or various reports with wrong time,
/// the check gives up. This is particularly a problem with WOD01 data - case studies
/// suggest that most suspect data is reasonable.
bool TrackCheckShip::earlyBreak(const std::vector<std::reference_wrapper<TrackObservation>>
                                &trackObs, const std::string trackId) const {
  bool breakResult = false;
  const auto& trackStats = *(trackObs[0].get().getFullTrackStatistics());
  // if at least half of the track segments have a time difference of less than an hour
  // (if non-buoy), are faster than a configured maximum speed, or exhibit at least a 90
  // degree bend
  if ((2 * ((options_.inputCategory.value() != SurfaceObservationSubtype::BUOY &&
             options_.inputCategory.value() != SurfaceObservationSubtype::BUOYPROF)
            * trackStats.numShort_ + trackStats.numFast_) + trackStats.numBends_)
      >= (trackObs.size() - 1)) {
    oops::Log::trace() << "ShipTrackCheck: " << trackId << "\n" <<
                          "Time difference < 1 hour: " << trackStats.numShort_ << "\n" <<
                          "Fast: " << trackStats.numFast_ << "\n" <<
                          "Bends: " << trackStats.numBends_ << "\n" <<
                          "Total observations: " << trackObs.size() << "\n" <<
                          "Track was not checked." << std::endl;

    breakResult = true;
  }
  if (options_.testingMode)
    diagnostics_->storeEarlyBreakResult(breakResult);
  return breakResult;
}

/// \brief Chooses which of the observations surrounding the speediest segment to remove,
/// flagging it accordingly.
void TrackCheckShip::removeFaultyObservation(
    std::vector<std::reference_wrapper<TrackObservation>> &track,
    const std::vector<std::reference_wrapper<TrackObservation>>::iterator
    &observationAfterFastestSegment,
    bool firstIterativeRemoval, const std::string trackId) const {
  int errorCategory = 0;
  util::Duration four_days{"P4D"};
  auto rejectedObservation = observationAfterFastestSegment;
  // lambda function to "fail" an observation that should be rejected
  auto fail = [&rejectedObservation](
      const std::vector<std::reference_wrapper<TrackObservation>>::iterator &iter) {
    iter->get().setRejected(true);
    rejectedObservation = iter;
  };
  auto neighborObservationStatistics = [&observationAfterFastestSegment](int index) ->
      const ObservationStatistics & {
    return (observationAfterFastestSegment + index)->get().getObservationStatistics();
  };
  auto meanSpeed = observationAfterFastestSegment->get().getFullTrackStatistics()->meanSpeed_;
  if (observationAfterFastestSegment == track.begin() + 1) {
    // Decide whether ob 0 or 1 agrees best with ob 2
    if (neighborObservationStatistics(0).speedAveraged <=
        options_.core.maxSpeed &&
        (neighborObservationStatistics(1).speed >
         options_.core.maxSpeed ||
         neighborObservationStatistics(1).angle > 45.0)) {
      fail(observationAfterFastestSegment);
      errorCategory = 2;
    } else {
      fail(observationAfterFastestSegment - 1);
      errorCategory = 1;
    }
  } else if (observationAfterFastestSegment == track.end() - 1) {
    if (neighborObservationStatistics(-1).speedAveraged <=
        options_.core.maxSpeed &&
        (neighborObservationStatistics(-1).speed >
         options_.core.maxSpeed ||
         neighborObservationStatistics(-2).angle > 45.0)) {
      fail(observationAfterFastestSegment - 1);
      errorCategory = 2;
    } else {
      fail(observationAfterFastestSegment);
      errorCategory = 1;
    }
  } else if (neighborObservationStatistics(-1).speed >
             options_.core.maxSpeed) {
    fail(observationAfterFastestSegment - 1);
    errorCategory = 4;
    //  Category 4: both segments surrounding observation have excessive speed
  } else if (neighborObservationStatistics(1).speed >
             options_.core.maxSpeed) {
    fail(observationAfterFastestSegment);
    errorCategory = 4;
  } else if (neighborObservationStatistics(0).speedAveraged >
             options_.core.maxSpeed) {
    fail(observationAfterFastestSegment - 1);
    errorCategory = 5;
    // Category 5: observation before fastest segment would still begin a fast segment
    // if observation after fastest segment were removed
  } else if (neighborObservationStatistics(-1).speedAveraged >
             options_.core.maxSpeed) {
    fail(observationAfterFastestSegment);
    errorCategory = 5;
    // Category 5: observation after fastest segment would still end a fast segment if
    // observation before fastest segment were removed
  } else if (neighborObservationStatistics(-1).angle >
             (45.0 + neighborObservationStatistics(0).angle)) {
    fail(observationAfterFastestSegment - 1);
    errorCategory = 6;
  } else if (neighborObservationStatistics(0).angle >
             45.0 + neighborObservationStatistics(-1).angle) {
    fail(observationAfterFastestSegment);
    errorCategory = 6;
  } else if (neighborObservationStatistics(-2).angle > 45.0 &&
             neighborObservationStatistics(-2).angle >
             neighborObservationStatistics(1).angle) {
    fail(observationAfterFastestSegment - 1);
    errorCategory = 7;
  } else if (neighborObservationStatistics(1).angle > 45.0) {
    fail(observationAfterFastestSegment);
    errorCategory = 7;
  } else if (neighborObservationStatistics(-1).speed <
             0.5 * std::min(
               neighborObservationStatistics(1).speed,
               meanSpeed)) {
    fail(observationAfterFastestSegment - 1);
    errorCategory = 8;
  } else if (neighborObservationStatistics(1).speed <
             0.5 * std::min(neighborObservationStatistics(-1).speed, meanSpeed)) {
    fail(observationAfterFastestSegment);
    errorCategory = 8;
  } else {
    double distanceSum = 0.0;
    distanceSum = std::accumulate(observationAfterFastestSegment - 1,
                    observationAfterFastestSegment + 2, distanceSum,
                    [](double summed, TrackObservation& obs) {
      return summed + obs.getObservationStatistics().distance;
    });

    double distancePrevObsOmitted =
          neighborObservationStatistics(-1).distanceAveraged +
        neighborObservationStatistics(1).distance;
    double distanceCurrentObsOmitted =
          neighborObservationStatistics(-1).distance +
        neighborObservationStatistics(0).distanceAveraged;
    util::Duration timeSum = (observationAfterFastestSegment + 1)->get().getTime() - (
          observationAfterFastestSegment - 2)->get().getTime();
    if (options_.testingMode.value()) {
      diagnostics_->storeDistanceSum(distanceSum);
      diagnostics_->storeDistancePrevObsOmitted(distancePrevObsOmitted);
      diagnostics_->storeDistanceCurrentObsOmitted(distanceCurrentObsOmitted);
      double timeDouble = timeSum.toSeconds();
      diagnostics_->storeTimeSum(timeDouble);
    }
    if (distancePrevObsOmitted < distanceCurrentObsOmitted - std::max(
          options_.core.spatialResolution.value(), 0.1 * distanceSum)) {
      fail(observationAfterFastestSegment - 1);
      errorCategory = 9;
    } else if (distanceCurrentObsOmitted < (
                 distancePrevObsOmitted - std::max(
                   options_.core.spatialResolution.value(), 0.1 * distanceSum))) {
      fail(observationAfterFastestSegment);
      errorCategory = 9;
    } else if (timeSum <= four_days && timeSum.toSeconds() > 0 &&
               std::min(distancePrevObsOmitted, distanceCurrentObsOmitted) > 0.0) {
      double previousSegmentDistanceProportion =
          // Prev segment dist/(prev segment distance + distAveragedCurrentObservation)
          neighborObservationStatistics(-1).distance /
          distanceCurrentObsOmitted;
      double previousObservationDistanceAveragedProportion =
          // Previous observation distAveraged / (prev obs distAveraged + next segment distance)
          neighborObservationStatistics(-1).
          distanceAveraged / distancePrevObsOmitted;
      double previousSegmentTimeProportion =
          static_cast<double>(((observationAfterFastestSegment - 1)->get().getTime() -
           (observationAfterFastestSegment - 2)->get().getTime()).toSeconds()) /
          timeSum.toSeconds();
      double previousAndFastestSegmentTimeProportion =
          static_cast<double>(((observationAfterFastestSegment->get().getTime()) -
            (observationAfterFastestSegment - 2)->get().getTime()).toSeconds()) /
          timeSum.toSeconds();
      if (options_.testingMode.value()) {
        diagnostics_->storePreviousSegmentDistanceProportion(previousSegmentDistanceProportion);
        diagnostics_->storePreviousObservationDistanceAveragedProportion(
              previousObservationDistanceAveragedProportion);
        diagnostics_->storePreviousSegmentTimeProportion(previousSegmentTimeProportion);
        diagnostics_->storePreviousAndFastestSegmentTimeProportion(
              previousAndFastestSegmentTimeProportion);
      }
      if (std::abs(previousSegmentDistanceProportion - previousSegmentTimeProportion) >
          0.1 + std::abs(previousObservationDistanceAveragedProportion -
                         previousAndFastestSegmentTimeProportion)) {
        // previous segment's spatial and temporal lengths are significantly more disproportionate
        // when compared to a larger portion of track
        // than the equivalents for the post-fastest segment
        fail(observationAfterFastestSegment - 1);
        errorCategory = 10;
      } else if (std::abs(previousObservationDistanceAveragedProportion -
                          previousAndFastestSegmentTimeProportion) > 0.1 +
                 std::abs(previousSegmentDistanceProportion - previousSegmentTimeProportion)) {
        // next segment after fastest has spatial and temporal lengths that are significantly more
        // disproportionate than the segment before the fastest
        fail(observationAfterFastestSegment);
        errorCategory = 10;
      } else {
        fail(observationAfterFastestSegment);
      }
      oops::Log::trace() << "CheckShipTrack: proportions " << previousSegmentDistanceProportion <<
                            " " << previousSegmentTimeProportion <<
                            " " << previousObservationDistanceAveragedProportion << " "
                         << previousAndFastestSegmentTimeProportion << " speeds: " << meanSpeed
                         << " " << neighborObservationStatistics(-1).speed << " " <<
                            neighborObservationStatistics(0).speed << " " <<
                            neighborObservationStatistics(1).speed << " [m/s]" << std::endl;
    }
    if (errorCategory == 9 || std::min(distancePrevObsOmitted, distanceCurrentObsOmitted) == 0.0) {
      oops::Log::trace() << "CheckShipTrack: Dist check, station id: " <<
                            trackId << std::endl <<
                            " error category: " << errorCategory << std::endl <<
                            " distances: " << distanceSum * 0.001 << " " <<
                            distancePrevObsOmitted * 0.001 << " " <<
                            distanceCurrentObsOmitted * 0.001 << " " <<
                            (distancePrevObsOmitted - distanceCurrentObsOmitted) * 0.001 <<
                            " " << std::max(options_.core.spatialResolution.value(),
                                              0.1 * distanceSum) *
                            0.001 << "[km]" << std::endl;
    }
  }
  if (errorCategory == 0 || ((rejectedObservation->get().getObservationStatistics().
                              speedAveraged) >
                             options_.core.maxSpeed.value())) {
    oops::Log::trace() << "CheckShipTrack: cannot decide between station id " <<
                          trackId << " observations " <<
                          (observationAfterFastestSegment - 1)->get().getObservationNumber() <<
                          " " << observationAfterFastestSegment->get().getObservationNumber() <<
                          " rejecting both." << std::endl;
    errorCategory += 100;
    if (options_.testingMode.value() && firstIterativeRemoval) {
      std::vector<size_t> observationNumbersAroundFastest{
        (observationAfterFastestSegment - 1)->get().getObservationNumber(),
            observationAfterFastestSegment->get().getObservationNumber()};
      diagnostics_->storeFirstIterativeRemovalInfo(
            std::make_pair(observationNumbersAroundFastest, errorCategory));
    }
    if (rejectedObservation == observationAfterFastestSegment) {
      fail(observationAfterFastestSegment - 1);
    } else {
      fail(observationAfterFastestSegment);
    }
    track.erase(observationAfterFastestSegment - 1, observationAfterFastestSegment);
  } else {
    if (options_.testingMode.value() && firstIterativeRemoval) {
      std::vector<size_t> rejectedObservationNumber{rejectedObservation->get().
            getObservationNumber()};
      diagnostics_->storeFirstIterativeRemovalInfo(
            std::make_pair(rejectedObservationNumber,
                           errorCategory));
    }
    oops::Log::trace() << "CheckShipTrack: rejecting station " << trackId << " observation " <<
                          rejectedObservation->get().getObservationNumber() << "\n" <<
                          "Error category: " << errorCategory << "\n" <<
                          "rejection candidates: " <<
                          (observationAfterFastestSegment - 1)->get().getObservationNumber() <<
                          " " << observationAfterFastestSegment->get().getObservationNumber() <<
                          "\n" << "speeds: " << (observationAfterFastestSegment - 1)->get().
                          getObservationStatistics().speed << " " <<
                          observationAfterFastestSegment->get().getObservationStatistics().speed <<
                          "\n" << (observationAfterFastestSegment - 1)->get().
                          getObservationStatistics().angle << " " <<
                          observationAfterFastestSegment->get().
                          getObservationStatistics().angle << "\n";
    track.erase(rejectedObservation);
  }
}
/// \todo Trace output will need to be changed to match that of OPS (indices, LWin)

/// \brief Calculates all of the statistics that require only two
/// adjacent \p TrackObservations, storing within the righthand observation.
///
/// This includes
/// distance between the two observations,
/// time difference between the observations, speed between the
/// observations, and if the
/// observations are recorded for the same time. Calls function to
/// increment track-wise counters based on
/// results.
void TrackCheckShip::TrackObservation::calculateTwoObservationValues(
    TrackObservation& prevObs, bool firstIteration,
    const TrackCheckShipParameters &options) {
  this->setDistance(TrackCheckShip::distance(prevObs, *this));
  (this->observationStatistics_.distance > options.core.spatialResolution) ?
        this->setSpeed(TrackCheckShip::speedEstimate(*this, prevObs, options)) :
        this->setSpeed(0.0);
  this->setTimeDifference(getTime() - prevObs.getTime());
  if (firstIteration) {
    adjustTwoObservationStatistics(options);
  }
}

void TrackCheckShip::TrackObservation::resetObservationCalculations() {
  this->setDistance(0.0);
  this->setSpeed(0.0);
  this->setDistanceAveraged(0.0);
  this->setSpeedAveraged(0.0);
  this->setAngle(0.0);
}

/// Calculates all of the statistics that require three
/// consecutive \p TrackObservations,
/// storing within the middle observation. This includes
/// distance between two alternating observations,
/// speed between these alternating observations (if the middle
/// observation was not recorded), and the
/// angle formed by the three-observation track segment.
/// Calls function to increment track-wise counters based on results.
void TrackCheckShip::TrackObservation::calculateThreeObservationValues(
    const TrackObservation& prevObs, const TrackObservation& nextObs,
    bool firstIteration, const TrackCheckShipParameters &options) {
  this->setDistanceAveraged(TrackCheckShip::distance(prevObs, nextObs));
  this->setSpeedAveraged(speedEstimate(prevObs, nextObs, options));
  if (std::min(this->observationStatistics_.distance,
               nextObs.observationStatistics_.distance) >
      options.core.spatialResolution) {
    this->observationStatistics_.angle = angle(prevObs, *this, nextObs);
  }
  if (firstIteration) {
    adjustThreeObservationStatistics();
  }
}

/// This performs all of the necessary calculations based on the
/// observations' locations and timestamps by calling
/// \p calculateTwoObservationValues and \p calculateThreeObservationValues for the
/// non-edge-case observations.
void TrackCheckShip::calculateTrackSegmentProperties(
    const std::vector<std::reference_wrapper<TrackObservation>> &trackObservations,
    CalculationMethod calculationMethod) const {
  if (trackObservations.size()) {
    if (calculationMethod == MAINLOOP)
      trackObservations[0].get().resetObservationCalculations();
    for (size_t obsIdx = 1; obsIdx < trackObservations.size(); obsIdx++) {
      TrackObservation &obs = trackObservations[obsIdx].get();
      TrackObservation &prevObs = trackObservations[obsIdx - 1].get();
      obs.calculateTwoObservationValues(prevObs, calculationMethod == FIRSTITERATION, options_);
      if (obsIdx > 1) {
        const TrackObservation &prevPrevObs = trackObservations[obsIdx - 2].get();
        prevObs.calculateThreeObservationValues(prevPrevObs, obs,
                                                calculationMethod == FIRSTITERATION, options_);
      }
      if (calculationMethod == FIRSTITERATION && (obsIdx == trackObservations.size() - 1)) {
        int potentialDenominator = trackObservations.size() - 1 -
            obs.getFullTrackStatistics()->numShort_ - obs.getFullTrackStatistics()->numFast_;
        (obs.getFullTrackStatistics()->meanSpeed_) = (obs.getFullTrackStatistics()->sumSpeed_) /
            std::max(1, potentialDenominator);
      }
    }
    if (options_.testingMode.value() && calculationMethod != MAINLOOP) {
      std::vector<TrackCheckShip::ObservationStatistics> obsStats;
      for (size_t obsIdx = 0; obsIdx < trackObservations.size(); ++obsIdx) {
        obsStats.push_back(trackObservations[obsIdx].get().getObservationStatistics());
      }
      auto trackStats = *(trackObservations[0].get().getFullTrackStatistics());
      if (calculationMethod == FIRSTITERATION)
        diagnostics_->storeInitialCalculationResults(std::make_pair(obsStats, trackStats));
    }
  }
}

/// \brief Keeps track of 0-distanced, short, and fast track segments,
/// as well as incrementing \p sumSpeed_ for normal track segments.
void TrackCheckShip::TrackObservation::adjustTwoObservationStatistics
(const TrackCheckShipParameters &options) const {
  util::Duration hour{"PT1H"};
  if (getObservationStatistics().timeDifference < hour) {
    getFullTrackStatistics()->numShort_++;
  } else if (getObservationStatistics().speed >= options.core.maxSpeed) {
    getFullTrackStatistics()->numFast_++;
  } else {
    getFullTrackStatistics()->sumSpeed_ += getObservationStatistics().speed;
  }
}

/// \brief Increments number of bends if angle is greater than or equal to 90 degrees
void TrackCheckShip::TrackObservation::adjustThreeObservationStatistics() const {
  if (getObservationStatistics().angle >= 90.0)
    getFullTrackStatistics()->numBends_++;
}

const TrackCheckShip::ObservationStatistics &
TrackCheckShip::TrackObservation::getObservationStatistics() const
{
  return observationStatistics_;
}

const std::shared_ptr<TrackCheckShip::TrackStatistics>
TrackCheckShip::TrackObservation::getFullTrackStatistics() const
{
  return fullTrackStatistics_;
}

void TrackCheckShip::TrackObservation::setDistance(double dist) {
  this->observationStatistics_.distance = dist;
}
void TrackCheckShip::TrackObservation::setTimeDifference(util::Duration tDiff) {
  this->observationStatistics_.timeDifference = tDiff;
}
void TrackCheckShip::TrackObservation::setSpeed(double speed) {
  this->observationStatistics_.speed = speed;
}
void TrackCheckShip::TrackObservation::setAngle(double angle) {
  this->observationStatistics_.angle = angle;
}
void TrackCheckShip::TrackObservation::setDistanceAveraged(double distAvg) {
  this->observationStatistics_.distanceAveraged = distAvg;
}
void TrackCheckShip::TrackObservation::setSpeedAveraged(double speedAvg) {
  this->observationStatistics_.speedAveraged = speedAvg;
}
}  // namespace ufo


// Copyright (c) 2020-2021 Franz Alt
// This code is licensed under MIT license (see LICENSE.txt for details).

#ifndef LIBCVPG_IMAGEPROC_ALGORITHMS_K_MEANS_HPP
#define LIBCVPG_IMAGEPROC_ALGORITHMS_K_MEANS_HPP

#include <boost/asynchronous/continuation_task.hpp>

#include <libcvpg/core/image.hpp>

namespace cvpg::imageproc::algorithms {

boost::asynchronous::detail::callback_continuation<image_gray_8bit> k_means(image_gray_8bit image, std::size_t k, std::size_t max_iterations, std::uint8_t eps);

boost::asynchronous::detail::callback_continuation<image_rgb_8bit> k_means(image_rgb_8bit image, std::size_t k, std::size_t max_iterations, std::uint8_t eps);

} // namespace cvpg::imageproc::algoritms

#endif // LIBCVPG_IMAGEPROC_ALGORITHMS_K_MEANS_HPP


/*-----------------------------------------------------------------------------+
Copyright (c) 2010-2010: Joachim Faulhaber
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/
#ifndef BOOST_ICL_TYPE_TRAITS_IS_NUMERIC_HPP_JOFA_100322
#define BOOST_ICL_TYPE_TRAITS_IS_NUMERIC_HPP_JOFA_100322

#include <limits>
#include <complex>
#include <functional>
#include <boost/type_traits/is_floating_point.hpp>
#include <boost/type_traits/ice.hpp>
#include <boost/type_traits/is_integral.hpp>

namespace boost{ namespace icl
{

template <class Type> struct is_fixed_numeric
{
    typedef is_fixed_numeric type;
    BOOST_STATIC_CONSTANT(bool, value = (0 < std::numeric_limits<Type>::digits));
};

template <class Type> struct is_std_numeric
{
    typedef is_std_numeric type;
    BOOST_STATIC_CONSTANT(bool,
        value = (std::numeric_limits<Type>::is_specialized));
};

template <class Type> struct is_std_integral
{
    typedef is_std_integral type;
    BOOST_STATIC_CONSTANT(bool,
        value = (std::numeric_limits<Type>::is_integer));
};

template <class Type> struct is_numeric
{
    typedef is_numeric type;
    BOOST_STATIC_CONSTANT(bool, value =
        (mpl::or_< is_std_numeric<Type>
                 , boost::is_integral<Type>
                 , is_std_integral<Type> >::value) );
};

template <class Type>
struct is_numeric<std::complex<Type> >
{
    typedef is_numeric type;
    BOOST_STATIC_CONSTANT(bool, value = true);
};

//--------------------------------------------------------------------------
template<class Type, class Compare, bool Enable = false>
struct numeric_minimum
{
    static bool is_less_than(Type){ return true; }
    static bool is_less_than_or(Type, bool){ return true; }
};

template<class Type>
struct numeric_minimum<Type, std::less<Type>, true>
{
    static bool is_less_than(Type value)
    { return std::less<Type>()((std::numeric_limits<Type>::min)(), value); }

    static bool is_less_than_or(Type value, bool cond)
    { return cond || is_less_than(value); }
};

template<class Type>
struct numeric_minimum<Type, std::greater<Type>, true>
{
    static bool is_less_than(Type value)
    { return std::greater<Type>()((std::numeric_limits<Type>::max)(), value); }

    static bool is_less_than_or(Type value, bool cond)
    { return cond || is_less_than(value); }
};

//--------------------------------------------------------------------------
template<class Type>
struct is_non_floating_point
{
    typedef is_non_floating_point type;
    BOOST_STATIC_CONSTANT(bool, value =
        (mpl::not_< is_floating_point<Type> >::value));
};

}} // namespace boost icl

#endif


#include <stdlib.h>
#include <stdio.h>
#include <vector>
#include <string>
#include <sstream>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <stdexcept>
#include <functional>
#include <random>
#include <Eigen/Geometry>
#include <visualization_msgs/Marker.h>
#include <arc_utilities/eigen_helpers.hpp>
#include <arc_utilities/eigen_helpers_conversions.hpp>
#include <arc_utilities/pretty_print.hpp>
#include <arc_utilities/voxel_grid.hpp>
#include <arc_utilities/simple_rrt_planner.hpp>
#include <uncertainty_planning_core/simple_pid_controller.hpp>
#include <uncertainty_planning_core/simple_uncertainty_models.hpp>
#include <uncertainty_planning_core/uncertainty_contact_planning.hpp>
#include <uncertainty_planning_examples/SetSimple6dofActuationError.h>
#include <uncertainty_planning_examples/se3_common_config.hpp>
#include <ros/ros.h>
#include <visualization_msgs/MarkerArray.h>
#include <geometry_msgs/PoseStamped.h>
#include <uncertainty_planning_examples/Simple6dofRobotMove.h>

using namespace uncertainty_contact_planning;

inline EigenHelpers::VectorIsometry3d move_robot(const Eigen::Isometry3d& target_transform, const Eigen::Isometry3d& expected_result_configuration, const double duration, const double execution_shortcut_distance, const bool reset, ros::ServiceClient& robot_control_service)
{
    UNUSED(expected_result_configuration);
    const geometry_msgs::PoseStamped target_pose = EigenHelpersConversions::EigenIsometry3dToGeometryPoseStamped(target_transform, "world");
    // Put together service call
    uncertainty_planning_examples::Simple6dofRobotMove::Request req;
    req.time_limit = ros::Duration(duration);
    req.execution_shortcut_distance = execution_shortcut_distance;
    if (reset)
    {
        std::cout << "Resetting robot to transform: " << PrettyPrint::PrettyPrint(target_transform) << std::endl;
        req.start = target_pose;
        req.mode = uncertainty_planning_examples::Simple6dofRobotMove::Request::RESET;
    }
    else
    {
        std::cout << "Commanding robot to transform: " << PrettyPrint::PrettyPrint(target_transform) << std::endl;
        req.target = target_pose;
        req.mode = uncertainty_planning_examples::Simple6dofRobotMove::Request::EXECUTE;
    }
    uncertainty_planning_examples::Simple6dofRobotMove::Response res;
    // Call service
    try
    {
        robot_control_service.call(req, res);
    }
    catch (...)
    {
        ROS_ERROR("Move service failed");
    }
    // Unpack result
    const std::vector<geometry_msgs::PoseStamped>& poses = res.trajectory;
    EigenHelpers::VectorIsometry3d transforms(poses.size());
    for (size_t idx = 0; idx < poses.size(); idx++)
    {
        transforms[idx] = EigenHelpersConversions::GeometryPoseToEigenIsometry3d(poses[idx].pose);
    }
    std::cout << "Reached transform: " << PrettyPrint::PrettyPrint(transforms.back()) << " after " << transforms.size() << " steps" << std::endl;
    return transforms;
}

void set_uncertainty(const double max_translation_error, const double max_rotation_error, ros::ServiceClient& set_uncertainty_service)
{
    uncertainty_planning_examples::SetSimple6dofActuationErrorRequest req;
    req.translation_actuator_error = max_translation_error;
    req.rotation_actuation_error = max_rotation_error;
    uncertainty_planning_examples::SetSimple6dofActuationErrorResponse res;
    if (set_uncertainty_service.call(req, res) == false)
    {
        throw std::invalid_argument("SetActuationError failed");
    }
}

void peg_in_hole_env_se3(ros::Publisher& display_debug_publisher, ros::ServiceClient& robot_control_service, ros::ServiceClient& set_uncertainty_service)
{
    const uncertainty_planning_core::PLANNING_AND_EXECUTION_OPTIONS options = se3_common_config::GetOptions();
    const config_common::TASK_CONFIG_PARAMS extra_options = se3_common_config::GetExtraOptions();
    std::cout << PrettyPrint::PrettyPrint(options) << "\n" << PrettyPrint::PrettyPrint(extra_options) << std::endl;
    const std::pair<uncertainty_planning_core::SE3Config, uncertainty_planning_core::SE3Config> start_and_goal = se3_common_config::GetStartAndGoal();
    const uncertainty_planning_core::SE3SamplerPtr sampler = se3_common_config::GetSampler();
    const simple_robot_models::SE3_ROBOT_CONFIG robot_config = se3_common_config::GetDefaultRobotConfig(extra_options);
    const uncertainty_planning_core::SE3Robot robot = se3_common_config::GetRobot(robot_config);
    const uncertainty_planning_core::SE3SimulatorPtr simulator = se3_common_config::GetSimulator(extra_options, options.debug_level);
    // Load the policy
    try
    {
        auto policy = uncertainty_planning_core::LoadSE3Policy(options.planned_policy_file);
        policy.SetPolicyActionAttemptCount(options.policy_action_attempt_count);
        std::map<std::string, double> complete_policy_stats;
    #ifndef FORCE_DEBUG
        std::cout << "Press ENTER to simulate policy..." << std::endl;
        std::cin.get();
    #endif
        const auto policy_simulation_results = uncertainty_planning_core::SimulateSE3UncertaintyPolicy(options, robot, simulator, sampler, policy, start_and_goal.first, start_and_goal.second, display_debug_publisher);
        const std::map<std::string, double> policy_simulation_stats = policy_simulation_results.second.first;
        const std::vector<int64_t> policy_simulation_step_counts = policy_simulation_results.second.second.first;
        std::cout << "Policy simulation success: " << PrettyPrint::PrettyPrint(policy_simulation_stats) << std::endl;
        complete_policy_stats.insert(policy_simulation_stats.begin(), policy_simulation_stats.end());
    #ifndef FORCE_DEBUG
        std::cout << "Press ENTER to execute policy..." << std::endl;
        std::cin.get();
    #endif
        if (options.num_policy_executions > 0)
        {
            std::cout << "Setting actuation uncertainty..." << std::endl;
            set_uncertainty(robot_config.max_actuator_noise, robot_config.r_max_actuator_noise, set_uncertainty_service);
        }
        std::function<EigenHelpers::VectorIsometry3d(const Eigen::Isometry3d&, const Eigen::Isometry3d&, const double, const double, const bool)> robot_execution_fn = [&] (const Eigen::Isometry3d& target_configuration, const Eigen::Isometry3d& expected_result_configuration, const double duration, const double execution_shortcut_distance, const bool reset) { return move_robot(target_configuration, expected_result_configuration, duration, execution_shortcut_distance, reset, robot_control_service); };
        const auto policy_execution_results = uncertainty_planning_core::ExecuteSE3UncertaintyPolicy(options, robot, simulator, sampler, policy, start_and_goal.first, start_and_goal.second, robot_execution_fn, display_debug_publisher);
        const std::map<std::string, double> policy_execution_stats = policy_execution_results.second.first;
        const std::vector<int64_t> policy_execution_step_counts = policy_execution_results.second.second.first;
        const std::vector<double> policy_execution_times = policy_execution_results.second.second.second;
        std::cout << "Policy execution success: " << PrettyPrint::PrettyPrint(policy_execution_stats) << std::endl;
        complete_policy_stats.insert(policy_execution_stats.begin(), policy_execution_stats.end());
        // Save the executed policy
        uncertainty_planning_core::SaveSE3Policy(policy_execution_results.first, options.executed_policy_file);
        // Print out the results & save them to the log file
        const std::string log_results = "++++++++++\n" + PrettyPrint::PrettyPrint(options) + "\n" + PrettyPrint::PrettyPrint(extra_options) + "\nRESULTS:\n" + PrettyPrint::PrettyPrint(complete_policy_stats, false, "\n") + "\nSimulation step counts: " + PrettyPrint::PrettyPrint(policy_simulation_step_counts) + "\nExecution step counts: " + PrettyPrint::PrettyPrint(policy_execution_step_counts) + "\nExecution times: " + PrettyPrint::PrettyPrint(policy_execution_times);
        std::cout << "Policy results:\n" << log_results << std::endl;
        std::ofstream log_file(options.policy_log_file, std::ios_base::out | std::ios_base::app);
        if (!log_file.is_open())
        {
            std::cerr << "\x1b[31;1m Unable to create folder/file to log to: " << options.policy_log_file << "\x1b[37m \n";
            throw std::invalid_argument( "Log filename must be write-openable" );
        }
        log_file << log_results << std::endl;
        log_file.close();
    }
    catch (...)
    {
        std::cout << "!!! Policy file does not exist, skipping !!!" << std::endl;
        const std::string log_results = "++++++++++\n" + PrettyPrint::PrettyPrint(options) + "\n" + PrettyPrint::PrettyPrint(extra_options) + "\nRESULTS:\n(Execution) Policy success: 0\n(Simulation) Policy success: 0\n(Simulation) Policy successful simulator resolves: 0\n(Simulation) Policy unsuccessful simulator environment resolves: 0\n(Simulation) Policy unsuccessful simulator resolves: 0\n(Simulation) Policy unsuccessful simulator self-collision resolves: 0\nSimulation step counts: -1\nExecution step counts: -1\nExecution times: -0.0";
        std::ofstream log_file(options.policy_log_file, std::ios_base::out | std::ios_base::app);
        if (!log_file.is_open())
        {
            std::cerr << "\x1b[31;1m Unable to create folder/file to log to: " << options.policy_log_file << "\x1b[37m \n";
            throw std::invalid_argument( "Log filename must be write-openable" );
        }
        log_file << log_results << std::endl;
        log_file.close();
    }
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "se3_contact_execution_node");
    ros::NodeHandle nh;
    ROS_INFO("Starting SE(3) Contact Execution Node...");
    ros::Publisher display_debug_publisher = nh.advertise<visualization_msgs::MarkerArray>("uncertainty_planning_debug_display_markers", 1, true);
    ros::ServiceClient robot_control_service = nh.serviceClient<uncertainty_planning_examples::Simple6dofRobotMove>("simple_6dof_robot_move");
    ros::ServiceClient set_uncertainty_service = nh.serviceClient<uncertainty_planning_examples::SetSimple6dofActuationError>("simple6dof_robot/set_actuation_uncertainty");
    peg_in_hole_env_se3(display_debug_publisher, robot_control_service, set_uncertainty_service);
    return 0;
}


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

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

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

#include <boost/simd/config.hpp>
#include <boost/simd/function/definition/compare_equal.hpp>
#include <boost/simd/detail/dispatch.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
    using compare_eq_ = compare_equal_;
  }

  BOOST_DISPATCH_CALLABLE_DEFINITION(tag::compare_eq_,compare_eq);
} }

#endif


/*

Software License Agreement (BSD License)

Copyright (c) 2016--, Liana Bertoni (liana.bertoni@gmail.com)
  All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

* Redistributions of source code must retain the above copyright notice, this
  list of conditions and the following disclaimer.

* Redistributions in binary form must reproduce the above copyright notice,
  this list of conditions and the following disclaimer in the documentation
  and/or other materials provided with the distribution.

* Neither the name of the copyright holder(s) nor the names of its
  contributors may be used to endorse or promote products derived from
  this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
Contact GitHub API Training Shop Blog About
*/

#include <ros/ros.h>
#include <ros/package.h>
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose.h>

#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>

#include <kdl/chain.hpp>
#include <kdl/chainfksolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/frames_io.hpp>

#include <boost/scoped_ptr.hpp>
#include <kdl/chainjnttojacsolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>

#include <kdl_parser/kdl_parser.hpp>
#include <urdf/model.h>

#include <iostream>
#include <fstream>
#include <sstream>
#include <cstdlib>
#include <string>

#include <math.h>
#include <stdio.h>
#include <ctime>
#include <time.h>

using namespace std;
using namespace Eigen;
using namespace KDL;

int main (int argc, char **argv)
{

	ros::init(argc, argv, "svm_predict");	// ROS node
	ros::NodeHandle nh;

	string relative_path_file_in;
	string file_name_in;

	string relative_path_file_model;
	string file_name_model;

	string relative_path_file_out;
	string file_name_out;

	nh.param<std::string>("filename_in", relative_path_file_in, "/box_test/test" );
	nh.param<std::string>("filename_model", relative_path_file_model, "/svm_model/box1model");
	nh.param<std::string>("filename_out", relative_path_file_out, "/box_estimate/" );

	///////////////////// load the data_base ////////////////////////////////////
	std::string path = ros::package::getPath("grasp_learning");

	file_name_in = path + relative_path_file_in;//input box
	ifstream file_in(file_name_in);

	std::cout << "file: " << file_name_in.c_str() << " is " << (file_in.is_open() == true ? "already" : "not") << " open" << std::endl;
	if(!file_in.is_open())
	return 0;

	file_name_model = path + relative_path_file_model;//input model
	ifstream file_model(file_name_model);

	std::cout << "file: " << file_name_model.c_str() << " is " << (file_model.is_open() == true ? "already" : "not") << " open" << std::endl;
	if(!file_model.is_open())
	return 0;

  ofstream file_output; //output file
  file_name_out = path + relative_path_file_out;
  std::string name = "box_estimate";
  file_output.open( file_name_out + name, ofstream::app);

  ofstream file_output_csv; //output file

  std::string name_csv = "box_estimate.csv";
  file_output_csv.open( file_name_out + name_csv, ofstream::app);

  ////////////////////////////////////////////////////////////////////////////////////

  int n_sv = 1;
  int dim_cols = 0;

  ///////////////////////////////// get values //////////////////////////////////
  std::string line_model;
  getline( file_model, line_model, '\n' ); //count cols
  std::istringstream iss_line_model(line_model);

  for(std::string value_model; getline(iss_line_model, value_model, ' ' ); )
  	dim_cols++;

  for(std::string line_model; getline( file_model, line_model, '\n' ); )
		n_sv++;
	////////////////////////////////////////////////////////////////////////////////

  cout << "N_sv : " << n_sv << endl;
  cout << "Dim_cols : " << dim_cols << endl;

	Eigen::MatrixXd Model = MatrixXd::Zero(n_sv,dim_cols);

	int n_samples = 1;
  int dim_sample = 0;

  ///////////////////////////////// get values //////////////////////////////////
  std::string line_in;
  getline( file_in, line_in, '\n' ); //count cols
  std::istringstream iss_line_in(line_in);

  for(std::string value_in; getline(iss_line_in, value_in, ' ' ); )
  	dim_sample++;

  for(std::string line; getline( file_in, line_in, '\n' ); )
		n_samples++;
	////////////////////////////////////////////////////////////////////////////////

  cout << "n_samples : " << n_samples << endl;
  cout << "dim_sample : " << dim_sample << endl;

	file_name_model = path + relative_path_file_model;//input model
	ifstream filemodel(file_name_model);

	std::cout << "file: " << file_name_model.c_str() << " is " << (filemodel.is_open() == true ? "already" : "not") << " open" << std::endl;
	if(!file_model.is_open())
	return 0;

	///////////////////////////////// get values //////////////////////////////////
	int i=0;
	int j=0;
	for(std::string linemodel; getline(filemodel, linemodel, '\n' ); )
	{

		//cout << "yyy "<<endl;

    std::istringstream iss_linemodel(linemodel);
    j=0;
    for(std::string valuemodel; getline(iss_linemodel, valuemodel, ' ' ); )
    {
    	//cout << " elem : " << stod(valuemodel) << endl;
    	Model(i,j) = stod(valuemodel);
    	j++;
    }

    i++;

  }

	//cout << "Model : "<< endl << Model << endl;

  ifstream filein(file_name_in);
	std::cout << "file: " << file_name_in.c_str() << " is " << (filein.is_open() == true ? "already" : "not") << " open" << std::endl;
	if(!file_in.is_open())
	return 0;

  Eigen::MatrixXd row = MatrixXd::Zero(1,dim_cols-1); // 11 elem
  Eigen::MatrixXd box_est = MatrixXd::Zero(n_samples,dim_cols-1);

  int n = 0;
	///////////////////////////////// get values //////////////////////////////////
	for(std::string line; getline( filein, line, '\n' ); )
	{
		std::vector<double> values_inline;
    std::istringstream iss_line(line);
    for(std::string value; getline(iss_line, value, ' ' ); )
    	values_inline.push_back(stod(value));

    Eigen::VectorXd X_sv(dim_cols-2);
    Eigen::VectorXd X_test(dim_cols-2);

    for(int i = 0; i < (dim_cols-2); i++)
    	X_test(i) = values_inline[i];

    Eigen::VectorXd dist = VectorXd::Zero(dim_cols-2);
    Eigen::VectorXd arg(1);
    arg << 0;
    double y_est = 0;

    for(int i = 0; i < n_sv ; i++)
    {

    	for(int j=0 ; j< (dim_cols-2) ; j++)
    		X_sv(j) = Model(i,2+j);

    	dist = X_sv - X_test;

    	arg = dist.transpose() * dist;

    	double gamma = Model(i,0);

    	double beta = Model(i,1);

    	double expo = std::exp(-(gamma*arg(0)));

    	double gaussWeight = beta*expo;

    	y_est += gaussWeight;

    	cout << "X_sv " << i << " : " << endl << X_sv.transpose() << endl;
    	cout << "X_test : " << endl << X_test.transpose() << endl;
    	cout << "dist  " << i << " : " <<endl<< dist.transpose() << endl;
    	cout << "arg  " << i << " : " << arg(0) << endl;
    	cout << "gamma " << i << "  : " << gamma << endl;
    	cout << "beta  " << i << " : " << beta << endl;
    	cout << "expo " << i << "  : " << expo << endl;
    	cout << "gaussWeight  " << i << " : " << gaussWeight << endl;
    	cout << "y_est " << i << " : " << y_est << endl;
    }

    cout << "y_est : " << y_est << endl;

    row(0,0) = y_est;

    for(int i = 0 ; i < (dim_cols-2) ; i++)
    	row(0,i+1) = X_test(i);

    box_est.block(n,0,1,11) = row;

    n++;
  }

  Eigen::MatrixXd row_app = MatrixXd::Zero(1,dim_cols-1); // 11 elem

  int max = 0;
  for(i=0; i<n_samples-1; i++)
  {
    max = i;

    for(j=i+1; j<n_samples; j++)
      if(box_est(j,0) > box_est(max,0))
        max = j;

    row_app = box_est.block(max,0,1,11);
    box_est.block(max,0,1,11) = box_est.block(i,0,1,11);
    box_est.block(i,0,1,11) = row_app;
  }

  for(int i = 0 ; i < box_est.rows(); i++)
  {
    for(int j = 0 ; j < box_est.cols(); j++)
    	file_output<<box_est(i,j)<<' ';
   	file_output<<endl;
  }

  for(int i = 0 ; i < box_est.rows(); i++)
  {
    for(int j = 1 ; j < box_est.cols(); j++)
    {
      file_output_csv<<box_est(i,j);

      if(j+1 < box_est.cols())
        file_output_csv<<',';
    }
    file_output_csv<<endl;
  }

  file_output.close();
  file_output_csv.close();

  cout << "HAPPY" << endl;

  ros::shutdown();
	return 0;
}


#include <Workspace.h>
#include <WorkspacePrivate.h>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/depth_first_search.hpp>

typedef boost::adjacency_list<boost::setS, boost::vecS, boost::directedS> Graph;

struct CircleDetector : boost::default_dfs_visitor
{
    bool &hasCircle;

    CircleDetector(bool &value) : hasCircle(value)
    { }

    template <class Edge, class Graph>
    void back_edge(Edge, const Graph &)
    { hasCircle = true; }
};

bool Workspace::hasCircle() const
{
    Graph g(d->edges.begin(), d->edges.end(), 4);

    bool v;
    CircleDetector vis(v);
    boost::depth_first_search(g, visitor(vis));
    return v;
}


#include <boost/hana/concept/ring.hpp>


/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\
|  Phycas: Python software for phylogenetic analysis                          |
|  Copyright (C) 2006 Mark T. Holder, Paul O. Lewis and David L. Swofford     |
|                                                                             |
|  This program is free software; you can redistribute it and/or modify       |
|  it under the terms of the GNU General Public License as published by       |
|  the Free Software Foundation; either version 2 of the License, or          |
|  (at your option) any later version.                                        |
|                                                                             |
|  This program is distributed in the hope that it will be useful,            |
|  but WITHOUT ANY WARRANTY; without even the implied warranty of             |
|  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the              |
|  GNU General Public License for more details.                               |
|                                                                             |
|  You should have received a copy of the GNU General Public License along    |
|  with this program; if not, write to the Free Software Foundation, Inc.,    |
|  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.                |
\~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

#include <boost/lambda/lambda.hpp>
#include <boost/lambda/bind.hpp>
#include <boost/format.hpp>
#include "ncl/nxsallocatematrix.h"
#include "q_matrix.hpp"
#include "xlikelihood.hpp"
using namespace phycas;

/*----------------------------------------------------------------------------------------------------------------------
|
*/
QMatrix::QMatrix()
  : dimension(0), flat_length(0)
  , qmat(0), qmat_begin(0), qmat_end(0)
  , w(0), w_begin(0), w_end(0)
  , z(0), z_begin(0), z_end(0)
  , fv(0)
  , q_dirty(true)
	{
	// Set up Q matrix representing the JC69 model by default
	rr.assign(6, 1.0);
	pi.assign(4, 0.25);
	sqrtPi.assign(4, 0.5);
	recalcQMatrix();
	}

/*----------------------------------------------------------------------------------------------------------------------
|
*/
QMatrix::~QMatrix()
	{
	clear();
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Returns object to its just-constructed state, with the exception that the vectors `rr', `pi' and `sqrtPi' are not
|	cleared.
*/
void QMatrix::clearAllExceptFreqsAndRates()
	{
	dimension	= 0;
	flat_length	= 0;
	qmat_begin	= 0;
	qmat_end	= 0;
	w_begin		= 0;
	w_end		= 0;
	z_begin		= 0;
	z_end		= 0;

	dim_vect.clear();

	if (qmat)
		{
		DeleteTwoDArray<double>(qmat);
		qmat = 0;
		}

	if (w)
		{
		delete [] w;
		w = 0;
		}

	if (fv)
		{
		delete [] fv;
		fv = 0;
		}

	if (z)
		{
		DeleteTwoDArray<double>(z);
		z = 0;
		}
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Returns object to its just-constructed state by calling clearAllExceptFreqsAndRates and then also clearing the
|	`rr', `pi' and `sqrtPi' vectors.
*/
void QMatrix::clear()
	{
	clearAllExceptFreqsAndRates();
	pi.clear();
	sqrtPi.clear();
	rr.clear();
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Accessor function that returns the current value of the data member `dimension', which is the number of rows (and
|	columns) in the Q matrix.
*/
unsigned QMatrix::getDimension()
	{
	return dimension;
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Copies elements from the supplied vector `rates' to the data member `rr' and sets `q_dirty' to true.
*/
void QMatrix::setRelativeRates(const std::vector<double> & rates)
	{
	rr.resize(rates.size());
	std::copy(rates.begin(), rates.end(), rr.begin());
	q_dirty = true;
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Copies elements from the supplied vector `rates' to the data member `rr' and sets `q_dirty' to true.
*/
void QMatrix::setStateFreqs(const std::vector<double> & freqs)
	{
	pi.resize(freqs.size());
	std::copy(freqs.begin(), freqs.end(), pi.begin());

	sqrtPi.resize(freqs.size());
	std::transform(pi.begin(), pi.end(), sqrtPi.begin(), boost::lambda::bind(static_cast<double(*)(double)>(&std::sqrt), boost::lambda::_1));
	q_dirty = true;
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Reallocates all arrays and vectors (except `rr', `pi' and `sqrtPi') according to the value of `new_dim' and sets
|	`dimension' equal to `new_dim' before returning. Assumes `new_dim' is greater than zero. Note that `qmat', `w', `z'
|	and `fv' are allocated but not filled with any values when this function returns.
*/
void QMatrix::redimension(unsigned new_dim)
	{
	PHYCAS_ASSERT(new_dim > 0);
	clearAllExceptFreqsAndRates();

	dimension = new_dim;
	flat_length = new_dim*new_dim;

	expwv.resize(new_dim);

	// Set the shape of a NumArray object that represents Q, P (transition probs), E (eigenvectors) or V (eigenvalues)
	dim_vect.push_back((int)dimension);
	dim_vect.push_back((int)dimension);

	// Create and fill qmat with default (Mk model) values
	qmat = NewTwoDArray<double>(dimension, dimension);
	PHYCAS_ASSERT(qmat);

	qmat_begin		= &qmat[0][0];
	qmat_end		= qmat_begin + flat_length;

	// Create w (array of eigenvalues)
	w = new double[dimension];
	w_begin = &w[0];
	w_end = w_begin + dimension;

	// Create z (two-dimensional array of eigenvectors)
	z = NewTwoDArray<double>(dimension, dimension);
	z_begin		= &z[0][0];
	z_end		= z_begin + flat_length;

	// Create fv (workspace used by EigenRealSymmetric)
	fv = new double[dimension];
	}

/*----------------------------------------------------------------------------------------------------------------------
|	If `q_dirty' is true, calls recalcQMatrixImpl to recompute the `qmat', `z' and `w' data members. If `q_dirty' is
|	false, however, this function returns immediately and thus (because it is declared inline) is computationally
|	inexpensive.
*/
void QMatrix::recalcQMatrix()
	{
	if (!q_dirty)
		return;
	recalcQMatrixImpl();
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Returns the eigenvalues in the data member `w' as a vector. Calls recalcQMatrix first to ensure that `w' is up to
|	date.
*/
std::vector<double> QMatrix::getEigenValues()
	{
	recalcQMatrix();
	std::vector<double> v(dimension, 0.0);
	std::copy(w_begin, w_end, v.begin());
	return v;
	}

/*----------------------------------------------------------------------------------------------------------------------
|
*/
std::string QMatrix::showQMatrix()
	{
	recalcQMatrix();
	return showMatrixImpl(qmat_begin);
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Stores a flattened version of the supplied 2-dimensional array `twoDarr', storing the result in the supplied std::vector<double>
|	reference variable `p'. The supplied `twoDarr' should be laid out so that rows occupy contiguous memory.
*/
void QMatrix::flattenTwoDMatrix(std::vector<double> & p, double * * twoDarr, unsigned dim) const
	{
	unsigned flat_length = dim*dim;
	p.resize(flat_length);
	double * twoD_begin = &twoDarr[0][0];
	double * twoD_end   = twoD_begin + flat_length;
	std::copy(twoD_begin, twoD_end, p.begin());
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Uses `pi' and `rr' vectors to create the two-dimensional array `qmat'. The data member `qmat' is reallocated if
|	changes in `pi' and `rr' imply a new dimension (or if `qmat' is NULL). Recomputes eigenvalues and eigenvectors
|	corresponding to the new Q matrix. Assumes `q_dirty' is true, and sets `q_dirty' to false when finished. Throws an
|	exception if lengths of the `pi' and `rr' vectors are incompatible.
*/
void QMatrix::recalcQMatrixImpl()
	{
	PHYCAS_ASSERT(q_dirty);

	// pi, sqrtPi and rr should all be non-empty
	PHYCAS_ASSERT(pi.size() > 0);
	PHYCAS_ASSERT(sqrtPi.size() == pi.size());
	PHYCAS_ASSERT(rr.size() > 0);

	// First check to make sure lengths of `rr' and `pi' are compatible with each other
	unsigned dim_pi = (unsigned)pi.size();
	unsigned dim_rr = (unsigned)((1.0 + std::sqrt(1.0 + 8.0*rr.size()))/2.0); // rr.size() = (n^2 - n)/2, so use quadratic formula to get n
	if (dim_pi != dim_rr)
		{
		throw XLikelihood(boost::str(boost::format("Number of relative rates (%d) and number of state frequencies (%d) specified are incompatible") % dim_rr % dim_pi));
		}

	// If qmat is NULL or dimension differs from dim_pi and dim_rr, reallocate qmat, z, w and dv
	if (!qmat || (dimension != dim_pi))
		redimension(dim_pi);

	// This vector will hold the row sums
	std::vector<double> row_sum(dimension, 0.0);

	unsigned i, j, k = 0;
	double sum_for_scaling = 0.0;
	for (i = 0; i < dimension; ++i)
		{
		double pi_i = pi[i];
		double sqrtPi_i = sqrtPi[i];
		for (j = i + 1; j < dimension; ++j, ++k)
			{
			double rr_k = rr[k];
			double pi_j = pi[j];
			double sqrtPi_j = sqrtPi[j];

			// set value in upper triangle
			qmat[i][j] = rr_k*sqrtPi_i*sqrtPi_j;

			// set value in lower triangle
			qmat[j][i] = qmat[i][j];

			// add to relevant row sums
			row_sum[i] += rr_k*pi_j;
			row_sum[j] += rr_k*pi_i;

			// add to total expected number of substitutions
			sum_for_scaling += 2.0*pi_i*rr_k*pi_j;
			}
		qmat[i][i] = -row_sum[i];
		}

	PHYCAS_ASSERT(sum_for_scaling > 0.0);
	edgelen_scaler = 1.0/sum_for_scaling;

	// Calculate eigenvalues (w) and eigenvectors (z)
	int err_code = EigenRealSymmetric(dimension, qmat, w, z, fv);

	if (err_code != 0)
		{
		clearAllExceptFreqsAndRates();
		throw XLikelihood("Error in the calculation of eigenvectors and eigenvalues of the Q matrix");
		}

	q_dirty = false;
	}

/*----------------------------------------------------------------------------------------------------------------------
|   Recomputes a transition probability matrix for an edge length `edgelen', storing it in `pmat'. If either state
|   frequencies or relative rates have changed, the Q matrix is reconstructed and eigenvalues and eigenvectors
|   recomputed before the transition matrix is recomputed.
*/
void QMatrix::recalcPMat(
  double * * pmat,		/**< is the transition matrix to recalculate */
  double edgelen) 		/**< is the edge length */
	{
	recalcQMatrix();
    double t = edgelen;

    // The next two lines fix the "Rota" bug; see BUGS file for details
    if (t < 1.e-8)
        t = 1.e-8; //TreeNode::edgeLenEpsilon;

	// Adjust the supplied edgelen to account for the fact that the expected number of substitutions
	// implied by the Q matrix is not unity
	double v = t*edgelen_scaler;

    // Precalculate exp to avoid doing the same calculation dimension*dimension times
	for (unsigned k = 0; k < dimension; ++k)
		{
		expwv[k] = std::exp(w[k]*v);
        }

	// Exponentiate eigenvalues and put everything back together again
	// Real symmetric matrices can be diagonalized using Z*exp(D)*Z^T, where Z is the
	// orthogonal matrix of eigenvectors and D is the diagonal matrix of eigenvalues,
	// each multiplied by time (scaled to equal expected number of substitutions)
	for (unsigned i = 0; i < dimension; ++i)
		{
		double sqrtPi_i = sqrtPi[i];
		for (unsigned j = 0; j < dimension; ++j)
			{
			double factor = sqrtPi[j]/sqrtPi_i;
			double Pij = 0.0;
			for (unsigned k = 0; k < dimension; ++k)
				{
				double tmp = z[i][k]*z[j][k]*expwv[k];
				Pij +=  tmp;
				}
			pmat[i][j] = Pij*factor;
			}
		}
	}

/*----------------------------------------------------------------------------------------------------------------------
|
*/
// void QMatrix::recalcPMatrix(
//   std::vector<double> & P,	/**< */
//   double edgelen)			/**< */
// 	{
// 	recalcQMatrix();
//
//     double t = edgelen;
//
//     // The next two lines fix the "Rota" bug; see BUGS file for details
//     if (t < 1.e-8)
//         t = 1.e-8; //TreeNode::edgeLenEpsilon;
//
//     // Adjust the supplied edgelen to account for the fact that the expected number of substitutions
// 	// implied by the Q matrix is not unity
// 	double v = t*edgelen_scaler;
//
// 	P.clear();
// 	P.reserve(flat_length);
//
// 	// Exponentiate eigenvalues and put everything back together again
// 	for (unsigned i = 0; i < dimension; ++i)
// 		{
// 		double sqrtPi_i = sqrtPi[i];
// 		for (unsigned j = 0; j < dimension; ++j)
// 			{
// 			double factor = sqrtPi[j]/sqrtPi_i;
// 			double Pij = 0.0;
// 			for (unsigned k = 0; k < dimension; ++k)
// 				{
// 				double tmp = z[i][k]*z[j][k]*std::exp(w[k]*v);
// 				Pij +=  tmp;
// 				}
// 			P.push_back(Pij*factor);
// 			}
// 		}
// 	}

/*----------------------------------------------------------------------------------------------------------------------
|
*/
std::string QMatrix::showMatrixImpl(const double * q) const
	{
	unsigned i, j;

	// Output one column for row labels and a label for every column of qmat
	std::string s = str(boost::format("%12s ") % " ");
	for (i = 0; i < dimension; ++i)
		{
		s += str(boost::format("%12d ") % i);
		}
	s += "\n";

	// Output rows of q
	double * pq = (double *)q;	//PELIGROSO
	for (i = 0; i < dimension; ++i)
		{
		s += str(boost::format("%12d ") % i);
		for (j = 0; j < dimension; ++j)
			{
			s += str(boost::format("%12.5f ") % *pq++);
			}
		s += "\n";
		}

	return s;
	}

#if defined(PYTHON_ONLY)
#if defined(USING_NUMARRAY)
/*----------------------------------------------------------------------------------------------------------------------
|	Returns the entries in the data member `qmat' as a NumArray. Calls recalcQMatrix first to ensure that `qmat' is up
|	to date.
*/
boost::python::numeric::array QMatrix::getQMatrix()
	{
	recalcQMatrix();
	return num_util::makeNum(qmat_begin, dim_vect);
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Returns the eigenvectors in the data member `z' as a NumArray. Calls recalcQMatrix first to ensure that `z' is up
|	to date.
*/
boost::python::numeric::array QMatrix::getEigenVectors()
	{
	recalcQMatrix();
	return num_util::makeNum(z_begin, dim_vect);
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Returns the transition probability matrix as a vector of double values (where the 2-dimensional matrix is converted
|   to a 1-dimensional vector by storing rows one after the other. Intended for debugging (not fast).
*/
boost::python::numeric::array QMatrix::getPMatrix(double edgelen)
	{
	double * * pMat = NewTwoDArray<double>(dimension, dimension);
	recalcPMat(pMat, edgelen);
	return num_util::makeNum(pMat, dim_vect);
	}
#else
/*----------------------------------------------------------------------------------------------------------------------
|	Returns the entries in the data member `qmat' as a NumArray. Calls recalcQMatrix first to ensure that `qmat' is up
|	to date.
*/
std::vector<double> QMatrix::getQMatrix()
	{
	recalcQMatrix();
	std::vector<double> p;
	flattenTwoDMatrix(p, qmat, dimension);
	return p;
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Returns the eigenvectors in the data member `z' as a NumArray. Calls recalcQMatrix first to ensure that `z' is up
|	to date.
*/
std::vector<double> QMatrix::getEigenVectors()
	{
	recalcQMatrix();
	std::vector<double> p;
	flattenTwoDMatrix(p, z, dimension);
	return p;
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Returns the transition probability matrix as a vector of double values (where the 2-dimensional matrix is converted
|   to a 1-dimensional vector by storing rows one after the other. Intended for debugging (not fast).
*/
std::vector<double> QMatrix::getPMatrix(double edgelen)
	{
	double * * pMat = NewTwoDArray<double>(dimension, dimension);
	recalcPMat(pMat, edgelen);
	std::vector<double> p;
	flattenTwoDMatrix(p, pMat, dimension);
	DeleteTwoDArray<double>(pMat);
	return p;
	}
#endif
#endif


#pragma once

#include <Eigen/Dense>

namespace cilantro {
    namespace internal {
        template <typename ScalarT, ptrdiff_t NRows, ptrdiff_t NCols>
        struct MatrixReductions {
#pragma omp declare reduction (+: Eigen::Matrix<ScalarT,NRows,NCols>: omp_out = omp_out + omp_in) initializer(omp_priv = Eigen::Matrix<ScalarT,NRows,NCols>::Zero(omp_orig.rows(), omp_orig.cols()))
        };
    }
}


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

/*!
 * \file NormalizeOpt.cc
 * \brief NormalizeOpt.cc
 */

#include "NormalizeOpt.h"

#include <alien/core/impl/MultiVectorImpl.h>
#include <alien/kernels/simple_csr/SimpleCSRMatrix.h>
#include <alien/kernels/simple_csr/SimpleCSRVector.h>

#ifdef ALIEN_USE_EIGEN2
#include <Eigen/Core>
#include <Eigen/LU>
#include <iostream>

#endif

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

namespace Alien
{

using namespace Arccore;

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

NormalizeOpt::NormalizeOpt()
: m_algo(StdLU)
, m_sum_first_eq(false)
{}

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

void NormalizeOpt::setAlgo(eAlgoType algo)
{
  m_algo = algo;
}

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

void NormalizeOpt::setOpt(eOptType opt, bool flag)
{
  switch (opt) {
  case SumFirstEq:
    m_sum_first_eq = flag;
    break;
  default:
    FatalErrorException(A_FUNCINFO, String::format("Unhandle option type: ", opt));
    break;
  }
}

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

NormalizeOpt::eErrorType
NormalizeOpt::normalize(IMatrix& m, IVector& x) const
{
  MatrixImpl& A = m.impl()->get<Alien::BackEnd::tag::simplecsr>(true);
  VectorImpl& b = x.impl()->get<Alien::BackEnd::tag::simplecsr>(true);
  return _normalize(A, b);
}

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

NormalizeOpt::eErrorType
NormalizeOpt::normalize(
CompositeMatrix& m, CompositeVector& x, ConstArrayView<Integer> eq_ids) const
{
  // need to update timestamp of all submatrices
  MatrixImpl& A00 = m(0, 0).impl()->get<Alien::BackEnd::tag::simplecsr>(true);
  MatrixImpl& A01 = m(0, 1).impl()->get<Alien::BackEnd::tag::simplecsr>(true);
  MatrixImpl& A10 = m(1, 0).impl()->get<Alien::BackEnd::tag::simplecsr>(true);
  MatrixImpl& A11 = m(1, 1).impl()->get<Alien::BackEnd::tag::simplecsr>(true);
  VectorImpl& b0 = x[0].impl()->get<Alien::BackEnd::tag::simplecsr>(true);
  VectorImpl& b1 = x[1].impl()->get<Alien::BackEnd::tag::simplecsr>(true);
  {
    eErrorType error =
    _normalize(A00, A01, m(0, 1).impl()->hasFeature("transposed"), eq_ids, b0);
    if (error != NoError)
      return error;
  }
  {
    eErrorType error =
    _normalize(A11, eq_ids, A10, m(1, 0).impl()->hasFeature("transposed"), b1);
    return error;
  }
}

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

template <>
void NormalizeOpt::Op::multInvDiag<0>()
{}

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

template <>
void NormalizeOpt::Op::multInvDiag<1>()
{
  for (Integer irow = 0; irow < m_local_size; ++irow) {
    Integer off = m_row_offset[irow];
    Real diag = m_matrix[off];
    for (Integer col = off; col < m_row_offset[irow + 1]; ++col) {
      m_matrix[col] /= diag;
    }
    m_rhs[irow] /= diag;

    if (!m_keep_diag)
      m_matrix[off] = 0.;
  }
}

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

template <Integer N>
void NormalizeOpt::Op::multInvDiag()
{
#ifdef ALIEN_USE_EIGEN2
  if (m_algo == EigenLU) {
    using namespace Eigen;
    using namespace std;
    typedef Eigen::Matrix<Real, N, N, RowMajor> MatrixType;
    typedef Eigen::Matrix<Real, N, 1> VectorType;
    const Integer NxN = N * N;
    // TOCHECK : to be removed ?
    // cout<<"MULT INV DIAG B"<<m_equations_num<<endl;
    UniqueArray<Real> block(NxN);
    UniqueArray<Real> vect(N);
    Map<MatrixType> m(block.begin(), N, N);
    Map<VectorType> v(vect.begin(), N);
    if (m_diag_first) {
      for (Integer irow = 0; irow < m_local_size; ++irow) {
        Integer off = m_row_offset[irow];

        Map<MatrixType> diag(&m_matrix[off * NxN], N, N);
        if (diag.determinant() == 0) {
          cout << "Non inversible diagonal : row=" << irow << endl;
          cout << " DIAG :" << endl;
          cout << diag << endl;
        }
        MatrixType inv_diag = diag.inverse();
        // TOCHECK : to be removed ?
        // PartialPivLU< MatrixType > lu(diag);
        // MatrixType inv_diag = lu.inverse() ;

        // OFF DIAGONAL treatment
        for (Integer col = off + 1; col < m_row_offset[irow + 1]; ++col) {
          block.copy(ArrayView<Real>(NxN, &m_matrix[col * NxN]));
          Map<MatrixType> matrix(&m_matrix[col * NxN], N, N);
          matrix = inv_diag * m;
          // TOCHECK : to be removed ?
          // matrix = lu.solve(m) ;
        }

        // RHS treatment
        vect.copy(ArrayView<Real>(N, &m_rhs[irow * N]));
        Map<VectorType> rhs(&m_rhs[irow * N], N);
        rhs = inv_diag * v;
        // TOCHECK : to be removed
        // rhs = lu.solve(v) ;

        if (m_keep_diag)
          diag.setIdentity();
        else
          diag.setZero();
      }
    }
    else {
      for (Integer irow = 0; irow < m_local_size; ++irow) {
        Integer off = m_row_offset[irow] + m_upper_diag_index[irow];

        Map<MatrixType> diag(&m_matrix[off * NxN], N, N);
        if (diag.determinant() == 0) {
          cout << "Non inversible diagonal : row=" << irow << endl;
          cout << " DIAG :" << endl;
          cout << diag << endl;
        }
        MatrixType inv_diag = diag.inverse();
        // TOCHECK : to be removed ?
        // PartialPivLU< MatrixType > lu(diag);
        // MatrixType inv_diag = lu.inverse() ;

        // OFF DIAGONAL treatment
        for (Integer col = m_row_offset[irow]; col < off; ++col) {
          block.copy(ArrayView<Real>(NxN, &m_matrix[col * NxN]));
          Map<MatrixType> matrix(&m_matrix[col * NxN], N, N);
          matrix = inv_diag * m;
          // TOCHECK : to be removed ?
          // matrix = lu.solve(m) ;
        }
        // skip diagonal block
        for (Integer col = off + 1; col < m_row_offset[irow + 1]; ++col) {
          block.copy(ArrayView<Real>(NxN, &m_matrix[col * NxN]));
          Map<MatrixType> matrix(&m_matrix[col * NxN], N, N);
          matrix = inv_diag * m;
          // TOCHECK : to be removed ?
          // matrix = lu.solve(m) ;
        }

        // RHS treatment
        vect.copy(ArrayView<Real>(N, &m_rhs[irow * N]));
        Map<VectorType> rhs(&m_rhs[irow * N], N);
        rhs = inv_diag * v;
        // TOCHECK : to be removed ?
        // rhs = lu.solve(v) ;

        if (m_keep_diag)
          diag.setIdentity();
        else
          diag.setZero();
      }
    }
  }
  else
#endif
  {
    if (m_diag_first) {
      for (Integer irow = 0; irow < m_local_size; ++irow) {
        Integer off = m_row_offset[irow];

        Real* diag = &m_matrix[off * N * N];
        LU<N> lu(diag);

        // OFF DIAGONAL treatment
        for (Integer col = off + 1; col < m_row_offset[irow + 1]; ++col) {
          lu.template solve<N, false>(&m_matrix[col * N * N]);
        }

        // RHS treatment
        lu.template solve<1, true>(&m_rhs[irow * N]);

        if (m_keep_diag)
          lu.setIdentity();
        else
          lu.setZero();
      }
    }
    else {
      for (Integer irow = 0; irow < m_local_size; ++irow) {
        Integer off = m_row_offset[irow] + m_upper_diag_index[irow];

        Real* diag = &m_matrix[off * N * N];
        LU<N> lu(diag);

        // OFF DIAGONAL treatment
        for (Integer col = m_row_offset[irow]; col < off; ++col) {
          lu.template solve<N, false>(&m_matrix[col * N * N]);
        }
        // skip diagonal block
        for (Integer col = off + 1; col < m_row_offset[irow + 1]; ++col) {
          lu.template solve<N, false>(&m_matrix[col * N * N]);
        }
        // RHS treatment
        lu.template solve<1, true>(&m_rhs[irow * N]);

        if (m_keep_diag)
          lu.setIdentity();
        else
          lu.setZero();
      }
    }
  }
}

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

NormalizeOpt::eErrorType
NormalizeOpt::_normalize(MatrixImpl& m, VectorImpl& x) const
{
  Op op(m, x, m_algo, m_sum_first_eq);
  if (m.block()) {
    switch (m.block()->size()) {
    case 1:
      op.multInvDiag<1>();
      break;
    case 2:
      op.multInvDiag<2>();
      break;
    case 3:
      op.multInvDiag<3>();
      break;
    case 4:
      op.multInvDiag<4>();
      break;
    default:
      op.multInvDiag<0>();
      break;
    }
  }
  return NoError;
}

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

template <>
void NormalizeOpt::Op2::multInvDiag<0>()
{}

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

template <>
void NormalizeOpt::Op2::multInvDiag<1>()
{
  if (m_submatrix2) {
    if (m_diag_first) {
      ////////////////////////////////////////////
      //
      // NORMALIZE SubMatrix00 and RHS
      for (Integer irow = 0; irow < m_local_size; ++irow) {
        Integer off = m_row_offset[irow];
        Real diag = m_matrix[off];
        for (Integer col = off + 1; col < m_row_offset[irow + 1]; ++col) {
          m_matrix[col] /= diag;
        }
        m_rhs[irow] /= diag;
      }

      ////////////////////////////////////////////
      //
      // NORMALIZE SubMatrix01
      if (m_trans)
        for (Integer i = 0; i < m_eq_ids.size(); ++i) {
          Integer ieq = m_eq_ids[i];
          for (Integer k = m_extra_eq_row_offset[ieq]; k < m_extra_eq_row_offset[ieq + 1];
               ++k) {
            Integer irow = m_extra_eq_cols[k];
            Integer off = m_row_offset[irow];
            Real diag = m_matrix[off];
            m_extra_eq_matrix[k] /= diag;
          }
        }
      else
        for (Integer i = 0; i < m_eq_ids.size(); ++i) {
          Integer irow = m_eq_ids[i];
          Integer off = m_row_offset[irow];
          Real diag = m_matrix[off];
          for (Integer k = m_extra_eq_row_offset[irow];
               k < m_extra_eq_row_offset[irow + 1]; ++k) {
            m_extra_eq_matrix[k] /= diag;
          }
        }
      ////////////////////////////////////////////
      //
      // SET DIAG TO Id
      for (Integer irow = 0; irow < m_local_size; ++irow) {
        Integer off_diag = m_row_offset[irow];
        m_matrix[off_diag] = m_keep_diag ? 1. : 0.;
      }
    }
    else {
      ////////////////////////////////////////////
      //
      // NORMALIZE SubMatrix00 and RHS
      for (Integer irow = 0; irow < m_local_size; ++irow) {
        Integer off_diag = m_row_offset[irow] + m_upper_diag_index[irow];
        Real diag = m_matrix[off_diag];
        for (Integer col = m_row_offset[irow]; col < off_diag; ++col) {
          m_matrix[col] /= diag;
        }
        for (Integer col = off_diag + 1; col < m_row_offset[irow + 1]; ++col) {
          m_matrix[col] /= diag;
        }
        m_rhs[irow] /= diag;
      }

      ////////////////////////////////////////////
      //
      // NORMALIZE SubMatrix01
      if (m_trans)
        for (Integer i = 0; i < m_eq_ids.size(); ++i) {
          Integer ieq = m_eq_ids[i];
          for (Integer k = m_extra_eq_row_offset[ieq]; k < m_extra_eq_row_offset[ieq + 1];
               ++k) {
            Integer irow = m_extra_eq_cols[k];
            Integer off = m_row_offset[irow] + m_upper_diag_index[irow];
            Real diag = m_matrix[off];
            m_extra_eq_matrix[k] /= diag;
          }
        }
      else
        for (Integer i = 0; i < m_eq_ids.size(); ++i) {
          Integer irow = m_eq_ids[i];
          Integer off = m_row_offset[irow] + m_upper_diag_index[irow];
          Real diag = m_matrix[off];
          for (Integer k = m_extra_eq_row_offset[irow];
               k < m_extra_eq_row_offset[irow + 1]; ++k) {
            m_extra_eq_matrix[k] /= diag;
          }
        }
      ////////////////////////////////////////////
      //
      // SET DIAG TO Id
      for (Integer irow = 0; irow < m_local_size; ++irow) {
        Integer off_diag = m_row_offset[irow] + m_upper_diag_index[irow];
        m_matrix[off_diag] = m_keep_diag ? 1. : 0.;
      }
    }
  }

  if (m_submatrix1) {
    ////////////////////////////////////////////
    //
    // NORMALIZE SubMatrix10 SubMatrix11 RHS
    for (Integer i = 0; i < m_eq_ids.size(); ++i) {
      Integer ieq = m_eq_ids[i];
      Real diag = m_matrix[ieq];
      for (Integer j = m_extra_eq_row_offset[ieq]; j < m_extra_eq_row_offset[ieq + 1];
           ++j) {
        for (Integer k = 0; k < m_nuk2; ++k)
          m_extra_eq_matrix[j * m_nuk2 + k] /= diag;
      }
      m_rhs[ieq] /= diag;
      m_matrix[ieq] = 1.;
    }
  }
}

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

template <Integer N>
void NormalizeOpt::Op2::multInvDiag()
{
  if (m_submatrix2) {
    if (m_diag_first) {
      ////////////////////////////////////////////
      //
      // NORMALIZE SubMatrix00
      for (Integer irow = 0; irow < m_local_size; ++irow) {
        Integer off = m_row_offset[irow];

        Real* diag = &m_matrix[off * N * N];
        LU<N> lu(diag);

        // OFF DIAGONAL treatment
        for (Integer col = off + 1; col < m_row_offset[irow + 1]; ++col) {
          lu.template solve<N, false>(&m_matrix[col * N * N]);
        }

        // RHS treatment
        lu.template solve<1, true>(&m_rhs[irow * N]);
      }

      ////////////////////////////////////////////
      //
      // NORMALIZE SubMatrix01
      if (m_trans)
        for (Integer i = 0; i < m_eq_ids.size(); ++i) {
          Integer ieq = m_eq_ids[i];
          for (Integer k = m_extra_eq_row_offset[ieq]; k < m_extra_eq_row_offset[ieq + 1];
               ++k) {
            Integer irow = m_extra_eq_cols[k];
            Integer off = m_row_offset[irow];
            Real* diag = &m_matrix[off * N * N];
            LU<N> lu(diag, false);

            lu.template solve<1, true>(&m_extra_eq_matrix[k * N]);
          }
        }
      else
        for (Integer i = 0; i < m_eq_ids.size(); ++i) {
          Integer irow = m_eq_ids[i];
          Integer off = m_row_offset[irow];
          Real* diag = &m_matrix[off * N * N];
          LU<N> lu(diag, false);
          for (Integer k = m_extra_eq_row_offset[irow];
               k < m_extra_eq_row_offset[irow + 1]; ++k) {
            lu.template solve<N, false>(&m_extra_eq_matrix[k * N]);
          }
        }

      ////////////////////////////////////////////
      //
      // SET DIAG TO Id
      if (m_keep_diag)
        for (Integer irow = 0; irow < m_local_size; ++irow) {
          Integer off = m_row_offset[irow];
          Real* diag = &m_matrix[off * N * N];
          LU<N> lu(diag, false);

          lu.setIdentity();
        }
      else
        for (Integer irow = 0; irow < m_local_size; ++irow) {
          Integer off = m_row_offset[irow];
          Real* diag = &m_matrix[off * N * N];
          LU<N> lu(diag, false);
          lu.setZero();
        }
    }
    else {
      ////////////////////////////////////////////
      //
      // NORMALIZE SubMatrix01
      for (Integer irow = 0; irow < m_local_size; ++irow) {
        Integer off = m_row_offset[irow] + m_upper_diag_index[irow];

        Real* diag = &m_matrix[off * N * N];
        LU<N> lu(diag);

        // OFF DIAGONAL treatment
        for (Integer col = m_row_offset[irow]; col < off; ++col) {
          lu.template solve<N, false>(&m_matrix[col * N * N]);
        }
        // skip diagonal block
        for (Integer col = off + 1; col < m_row_offset[irow + 1]; ++col) {
          lu.template solve<N, false>(&m_matrix[col * N * N]);
        }
        // RHS treatment
        lu.template solve<1, true>(&m_rhs[irow * N]);
      }
      ////////////////////////////////////////////
      //
      // NORMALIZE SubMatrix01
      if (m_trans)
        for (Integer i = 0; i < m_eq_ids.size(); ++i) {
          Integer ieq = m_eq_ids[i];
          for (Integer k = m_extra_eq_row_offset[ieq]; k < m_extra_eq_row_offset[ieq + 1];
               ++k) {
            Integer irow = m_extra_eq_cols[k] - m_local_offset;

            Integer off = m_row_offset[irow] + m_upper_diag_index[irow];
            Real* diag = &m_matrix[off * N * N];
            LU<N> lu(diag, false);

            lu.template solve<1, true>(&m_extra_eq_matrix[k * N]);
          }
        }
      else {
        for (Integer i = 0; i < m_eq_ids.size(); ++i) {
          Integer irow = m_eq_ids[i];
          Integer off = m_row_offset[irow] + m_upper_diag_index[irow];
          Real* diag = &m_matrix[off * N * N];
          LU<N> lu(diag, false);
          for (Integer k = m_extra_eq_row_offset[irow];
               k < m_extra_eq_row_offset[irow + 1]; ++k) {
            lu.template solve<N, false>(&m_extra_eq_matrix[k * N]);
          }
        }
      }

      ////////////////////////////////////////////
      //
      // SET DIAG TO Id
      if (m_keep_diag)
        for (Integer irow = 0; irow < m_local_size; ++irow) {
          Integer off = m_row_offset[irow] + m_upper_diag_index[irow];
          Real* diag = &m_matrix[off * N * N];
          LU<N> lu(diag, false);

          lu.setIdentity();
        }
      else
        for (Integer irow = 0; irow < m_local_size; ++irow) {
          Integer off = m_row_offset[irow] + m_upper_diag_index[irow];
          Real* diag = &m_matrix[off * N * N];
          LU<N> lu(diag, false);
          lu.setZero();
        }
    }
  }

  if (m_submatrix1) {
    ////////////////////////////////////////////
    //
    // NORMALIZE SubMatrix10 SubMatrix11 RHS
    for (Integer i = 0; i < m_eq_ids.size(); ++i) {
      Integer ieq = m_eq_ids[i];
      Real diag = m_matrix[ieq];
      for (Integer j = m_extra_eq_row_offset[ieq]; j < m_extra_eq_row_offset[ieq + 1];
           ++j) {
        for (Integer k = 0; k < N; ++k)
          m_extra_eq_matrix[j * N + k] /= diag;
      }
      m_rhs[ieq] /= diag;
      m_matrix[ieq] = 1.;
    }
  }
}

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

NormalizeOpt::eErrorType
NormalizeOpt::_normalize(MatrixImpl& m, MatrixImpl& m2, bool trans,
                         ConstArrayView<Integer> eq_ids, VectorImpl& x) const
{
  Op2 op(m, m2, trans, eq_ids, x, m_algo, m_sum_first_eq);

  switch (x.block()->size()) {
  case 1:
    op.multInvDiag<1>();
    break;
  case 2:
    op.multInvDiag<2>();
    break;
  case 3:
    op.multInvDiag<3>();
    break;
  case 4:
    op.multInvDiag<4>();
    break;
  default:
    op.multInvDiag<0>();
    break;
  }
  return NoError;
}

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

NormalizeOpt::eErrorType
NormalizeOpt::_normalize(MatrixImpl& m, ConstArrayView<Integer> eq_ids, MatrixImpl& m2,
                         bool trans, VectorImpl& x) const
{
  Op2 op(m, eq_ids, m2, trans, x, m_algo, m_sum_first_eq);

  switch (x.block()->size()) {
  case 1:
    op.multInvDiag<1>();
    break;
  case 2:
    op.multInvDiag<2>();
    break;
  case 3:
    op.multInvDiag<3>();
    break;
  case 4:
    op.multInvDiag<4>();
    break;
  default:
    op.multInvDiag<0>();
    break;
  }
  return NoError;
}

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

} // namespace Alien

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