/******************************************************************************
 * Copyright (C) 2014 by Jerome Maye                                          *
 * jerome.maye@gmail.com                                                      *
 ******************************************************************************/

#include "aslam/calibration/camera/CameraCalibrator.h"

#include <cmath>

#include <iostream>
#include <iterator>
#include <algorithm>
#include <sstream>

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

#include <sm/PropertyTree.hpp>

#include <sm/kinematics/homogeneous_coordinates.hpp>
#include <sm/kinematics/Transformation.hpp>

#include <sm/boost/null_deleter.hpp>

#include <aslam/Time.hpp>

#include <aslam/cameras.hpp>

#include <aslam/cameras/GridCalibrationTargetCheckerboard.hpp>
#include <aslam/cameras/GridDetector.hpp>
#include <aslam/cameras/GridCalibrationTargetObservation.hpp>

#include <aslam/backend/HomogeneousPoint.hpp>
#include <aslam/backend/CameraDesignVariable.hpp>
#include <aslam/backend/RotationQuaternion.hpp>
#include <aslam/backend/EuclideanPoint.hpp>
#include <aslam/backend/TransformationExpression.hpp>
#include <aslam/backend/RotationExpression.hpp>
#include <aslam/backend/EuclideanExpression.hpp>
#include <aslam/backend/DesignVariable.hpp>
#include <aslam/backend/ErrorTerm.hpp>
#include <aslam/backend/MEstimatorPolicies.hpp>

#include <aslam/ReprojectionError.hpp>
#include <aslam/CameraGeometryDesignVariableContainer.hpp>

#include <aslam/calibration/core/IncrementalEstimator.h>
#include <aslam/calibration/core/IncrementalOptimizationProblem.h>
#include <aslam/calibration/core/OptimizationProblem.h>
#include <aslam/calibration/exceptions/BadArgumentException.h>
#include <aslam/calibration/exceptions/InvalidOperationException.h>
#include <aslam/calibration/exceptions/OutOfBoundException.h>
#include <aslam/calibration/base/Timestamp.h>
#include <aslam/calibration/statistics/EstimatorML.h>
#include <aslam/calibration/statistics/NormalDistribution.h>

namespace aslam {
  namespace calibration {

/******************************************************************************/
/* Constructors and Destructor                                                */
/******************************************************************************/

    CameraCalibrator::CameraCalibrator(const IncrementalEstimatorPtr& estimator,
        const Options& options) :
        _options(options),
        _estimator(estimator),
        _geometryInitialized(false),
        _batchNumImages(0),
        _q(0.0) {
      initVisionFramework();
    }

    CameraCalibrator::CameraCalibrator(const sm::PropertyTree& config) :
        _geometryInitialized(false),
        _batchNumImages(0),
        _q(0.0) {
      // read the options from the property tree
      _options.rows = config.getInt("rows", _options.rows);
      _options.cols = config.getInt("cols", _options.cols);
      _options.rowSpacingMeters = config.getDouble("rowSpacingMeters",
        _options.rowSpacingMeters);
      _options.colSpacingMeters = config.getDouble("colSpacingMeters",
        _options.colSpacingMeters);
      _options.useAdaptiveThreshold = config.getBool("useAdaptiveThreshold",
        _options.useAdaptiveThreshold);
      _options.normalizeImage = config.getBool("normalizeImage",
        _options.normalizeImage);
      _options.filterQuads = config.getBool("filterQuads",
        _options.filterQuads);
      _options.doSubpixelRefinement = config.getBool("doSubpixelRefinement",
        _options.doSubpixelRefinement);
      _options.showExtractionVideo = config.getBool("showExtractionVideo",
        _options.showExtractionVideo);
      _options.plotCornerReprojection =
        config.getBool("plotCornerReprojection");
      _options.imageStepping = config.getBool("imageStepping");
      _options.filterCornerOutliers = config.getBool("filterCornerOutliers");
      _options.filterCornerSigmaThreshold =
        config.getDouble("filterCornerSigmaThreshold");
      _options.filterCornerMinReprojError =
        config.getDouble("filterCornerMinReprojError");
      _options.cameraProjectionType = config.getString("cameraProjectionType",
        _options.cameraProjectionType);
      _options.estimateLandmarks = config.getBool("estimateLandmarks",
        _options.estimateLandmarks);
      _options.landmarksGroupId = config.getInt("landmarksGroupId",
        _options.landmarksGroupId);
      _options.calibrationGroupId = config.getInt("calibrationGroupId",
        _options.calibrationGroupId);
      _options.transformationsGroupId = config.getInt("transformationsGroupId",
        _options.transformationsGroupId);
      _options.batchNumImages = config.getInt("batchNumImages",
        _options.batchNumImages);
      _options.useMEstimator = config.getBool("useMEstimator",
        _options.useMEstimator);
      _options.sigma2 = config.getDouble("sigma2", _options.sigma2);
      _options.verbose = config.getBool("verbose", _options.verbose);

      // init vision framework
      initVisionFramework();

      // create the underlying estimator
      _estimator = boost::make_shared<IncrementalEstimator>(
        sm::PropertyTree(config, "estimator"));
    }

    CameraCalibrator::~CameraCalibrator() {
    }

/******************************************************************************/
/* Accessors                                                                  */
/******************************************************************************/

    const CameraCalibrator::Options& CameraCalibrator::getOptions() const {
      return _options;
    }

    CameraCalibrator::Options& CameraCalibrator::getOptions() {
      return _options;
    }

    const CameraCalibrator::IncrementalEstimatorPtr
        CameraCalibrator::getEstimator() const {
      return _estimator;
    }

    CameraCalibrator::IncrementalEstimatorPtr CameraCalibrator::getEstimator() {
      return _estimator;
    }

    size_t CameraCalibrator::getBatchNumImages() const {
      return _batchNumImages;
    }

    Eigen::VectorXd CameraCalibrator::getProjection() const {
      Eigen::MatrixXd params;
      _geometry->getParameters(params, true, false, false);
      return params;
    }

    Eigen::VectorXd CameraCalibrator::getProjectionVariance() const {
      if (_estimator->getNumBatches())
        return _estimator->getSigma2Theta().diagonal().head(
          _geometry->minimalDimensionsProjection());
      else
        return Eigen::VectorXd::Zero(0);
    }

    Eigen::VectorXd CameraCalibrator::getProjectionStandardDeviation() const {
      if (_estimator->getNumBatches())
        return getProjectionVariance().array().sqrt();
      else
        return Eigen::VectorXd::Zero(0);
    }

    Eigen::VectorXd CameraCalibrator::getDistortion() const {
      Eigen::MatrixXd params;
      _geometry->getParameters(params, false, true, false);
      return params;
    }

    Eigen::VectorXd CameraCalibrator::getDistortionVariance() const {
      if (_estimator->getNumBatches())
        return _estimator->getSigma2Theta().diagonal().tail(
          _geometry->minimalDimensionsDistortion());
      else
        return Eigen::VectorXd::Zero(0);
    }

    Eigen::VectorXd CameraCalibrator::getDistortionStandardDeviation() const {
      if (_estimator->getNumBatches())
        return getDistortionVariance().array().sqrt();
      else
        return Eigen::VectorXd::Zero(0);
    }

    const std::vector<CameraCalibrator::ObservationPtr>&
        CameraCalibrator::getBatchObservations() const {
      return _batchObservations;
    }

    const std::vector<CameraCalibrator::ObservationPtr>&
        CameraCalibrator::getEstimatorObservations() const {
      return _estimatorObservations;
    }

    double CameraCalibrator::getInitialCost() const {
      return _estimator->getInitialCost();
    }

    double CameraCalibrator::getFinalCost() const {
      return _estimator->getFinalCost();
    }

    Eigen::Matrix4d CameraCalibrator::getTransformation(size_t idx) const {
      if (idx >= _estimatorObservations.size())
        throw OutOfBoundException<size_t>(idx, _estimatorObservations.size(),
          "idx must be stricly smaller than the number of observations",
          __FILE__, __LINE__, __PRETTY_FUNCTION__);
      auto dvs = _estimator->getProblem()->getDesignVariablesGroup(
        _options.transformationsGroupId);
      auto q_dv = const_cast<aslam::backend::RotationQuaternion*>(
        dynamic_cast<const aslam::backend::RotationQuaternion*>(
        dvs.at(idx * 2)));
      auto t_dv = dynamic_cast<const aslam::backend::EuclideanPoint*>(
        dvs.at(idx * 2 + 1));
      sm::kinematics::Transformation T(q_dv->getQuaternion(),
        t_dv->toEuclidean());
      return T.T();
    }

    Eigen::MatrixXd CameraCalibrator::getNobsBasis(bool scaled) const {
      return _estimator->getNobsBasis(scaled);
    }

    void CameraCalibrator::getStatistics(Eigen::VectorXd&
        mean, Eigen::VectorXd& variance, Eigen::VectorXd& standardDeviation,
        double& maxXError, double& maxYError, size_t& numOutliers) {
      std::vector<Eigen::Vector2d> errors;
      std::vector<double> errorsMd2;
      getErrors(errors, errorsMd2);
      EstimatorML<NormalDistribution<2> > statistics;
      statistics.addPoints(errors);
      if (statistics.getValid()) {
        mean = statistics.getDistribution().getMean();
        variance = statistics.getDistribution().getCovariance().diagonal();
        standardDeviation = variance.array().sqrt();
        maxXError = 0.0;
        maxYError = 0.0;
        for (auto it = errors.cbegin(); it != errors.cend(); ++it) {
          if (std::fabs((*it)(0)) > maxXError)
            maxXError = std::fabs((*it)(0));
          if (std::fabs((*it)(1)) > maxYError)
            maxYError = std::fabs((*it)(1));
        }
        if (_q == 0.0)
          _q = boost::math::quantile(boost::math::chi_squared_distribution<>(2),
            0.975);
        numOutliers = std::count_if(errorsMd2.cbegin(), errorsMd2.cend(),
          [&](decltype(*errorsMd2.cbegin()) x){return x > _q;});
      }
      else {
        mean.resize(0);
        variance.resize(0);
        standardDeviation.resize(0);
        maxXError = 0.0;
        maxYError = 0.0;
        numOutliers = 0;
      }
    }

    void CameraCalibrator::getErrors(std::vector<Eigen::Vector2d>& errors,
        std::vector<double>& errorsMd2) {
      errors.clear();
      errorsMd2.clear();
      auto problem = const_cast<IncrementalOptimizationProblem*>(
        _estimator->getProblem());
      errors.reserve(problem->numErrorTerms());
      errorsMd2.reserve(problem->numErrorTerms());
      for (size_t i = 0; i != problem->numErrorTerms(); ++i) {
        auto e_re = problem->errorTerm(i);
        errorsMd2.push_back(e_re->evaluateError());
        errors.push_back(
          dynamic_cast<aslam::ReprojectionError*>(e_re)->error());
      }
    }

    void CameraCalibrator::getLastCheckerboardImage(cv::Mat& image) const {
      if (_lastObservation)
        image = _lastObservation->image();
    }

/******************************************************************************/
/* Methods                                                                    */
/******************************************************************************/

    void CameraCalibrator::initVisionFramework() {
      // create calibration target
      CalibrationTarget::CheckerboardOptions targetOptions;
      targetOptions.useAdaptiveThreshold = _options.useAdaptiveThreshold;
      targetOptions.normalizeImage = _options.normalizeImage;
      targetOptions.filterQuads = _options.filterQuads;
      targetOptions.doSubpixelRefinement = _options.doSubpixelRefinement;
      targetOptions.showExtractionVideo = _options.showExtractionVideo;
      _calibrationTarget = boost::make_shared<CalibrationTarget>(_options.rows,
        _options.cols, _options.rowSpacingMeters, _options.colSpacingMeters,
        targetOptions);

      // create camera geometry
      if (_options.cameraProjectionType == "omni")
        _geometry =
          boost::make_shared<aslam::cameras::DistortedOmniCameraGeometry>();
      else if (_options.cameraProjectionType == "pinhole")
        _geometry =
          boost::make_shared<aslam::cameras::DistortedPinholeCameraGeometry>();
      else
        throw BadArgumentException<std::string>(_options.cameraProjectionType,
          "unkown camera projection type", __FILE__, __LINE__,
          __PRETTY_FUNCTION__);

      // create detector
      Detector::GridDetectorOptions detectorOptions;
      detectorOptions.plotCornerReprojection = _options.plotCornerReprojection;
      detectorOptions.imageStepping = _options.imageStepping;
      detectorOptions.filterCornerOutliers = _options.filterCornerOutliers;
      detectorOptions.filterCornerSigmaThreshold =
        _options.filterCornerSigmaThreshold;
      detectorOptions.filterCornerMinReprojError =
        _options.filterCornerMinReprojError;
      _detector = boost::make_shared<Detector>(_geometry, _calibrationTarget,
        detectorOptions);

      // create design variables for landmarks
      _landmarkDesignVariables.reserve(_calibrationTarget->size());
      for (size_t i = 0; i < _calibrationTarget->size(); ++i) {
        auto dv =  boost::make_shared<LandmarkDesignVariable>(
          sm::kinematics::toHomogeneous(_calibrationTarget->point(i)));
        dv->setActive(_options.estimateLandmarks);
        _landmarkDesignVariables.push_back(dv);
      }

      // create camera intrinsics design variable
      _cameraDesignVariableContainer = boost::make_shared<
        CameraDesignVariableContainer>(_geometry, true, true, false);
    }

    bool CameraCalibrator::initGeometry(const cv::Mat& image) {
      if (_geometryInitialized)
        return true;
      if (_detector->initCameraGeometryFromObservation(image)) {
        _geometryInitialized = true;
        return true;
      }
      else
        return false;
    }

    void CameraCalibrator::initBatch() {
      // create batch / overwrite older if already existing
      _batch = boost::make_shared<OptimizationProblem>();

      // add the landmark design variables
      for (auto it = _landmarkDesignVariables.cbegin();
          it != _landmarkDesignVariables.cend(); ++it)
        _batch->addDesignVariable(*it, _options.landmarksGroupId);

      // add the calibration design variables
      aslam::backend::DesignVariable::set_t cameraDesignVariables;
      _cameraDesignVariableContainer->getDesignVariables(cameraDesignVariables);
      for (auto it = cameraDesignVariables.begin();
          it != cameraDesignVariables.end(); ++it)
        _batch->addDesignVariable(
          boost::shared_ptr<aslam::backend::DesignVariable>(
          *it, sm::null_deleter()), _options.calibrationGroupId);

      // clear the currently stored observations
      _batchObservations.clear();
    }

    void CameraCalibrator::addObservation(const Observation& observation) {
      // if the batch does not exist, create it
      if (!_batch)
        initBatch();

      // get the transformation that takes points from camera coordinates to
      // target coordinates
      auto T_t_c = const_cast<Observation&>(observation).T_t_c();

      // create a rotation quaternion design variable and add it
      auto q_dv = boost::make_shared<aslam::backend::RotationQuaternion>(
        T_t_c.q());
      q_dv->setActive(true);
      _batch->addDesignVariable(q_dv, _options.transformationsGroupId);

      // create an Euclidean point design variable and add it
      auto t_dv = boost::make_shared<aslam::backend::EuclideanPoint>(T_t_c.t());
      t_dv->setActive(true);
      _batch->addDesignVariable(t_dv, _options.transformationsGroupId);

      // expression for the transformation
      aslam::backend::RotationExpression q_dv_e(q_dv);
      aslam::backend::EuclideanExpression t_dv_e(t_dv);
      aslam::backend::TransformationExpression T_t_c_e(q_dv_e, t_dv_e);

      // inverse transformation
      auto T_c_t_e = T_t_c_e.inverse();

      // add the reprojection error terms
      for (auto it = _landmarkDesignVariables.cbegin();
          it != _landmarkDesignVariables.cend(); ++it) {
        auto targetPoint = (*it)->toExpression();
        Eigen::Vector2d obsPoint;
        const bool success = observation.imagePoint(std::distance(
          _landmarkDesignVariables.cbegin(), it), obsPoint);
        if (success) {
          auto e_re = boost::make_shared<aslam::ReprojectionError>(obsPoint,
            _options.sigma2 * Eigen::Matrix2d::Identity(),
            T_c_t_e * targetPoint, _cameraDesignVariableContainer.get());
          if (_options.useMEstimator)
            e_re->setMEstimatorPolicy(
              boost::make_shared<aslam::backend::BlakeZissermanMEstimator>(
              e_re->dimension()));
          _batch->addErrorTerm(e_re);
        }
      }

      _batchNumImages++;
    }

    bool CameraCalibrator::addImage(const cv::Mat& image, sm::timing::NsecTime
        timestamp) {
      if (!_geometryInitialized)
        throw InvalidOperationException("geometry not initialized", __FILE__,
          __LINE__, __PRETTY_FUNCTION__);

      // find the target in the input image
      auto observation = boost::make_shared<Observation>();
      const bool status = _detector->findTarget(image, aslam::Time(
        sm::timing::nsecToSec(timestamp)), *observation);
      if (!status) {
        if (_options.verbose)
          std::cerr << __PRETTY_FUNCTION__ << ": target not found at time "
            << sm::timing::nsecToSec(timestamp) << std::endl;
        return status;
      }
      else {
        if (_options.verbose)
          std::cout << __PRETTY_FUNCTION__ << ": target found at time "
            << sm::timing::nsecToSec(timestamp) << std::endl;
      }

      // add observation to the batch
      addObservation(*observation);
      _batchObservations.push_back(observation);
      _lastObservation = observation;

      // add batch if needed
      if (_batchNumImages == _options.batchNumImages)
        processBatch();

      return true;
    }

    void CameraCalibrator::processBatch() {
      if (!_batch)
        return;
      auto ret = _estimator->addBatch(_batch);
      if (ret.batchAccepted) {
        _estimatorObservations.insert(_estimatorObservations.begin(),
          _batchObservations.begin(), _batchObservations.end());
      }
      if (_options.verbose) {
        std::cout << std::endl;
        ret.batchAccepted ? std::cout << "ACCEPTED" : std::cout << "REJECTED";
        std::cout << std::endl;
        std::cout << "information gain: " << ret.informationGain << std::endl;
        std::cout << "unobservable basis: " << std::endl << ret.nobsBasis
          << std::endl;
        std::cout << "singular values: "
          << ret.singularValues.transpose() << std::endl;
        std::cout << "unobservable basis (scaled): " << std::endl
          << ret.nobsBasisScaled << std::endl;
        std::cout << "singular values (scaled): "
          << ret.singularValuesScaled.transpose() << std::endl;
        std::cout << "projection: " << getProjection().transpose() << std::endl;
        std::cout << "projection standard deviation: "
          << getProjectionStandardDeviation().transpose() << std::endl;
        std::cout << "distortion: " << getDistortion().transpose() << std::endl;
        std::cout << "distortion standard deviation: "
          << getDistortionStandardDeviation().transpose() << std::endl;
        Eigen::VectorXd mean, variance, standardDeviation;
        double maxXError, maxYError;
        size_t numOutliers;
        getStatistics(mean, variance, standardDeviation, maxXError, maxYError,
          numOutliers);
        std::cout << "reprojection error mean: " << mean.transpose()
          << std::endl;
        std::cout << "reprojection error standard deviation: " <<
          standardDeviation.transpose() << std::endl;
        std::cout << "max x reprojection error: " << maxXError << std::endl;
        std::cout << "max y reprojection error: " << maxYError << std::endl;
        std::cout << "number of outliers: " << numOutliers << std::endl;
        std::cout << "number of images used: " << _estimatorObservations.size()
          << std::endl;
         std::cout << std::endl;
      }
      initBatch();
      _batchNumImages = 0;
    }

    void CameraCalibrator::write(sm::PropertyTree& config) const {
      auto projection = getProjection();
      Eigen::VectorXd projectionStd = getProjectionStandardDeviation();
      auto distortion = getDistortion();
      Eigen::VectorXd distortionStd = getDistortionStandardDeviation();
      if (_options.cameraProjectionType == "omni") {
        config.setDouble("projection/xi", projection(0));
        config.setDouble("projection/sigma_xi", projectionStd(0));
        config.setDouble("projection/fu", projection(1));
        config.setDouble("projection/sigma_fu", projectionStd(1));
        config.setDouble("projection/fv", projection(2));
        config.setDouble("projection/sigma_fv", projectionStd(2));
        config.setDouble("projection/cu", projection(3));
        config.setDouble("projection/sigma_cu", projectionStd(3));
        config.setDouble("projection/cv", projection(4));
        config.setDouble("projection/sigma_cv", projectionStd(4));
        config.setInt("projection/ru",
          dynamic_cast<aslam::cameras::DistortedOmniCameraGeometry*>(
          _geometry.get())->projection().ru());
        config.setInt("projection/rv",
          dynamic_cast<aslam::cameras::DistortedOmniCameraGeometry*>(
          _geometry.get())->projection().rv());
      }
      else {
        config.setDouble("projection/fu", projection(0));
        config.setDouble("projection/sigma_fu", projectionStd(0));
        config.setDouble("projection/fv", projection(1));
        config.setDouble("projection/sigma_fv", projectionStd(1));
        config.setDouble("projection/cu", projection(2));
        config.setDouble("projection/sigma_cu", projectionStd(2));
        config.setDouble("projection/cv", projection(3));
        config.setDouble("projection/sigma_cv", projectionStd(3));
        config.setInt("projection/ru",
          dynamic_cast<aslam::cameras::DistortedPinholeCameraGeometry*>
          (_geometry.get())->projection().ru());
        config.setInt("projection/rv",
          dynamic_cast<aslam::cameras::DistortedPinholeCameraGeometry*>(
          _geometry.get())->projection().rv());
      }
      config.setString("projection/type", _options.cameraProjectionType);
      config.setDouble("projection/distortion/k1", distortion(0));
      config.setDouble("projection/distortion/sigma_k1", distortionStd(0));
      config.setDouble("projection/distortion/k2", distortion(1));
      config.setDouble("projection/distortion/sigma_k2", distortionStd(1));
      config.setDouble("projection/distortion/p1", distortion(2));
      config.setDouble("projection/distortion/sigma_p1", distortionStd(2));
      config.setDouble("projection/distortion/p2", distortion(3));
      config.setDouble("projection/distortion/sigma_p2", distortionStd(3));
      config.setDouble("shutter/line-delay", 0.0);
      config.setString("mask/mask-file", "");
      Eigen::MatrixXd nobsBasis = getNobsBasis();
      for (std::ptrdiff_t c = 0; c < nobsBasis.cols(); ++c) {
        for (std::ptrdiff_t r = 0; r < nobsBasis.rows(); ++r) {
          std::stringstream stream;
          stream << "nobsBasis/c" << c << "/r" << r;
          config.setDouble(stream.str(), nobsBasis(r, c));
        }
      }
      nobsBasis = getNobsBasis(true);
      for (std::ptrdiff_t c = 0; c < nobsBasis.cols(); ++c) {
        for (std::ptrdiff_t r = 0; r < nobsBasis.rows(); ++r) {
          std::stringstream stream;
          stream << "nobsBasisScaled/c" << c << "/r" << r;
          config.setDouble(stream.str(), nobsBasis(r, c));
        }
      }
      Eigen::VectorXd singularValues = _estimator->getSingularValues();
      for (std::ptrdiff_t i = 0; i < singularValues.size(); ++i) {
        std::stringstream stream;
        stream << "singularValues/x" << i;
        config.setDouble(stream.str(), singularValues(i));
      }
      singularValues = _estimator->getSingularValues(true);
      for (std::ptrdiff_t i = 0; i < singularValues.size(); ++i) {
        std::stringstream stream;
        stream << "scaledSingularValues/x" << i;
        config.setDouble(stream.str(), singularValues(i));
      }
      config.setDouble("epsSVD", _estimator->getLinearSolverOptions().epsSVD);
      config.setDouble("infoGainDelta", _estimator->getOptions().infoGainDelta);
    }

  }
}


/**
 * Parrot Drones Awesome Video Viewer
 * Desktop application
 *
 * Copyright (c) 2018 Parrot Drones SAS
 * Copyright (c) 2016 Aurelien Barre
 *
 * 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 holders 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 HOLDERS 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.
 */

#include <Eigen/Eigen>

#include "pdraw_desktop.h"

void pdraw_desktop_view_create_matrices(struct pdraw_desktop *self,
					unsigned int width,
					unsigned int height,
					float *view_mat,
					float *proj_mat,
					float near,
					float far)
{
	float w = 1.f;
	float h = (float)width / (float)height;
	float a = (far + near) / (far - near);
	float b = -((2 * far * near) / (far - near));

	Eigen::Matrix4f p_mat;
	p_mat << w, 0, 0, 0, 0, h, 0, 0, 0, 0, a, b, 0, 0, 1, 0;

	Eigen::Matrix4f v_mat = Eigen::Matrix4f::Identity();

	unsigned int i, j;
	for (i = 0; i < 4; i++) {
		for (j = 0; j < 4; j++)
			view_mat[j * 4 + i] = v_mat(i, j);
	}
	for (i = 0; i < 4; i++) {
		for (j = 0; j < 4; j++)
			proj_mat[j * 4 + i] = p_mat(i, j);
	}
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2019 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_OVERLAY_GET_DISTANCE_MEASURE_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_OVERLAY_GET_DISTANCE_MEASURE_HPP

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/coordinate_system.hpp>
#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/arithmetic/infinite_line_functions.hpp>
#include <boost/geometry/algorithms/detail/make/make.hpp>
#include <boost/geometry/algorithms/not_implemented.hpp>
#include <boost/geometry/util/select_coordinate_type.hpp>

#include <cmath>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

template <typename T>
struct distance_measure
{
    T measure;

    distance_measure()
        : measure(T())
    {}

    bool is_small() const { return false; }
    bool is_zero() const { return false; }
    bool is_positive() const { return false; }
    bool is_negative() const { return false; }
};

template <typename T>
struct distance_measure_floating
{
    T measure;

    distance_measure_floating()
        : measure(T())
    {}

    // Returns true if the distance measure is small.
    // This is an arbitrary boundary, to enable some behaviour
    // (for example include or exclude turns), which are checked later
    // with other conditions.
    bool is_small() const { return std::abs(measure) < 1.0e-3; }

    // Returns true if the distance measure is absolutely zero
    bool is_zero() const { return measure == 0.0; }

    // Returns true if the distance measure is positive. Distance measure
    // algorithm returns positive value if it is located on the left side.
    bool is_positive() const { return measure > 0.0; }

    // Returns true if the distance measure is negative. Distance measure
    // algorithm returns negative value if it is located on the right side.
    bool is_negative() const { return measure < 0.0; }
};

template <>
struct distance_measure<long double>
    : public distance_measure_floating<long double> {};

template <>
struct distance_measure<double>
    : public distance_measure_floating<double> {};

template <>
struct distance_measure<float>
    : public distance_measure_floating<float> {};

} // detail

namespace detail_dispatch
{

// TODO: this is effectively a strategy, but for internal usage.
// It might be moved to the strategies folder.

template <typename CalculationType, typename CsTag>
struct get_distance_measure
        : not_implemented<CsTag>
{};

template <typename CalculationType>
struct get_distance_measure<CalculationType, cartesian_tag>
{
    typedef detail::distance_measure<CalculationType> result_type;

    template <typename SegmentPoint, typename Point>
    static result_type apply(SegmentPoint const& p1, SegmentPoint const& p2,
                             Point const& p)
    {
        // Get the distance measure / side value
        // It is not a real distance and purpose is
        // to detect small differences in collinearity

        typedef model::infinite_line<CalculationType> line_type;
        line_type const line = detail::make::make_infinite_line<CalculationType>(p1, p2);
        result_type result;
        result.measure = arithmetic::side_value(line, p);
        return result;
    }
};

template <typename CalculationType>
struct get_distance_measure<CalculationType, spherical_tag>
{
    typedef detail::distance_measure<CalculationType> result_type;

    template <typename SegmentPoint, typename Point>
    static result_type apply(SegmentPoint const& , SegmentPoint const& ,
                             Point const& )
    {
        // TODO, optional
        result_type result;
        return result;
    }
};

template <typename CalculationType>
struct get_distance_measure<CalculationType, geographic_tag>
        : get_distance_measure<CalculationType, spherical_tag> {};

} // namespace detail_dispatch

namespace detail
{

// Returns a (often very tiny) value to indicate its side, and distance,
// 0 (absolutely 0, not even an epsilon) means collinear. Like side,
// a negative means that p is to the right of p1-p2. And a positive value
// means that p is to the left of p1-p2.

template <typename SegmentPoint, typename Point>
static distance_measure<typename select_coordinate_type<SegmentPoint, Point>::type>
get_distance_measure(SegmentPoint const& p1, SegmentPoint const& p2, Point const& p)
{
    typedef typename geometry::cs_tag<Point>::type cs_tag;
    return detail_dispatch::get_distance_measure
            <
                typename select_coordinate_type<SegmentPoint, Point>::type,
                cs_tag
            >::apply(p1, p2, p);

}

} // namespace detail
#endif // DOXYGEN_NO_DETAIL

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_OVERLAY_GET_DISTANCE_MEASURE_HPP


// COLMAP - Structure-from-Motion and Multi-View Stereo.
// Copyright (C) 2017  Johannes L. Schoenberger <jsch at inf.ethz.ch>
//
// 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 3 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, see <http://www.gnu.org/licenses/>.

#include "controllers/incremental_mapper.h"
#include "controllers/bundle_adjustment.h"
#include "util/logging.h"
#include "util/misc.h"
#include "util/option_manager.h"
#include "base/reconstruction_manager.h"
#include "sfm/incremental_triangulator.h"
#include <Eigen/Geometry>

using namespace colmap;

namespace {
    typedef Eigen::Matrix3d R_t;
    typedef Eigen::Vector3d tvec_t;
    typedef Eigen::Vector4d qvec_t;
    typedef Eigen::Matrix<double, 9, 9> cov_t;
}

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

  std::string export_path;
  std::string metadata_path;

  // ssba --database_path database.db --image_path images --metadata_path
  // images/image_metadata_local.txt --export_path sparse
  OptionManager options;
  options.AddDatabaseOptions();
  options.AddImageOptions();
  options.AddRequiredOption("metadata_path", &metadata_path);
  options.AddRequiredOption("export_path", &export_path);
  options.AddBundleAdjustmentOptions();
  options.Parse(argc, argv);

  /* GAME PLAN
   * 1) Create new reconstruction
   * 2) Load all images into reconstruction
   * 3) Load camera params
   * 3) Load priors into reconstruction
   * 4) Tringulate all 3D points based on priors
   * 5) Run global bundle adjustment
   */

  // Create a new reconstruction
  ReconstructionManager reconstruction_manager;
  const size_t reconstruction_idx = reconstruction_manager.Add();
  Reconstruction& reconstruction = reconstruction_manager.Get(reconstruction_idx);

  // Load images and features from database
  Database db(*options.database_path);
  DatabaseCache db_cache;
  const size_t min_num_matches = 8;
  const bool ignore_watermarks = true;
  const std::set<std::string> image_cache_names;
  db_cache.Load(db, min_num_matches, ignore_watermarks, image_cache_names);

  // Set up reconstruction
  std::cout << "Setting up reconstruction..." << std::endl;
  {
    reconstruction.Load(db_cache);
    reconstruction.SetUp(&db_cache.CorrespondenceGraph());
    const EIGEN_STL_UMAP(image_t, class Image)& images = db_cache.Images();

    for(auto it = images.begin(); it != images.end(); ++it) {
      reconstruction.RegisterImage(it->first);
    }
  }

  // Add pre-computed params to camera
  std::cout << "Loading camera params..." << std::endl;

  const camera_t camera_id = 1;
  const size_t width = 3840;
  const size_t height = 2160;
  const std::string params = "1662.07,1920,1080,-0.021983";

  Camera& cam = reconstruction.Camera(camera_id);
  cam.SetWidth(width);
  cam.SetHeight(height);
  cam.SetParamsFromString(params);

  if(!cam.VerifyParams()) {
    std::cerr << "Camera params wrong!" << std::endl;
    return EXIT_FAILURE;
  }

  // Sets both prior and initial guess
  reconstruction.AddPriors(image_priors);

  // Triangulate the 3D position of all points
  std::cout << "Triangulating..." << std::endl;

  IncrementalTriangulator triangulator(&db_cache.CorrespondenceGraph(), &reconstruction);
  const std::vector<image_t>& image_ids = reconstruction.RegImageIds();
  const IncrementalTriangulator::Options triangulator_options;
  for(const image_t& image_id: image_ids) {
    triangulator.TriangulateImage(triangulator_options, image_id);
    triangulator.CompleteImage(triangulator_options, image_id);
  }
  triangulator.CompleteAllTracks(triangulator_options);
  triangulator.MergeAllTracks(triangulator_options);

  for(size_t i = 0; i < 2; i++) {
    if(i != 0) {
      // Retriangulate
      std::cout << "Retriangulating..." << std::endl;
      {
        triangulator.Retriangulate(triangulator_options);
        triangulator.CompleteAllTracks(triangulator_options);
        triangulator.MergeAllTracks(triangulator_options);
      }
    }

    // Run robust global BA
    std::cout << "Running robust global BA..." << std::endl;
    {
      OptionManager options_(options);
      options_.bundle_adjustment->priors = true;
      options_.bundle_adjustment->loss_function_type =
          BundleAdjustmentOptions::LossFunctionType::CAUCHY;
      options_.bundle_adjustment->loss_function_scale = 1;
      options_.bundle_adjustment->solver_options.max_num_iterations = 100;
      options_.bundle_adjustment->refine_focal_length = false;
      options_.bundle_adjustment->refine_extra_params = false;

      BundleAdjustmentController ba_controller(options_, &reconstruction);
      ba_controller.Start();
      ba_controller.Wait();
    }
  }

  // Filter points
  std::cout << "Filtering 3D points..." << std::endl;
  {
    const double max_reproj_error = 4.0;
    const double min_tri_angle = 10; // deg
    reconstruction.FilterAllPoints3D(max_reproj_error, min_tri_angle);
  }

  // Iteratively run global BA
  for(size_t i = 0; i < 5; i++) {

    // Run strict global BA
    std::cout << "Running strict global BA..." << std::endl;
    {
      OptionManager options_(options);
      options_.bundle_adjustment->priors = true;
      options_.bundle_adjustment->loss_function_type =
          BundleAdjustmentOptions::LossFunctionType::TRIVIAL;
      options_.bundle_adjustment->loss_function_scale = 1;

      BundleAdjustmentController ba_controller(options_, &reconstruction);
      ba_controller.Start();
      ba_controller.Wait();
    }

    // Complete and merge 3D point tracks
    std::cout << "Completing and merging tracks..." << std::endl;
    {
      triangulator.CompleteAllTracks(triangulator_options);
      triangulator.MergeAllTracks(triangulator_options);
    }

    // Filter points
    std::cout << "Filtering 3D points..." << std::endl;
    {
      const double max_reproj_error = 4.0;
      const double min_tri_angle = 10; // deg
      reconstruction.FilterAllPoints3D(max_reproj_error, min_tri_angle);
    }
  }

  // Save output
  std::cout << "Saving output..." << std::endl;
  reconstruction.Write(export_path);
  reconstruction.WriteText(export_path);
  // reconstruction.ExportPLY(export_path + "/model.ply");

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

  return EXIT_SUCCESS;
}


#include "QAlg/QITE/QITE.h"
#include "Core/QuantumMachine/QuantumMachineFactory.h"
#include <Eigen/Eigenvalues>

QPANDA_BEGIN

QITE::QITE()
{
}

int QITE::exec()
{
    initEnvironment();
    srand((int)time(0));

    auto tmp_ansatz = m_ansatz;
    for (int t = 0; t < m_upthrow_num; t++)
    {
        for (int i = 0; i < m_iter_num; i++)
        {
            calcParaA();
            calcParaC();

            Eigen::MatrixXd A_inverse = pseudoinverse(m_A);
            Eigen::VectorXd theta_dot = -A_inverse * m_C;
            double delta_k = m_delta_tau * pow(m_Q, i);

            for (int j = 0; j < m_theta_index_vec.size(); j++)
            {
                auto t_j = m_theta_index_vec[j];
                double r = (rand() / double(RAND_MAX));
                if (m_update_mode == UpdateMode::GD_VALUE)
                {
                    m_ansatz[t_j].theta += theta_dot[j];
                }
                else if(m_update_mode == UpdateMode::GD_DIRECTION)
                {
                    if (theta_dot[j] > 0)
                    {
                        m_ansatz[t_j].theta -= delta_k * r;
                    }
                    else
                    {
                        m_ansatz[t_j].theta += delta_k * r;
                    }
                }
                else
                {
                    return -1;
                }

                int n_period = int(m_ansatz[t_j].theta / (2 * PI));
                if (m_ansatz[t_j].theta < 0)
                {
                    m_ansatz[t_j].theta += (n_period + 1) * 2 * PI;
                }
                else
                {
                    m_ansatz[t_j].theta -= n_period * 2 * PI;
                }
            }

            double tmp_exp = getExpectation(m_ansatz);
            m_log_writer << tmp_exp << std::endl;

            if (tmp_exp < m_expectation)
            {
                m_expectation = tmp_exp;
                m_best_ansatz = m_ansatz;
            }
        }
    }

    std::fstream para_out;
    para_out.open(m_log_file + "_para.txt", std::ios::out);
    para_out << "Last_para:" << std::endl;
    for (int i = 0; i < m_theta_index_vec.size(); i++)
    {
        para_out << m_ansatz[m_theta_index_vec[i]].theta << ", ";
    }

    para_out << std::endl << "Best_para:" << std::endl;
    for (int i = 0; i < m_theta_index_vec.size(); i++)
    {
        para_out << m_best_ansatz[m_theta_index_vec[i]].theta << ", ";
    }
    para_out.close();
    m_log_writer.close();

    m_ansatz = tmp_ansatz;
    //m_machine->Free_Qubits(m_qlist);

    return 0;
}

prob_tuple QITE::getResult()
{
    QProg prog;
    prog << constructCircuit(m_best_ansatz);
    m_machine->directlyRun(prog);

    auto temp = dynamic_cast<IdealMachineInterface*>(m_machine.get());
    if (nullptr == temp)
    {
        QCERR("m_machine is not ideal machine");
        throw std::runtime_error("m_machine is not ideal machine");
    }
    auto measure_qubits = m_qlist;
    measure_qubits.pop_back();
    auto result = temp->PMeasure(measure_qubits, -1);

    std::fstream fout;
    fout.open(m_log_file + "_measure.txt", std::ios::out);

    std::cout << "Measure result: " << std::endl;
    for (auto& i : result)
    {
        if (fabs(i.second) < 1e-4)
        {
            break;
        }
        std::cout << i.first << " " << i.second << std::endl;
        fout << i.first << " " << i.second << std::endl;
    }

    fout.close();

    return result;
}

void QITE::initEnvironment()
{
    if (!m_log_file.empty())
    {
        m_log_writer.open(m_log_file, std::ios::out);
    }
    else
    {
        m_log_writer.open("test.log", std::ios::out);
    }

    m_machine.reset(QuantumMachineFactory::GetFactoryInstance()
        .CreateByType(m_quantum_machine_type));
    m_machine->init();

    m_theta_index_vec.clear();
    m_hamiltonian = m_pauli.toHamiltonian();
    auto qubits_num = 0;
    for (int i = 0; i < m_ansatz.size(); i++)
    {
        if (m_ansatz[i].target > qubits_num)
        {
            qubits_num = m_ansatz[i].target;
        }

        if (m_ansatz[i].control > qubits_num)
        {
            qubits_num = m_ansatz[i].control;
        }

        if (m_ansatz[i].type == AnsatzGateType::AGT_RX ||
            m_ansatz[i].type == AnsatzGateType::AGT_RY ||
            m_ansatz[i].type == AnsatzGateType::AGT_RZ)
        {
            m_theta_index_vec.push_back(i);
        }
    }
    qubits_num++;

    m_best_ansatz = m_ansatz;
    m_qlist = m_machine->allocateQubits(qubits_num+1);
    m_expectation = getExpectation(m_ansatz);
}

double QITE::getExpectation(const std::vector<AnsatzGate>& ansatz)
{
    QCircuit circuit = constructCircuit(ansatz);
    double expectation = 0.0;
    for (size_t i = 0; i < m_hamiltonian.size(); i++)
    {
        expectation += getExpectationOneTerm(circuit, m_hamiltonian[i]);
    }

    return expectation;
}

double QITE::getExpectationOneTerm( QCircuit c, const QHamiltonianItem& component)
{
    if (component.first.empty())
    {
        return component.second;
    }

    QProg prog;
    prog << c;

    for (auto iter : component.first)
    {
        if (iter.second == 'X')
        {
            prog << H(m_qlist[iter.first]);
        }
        else if (iter.second == 'Y')
        {
            prog << RX(m_qlist[iter.first], PI / 2);
        }
    }

    m_machine->directlyRun(prog);
    double expectation = 0;

    auto temp = dynamic_cast<IdealMachineInterface*>(m_machine.get());
    if (nullptr == temp)
    {
        QCERR("m_machine is not ideal machine");
        throw std::runtime_error("m_machine is not ideal machine");
    }
    auto measure_qubit = m_qlist;
    measure_qubit.pop_back();
    auto result = temp->PMeasure(measure_qubit, -1);

    for (auto i = 0u; i < result.size(); i++)
    {
        if (ParityCheck(result[i].first, component.first))
        {
            expectation -= result[i].second;
        }
        else
        {
            expectation += result[i].second;
        }
    }

    return expectation * component.second;
}

bool QITE::ParityCheck(size_t state, const QTerm& paulis) const
{
    size_t check = 0;
    for (auto iter = paulis.begin(); iter != paulis.end(); iter++)
    {
        auto value = state >> iter->first;
        if ((value % 2) == 1)
        {
            check++;
        }
    }

    return 1 == check % 2;
}

void QITE::calcParaA()
{
    int theta_num = m_theta_index_vec.size();
    m_A = Eigen::MatrixXd(theta_num, theta_num);
    for (int i = 0; i < theta_num; i++)
    {
        for (int j = 0; j < theta_num; j++)
        {
            auto t_i = m_theta_index_vec[i];
            auto t_j = m_theta_index_vec[j];
            if (i > j)
            {
                m_A.row(i)[j] = m_A.row(j)[i];
                continue;
            }

            int k = getAnsatzDerivativeParaNum(t_i);
            int l = getAnsatzDerivativeParaNum(t_j);

            double sum = 0;
            for (int p = 0; p < k; p++)
            {
                for (int q = 0; q < l; q++)
                {
                    auto f_ik = getAnsatzDerivativePara(t_i, p);
                    auto f_jl = getAnsatzDerivativePara(t_j, q);
                    auto ff = complexDagger(f_ik) * f_jl;
                    auto die_len = std::abs(ff);

                    if ((i ==j) &&(p == q))
                    {
                        // Identity
                        sum += die_len;
                    }
                    else
                    {
                        auto phase = std::arg(ff);
                        auto value = calcSubCircuit(
                            t_i,
                            t_j,
                            phase,
                            getAnsatzDerivativeCircuit(t_i, p),
                            getAnsatzDerivativeCircuit(t_j, q));

                        sum += die_len * value;
                    }
                }
            }

            m_A.row(i)[j] = sum;
        }
    }
}

void QITE::calcParaC()
{
    int theta_num = m_theta_index_vec.size();
    m_C = Eigen::VectorXd(theta_num);
    for (int i = 0; i < theta_num; i++)
    {
        auto t_i = m_theta_index_vec[i];
        double sum = 0;
        int k = getAnsatzDerivativeParaNum(t_i);
        int l = getHamiltonianItemNum();
        for (int p = 0; p < k; p++)
        {
            for (int q = 0; q < l; q++)
            {
                auto f_ik = getAnsatzDerivativePara(t_i, p);
                auto f_l = getHamiltonianItemPara(q);
                auto ff = complexDagger(f_ik) * f_l;
                auto die_len = std::abs(ff);
                auto phase = std::arg(ff);

                auto value = calcSubCircuit(
                    t_i,
                    m_ansatz.size(),
                    phase,
                    getAnsatzDerivativeCircuit(t_i, p),
                    getHamiltonianItemCircuit(q));

                sum += die_len * value;
            }
        }
        m_C[i] = sum;
    }
    m_C *= -1;
}

QCircuit QITE::constructCircuit(const std::vector<AnsatzGate>& ansatz)
{
    QCircuit circuit;
    for (int i = 0; i < ansatz.size(); i++)
    {
        circuit << convertAnsatzToCircuit(ansatz[i]);
    }

    return circuit;
}

std::complex<double> QITE::complexDagger(std::complex<double>& value)
{
    return std::complex<double>(value.real(), -value.imag());
}

int QITE::getAnsatzDerivativeParaNum(int i)
{
    if (i < 0 ||
        i >= m_ansatz.size())
    {
        QCERR_AND_THROW_ERRSTR(std::runtime_error,
            "bad para of i in getAnsatzDerivativeParaNum");
    }

    auto& u = m_ansatz[i];

    return u.control == -1 ? 1 : 2;
}

int QITE::getHamiltonianItemNum()
{
    return m_hamiltonian.size();
}

std::complex<double> QITE::getAnsatzDerivativePara(int i, int cnt)
{
    if (i < 0 ||
        i >= m_ansatz.size())
    {
        QCERR_AND_THROW_ERRSTR(std::runtime_error,
            "bad para of i in getAnsatzDerivativePara");
    }

    auto& u = m_ansatz[i];

    if (u.control != -1)
    {
        if (cnt < 0 ||
            cnt > 1)
        {
            QCERR_AND_THROW_ERRSTR(std::runtime_error,
                "bad para of cnt in getAnsatzDerivativePara");
        }

        return cnt == 0 ? std::complex<double>(0, -0.25) :
            std::complex<double>(0, 0.25);
    }
    else
    {
        if (cnt != 0)
        {
            QCERR_AND_THROW_ERRSTR(std::runtime_error,
                "bad para of cnt in getAnsatzDerivativePara");
        }

        return std::complex<double>(0, -0.5);
    }
}

double QITE::getHamiltonianItemPara(int i)
{
    if (i < 0 ||
        i >= m_hamiltonian.size())
    {
        QCERR_AND_THROW_ERRSTR(std::runtime_error,
            "bad para of i in getHamiltonianItemPara");
    }

    return m_hamiltonian[i].second;
}

QCircuit QITE::getAnsatzDerivativeCircuit(int i, int cnt)
{
    if (i < 0 ||
        i >= m_ansatz.size())
    {
        QCERR_AND_THROW_ERRSTR(std::runtime_error,
            "bad para of i in getAnsatzDerivativePara");
    }

    QCircuit sub_cir;
    int anc = m_qlist.size() - 1;

    auto& u = m_ansatz[i];

    if (u.control != -1)
    {
        if (cnt < 0 ||
            cnt > 1)
        {
            QCERR_AND_THROW_ERRSTR(std::runtime_error,
                "bad para of cnt in getAnsatzDerivativePara");
        }

        sub_cir << (cnt == 0 ? I(m_qlist[u.control]) : Z(m_qlist[u.control]));
    }

    switch (u.type)
    {
    case AnsatzGateType::AGT_RX:
        sub_cir << X(m_qlist[u.target]);
        break;
    case AnsatzGateType::AGT_RY:
        sub_cir << Y(m_qlist[u.target]);
        break;
    case AnsatzGateType::AGT_RZ:
        sub_cir << Z(m_qlist[u.target]);
        break;
    }

    return sub_cir.control({ m_qlist[anc] });
}

QCircuit QITE::getHamiltonianItemCircuit(int cnt)
{
    if (cnt < 0 ||
        cnt >= m_hamiltonian.size())
    {
        QCERR_AND_THROW_ERRSTR(std::runtime_error,
            "bad para of cnt in getHamiltonianItemPara");
    }

    QCircuit cir;
    auto term = m_hamiltonian[cnt].first;

    for (auto& iter : term)
    {
        switch (iter.second)
        {
        case 'X':
            cir << X(m_qlist[iter.first]);
            break;
        case 'Y':
            cir << Y(m_qlist[iter.first]);
            break;
        case 'Z':
            cir << Z(m_qlist[iter.first]);
            break;
        default:
            cir << I(m_qlist[iter.first]);
            break;
        }
    }

    return cir.control({ m_qlist[m_qlist.size() - 1] });
}

double QITE::calcSubCircuit(
    int index1,
    int index2,
    double theta,
    QCircuit cir1,
    QCircuit cir2)
{
    auto anc_quibit = m_qlist[m_qlist.size() - 1];

    QProg prog;
    for (int i = 0; i < index1; i++)
    {
        prog << convertAnsatzToCircuit(m_ansatz[i]);
    }

    prog << H(anc_quibit)
        << U1(anc_quibit, theta)
        << X(anc_quibit)
        << cir1
        << X(anc_quibit);

    for (int i = index1; i < index2; i++)
    {
        prog << convertAnsatzToCircuit(m_ansatz[i]);
    }

    prog << cir2
        << H(anc_quibit);

    auto temp = dynamic_cast<IdealMachineInterface*>(m_machine.get());
    auto result = temp->probRunDict(prog, { anc_quibit }, -1);

    return 2 * result["0"] - 1;
}

QCircuit QITE::convertAnsatzToCircuit(const AnsatzGate& u)
{
    if (u.target < 0 || u.target >= m_qlist.size())
    {
        QCERR_AND_THROW_ERRSTR(std::runtime_error,
            "bad para of target in convertAnsatzToCircuit");
    }

    QCircuit sub_cir;
    switch (u.type)
    {
    case AnsatzGateType::AGT_NOT:
        sub_cir << X(m_qlist[u.target]);
        break;
    case AnsatzGateType::AGT_H:
        sub_cir << H(m_qlist[u.target]);
        break;
    case AnsatzGateType::AGT_RX:
        sub_cir << RX(m_qlist[u.target], u.theta);
        break;
    case AnsatzGateType::AGT_RY:
        sub_cir << RY(m_qlist[u.target], u.theta);
        break;
    case AnsatzGateType::AGT_RZ:
        sub_cir << RZ(m_qlist[u.target], u.theta);
        break;
    default:
        break;
    }

    if (u.control != -1)
    {
        sub_cir.setControl({ m_qlist[u.control] });
    }

    return sub_cir;
}

Eigen::MatrixXd QITE::pseudoinverse(Eigen::MatrixXd matrix)
{
    auto svd = matrix.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
    const auto& singularValues = svd.singularValues();
    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>
        singularValuesInv(matrix.cols(), matrix.rows());
    singularValuesInv.setZero();
    double  pinvtoler = m_arbitary_cofficient;
    for (unsigned int i = 0; i < singularValues.size(); ++i) {
        if (singularValues(i) > pinvtoler)
            singularValuesInv(i, i) = 1.0 / singularValues(i);
        else
            singularValuesInv(i, i) = 0.0;
    }
    Eigen::MatrixXd pinvmat =
        svd.matrixV() * singularValuesInv * svd.matrixU().transpose();

    return pinvmat;
}

prob_tuple qite(
    const PauliOperator& h,
    const std::vector<AnsatzGate>& ansatz_gate,
    size_t iter_num,
    std::string log_file,
    QITE::UpdateMode mode,
    size_t up_throw_num,
    double delta_tau,
    double convergence_factor_Q,
    double arbitary_cofficient,
    QMachineType type)
{
    QITE alg;
    alg.setHamiltonian(h);
    alg.setAnsatzGate(ansatz_gate);
    alg.setIterNum(iter_num);
    alg.setLogFile(log_file);
    alg.setParaUpdateMode(mode);
    alg.setUpthrowNum(up_throw_num);
    alg.setDeltaTau(delta_tau);
    alg.setConvergenceFactorQ(convergence_factor_Q);
    alg.setArbitaryCofficient(arbitary_cofficient);
    alg.setQuantumMachineType(type);

    if (alg.exec() != 0)
    {
        return prob_tuple();
    }

    return alg.getResult();
}

QPANDA_END


//#include "convexdecomp.hpp"
#include <pcl/point_types.h>
#include <Eigen/Core>
#include <set>
#include <map>
#include <queue>
#include <boost/foreach.hpp>
#include <pcl/kdtree/kdtree_flann.h>
#include "sphere_sampling.hpp"
#include <iostream>
#include "utils/stl_to_string.hpp"
#include "convexdecomp.hpp"
using namespace pcl;
using namespace Eigen;
using namespace std;
using namespace util;

typedef Matrix<bool, Dynamic, Dynamic> MatrixXb;
typedef Matrix<bool, Dynamic, 1> VectorXb;
typedef vector<int> IntVec;
typedef set<int> IntSet;
typedef vector<float> FloatVec;
typedef map<int, int> Int2Int; // ass to ass
typedef map<int, IntSet> Int2IntSet;

//#define DEBUG_PRINT(...) printf(__VA_ARGS__)
#define DEBUG_PRINT(...)

namespace {

struct SupInfo {
  IntVec inds;
  FloatVec sups;
  float best;
};

vector<int> getNeighbors(const pcl::KdTreeFLANN<PointXYZ>& tree, int i_pt, int k_neighbs, float maxdist) {
  k_neighbs += 1;
  IntVec neighb_inds(k_neighbs, -666);
  FloatVec sqdists(k_neighbs, -666);
//  int n_neighbs = tree.nearestKSearch(i_pt, k_neighbs, neighb_inds, sqdists);
  int n_neighbs = tree.radiusSearch(i_pt, maxdist, neighb_inds, sqdists, k_neighbs);
  return vector<int>(neighb_inds.begin()+1, neighb_inds.begin() + n_neighbs);
}

}

namespace cloudproc {

void ConvexDecomp1(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& cloud, float thresh,
    /*optional outputs: */ std::vector<IntVec>* indices, std::vector< IntVec >* hull_indices) {
  MatrixXf dirs = getSpherePoints(1);
  ConvexDecomp(cloud, dirs, thresh, indices, hull_indices);
}

void ConvexDecomp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& cloud, const Eigen::MatrixXf& dirs, float thresh,
    /*optional outputs: */ std::vector<IntVec>* indices, std::vector< IntVec >* hull_indices) {

  int k_neighbs = 5;
  pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr tree (new pcl::KdTreeFLANN<pcl::PointXYZ>(true));
  tree->setEpsilon(0);
  tree->setInputCloud (cloud);
  int n_pts = cloud->size();
  int n_dirs = dirs.rows();

  DEBUG_PRINT("npts, ndirs %i %i\n", n_pts, n_dirs);

  MatrixXf dirs4(n_dirs, 4);
  dirs4.leftCols(3) = dirs;
  dirs4.col(3).setZero();
  MatrixXf pt2supports = Map< const Matrix<float, Dynamic, Dynamic,RowMajor > >(reinterpret_cast<const float*>(cloud->points.data()), n_pts, 4) * dirs4.transpose();

  const int UNLABELED = -1;
  IntVec pt2label(n_pts, UNLABELED);

  IntSet alldirs;
  for (int i=0; i < n_dirs; ++i) alldirs.insert(i);

  int i_seed=0;
  int i_label = 0;

  // each loop cycle, add a new cluster
  while (true) {

    // find first unlabeled point
    while (true) {
      if (i_seed == n_pts) return;
      if (pt2label[i_seed] == UNLABELED) break;
      ++i_seed;
    }

    pt2label[i_seed] = i_label;
    map<int, IntSet> pt2dirs;
    pt2dirs[i_seed] = alldirs;
    vector<SupInfo> dir2supinfo(n_dirs);
    for (int i_dir=0; i_dir < n_dirs; ++i_dir) {
      float seedsup = pt2supports(i_seed, i_dir);
      dir2supinfo[i_dir].inds.push_back(i_seed);
      dir2supinfo[i_dir].sups.push_back(seedsup);
      dir2supinfo[i_dir].best = seedsup;

    }

    DEBUG_PRINT("seed: %i\n", i_seed);

    IntSet exclude_frontier;
    exclude_frontier.insert(i_seed);
    queue<int> frontier;

    BOOST_FOREACH(const int& i_nb,  getNeighbors(*tree, i_seed, k_neighbs, 2*thresh)) {
      if (pt2label[i_nb]==UNLABELED && exclude_frontier.find(i_nb) == exclude_frontier.end()) {
        DEBUG_PRINT("adding %i to frontier\n", i_nb);
        frontier.push(i_nb);
        exclude_frontier.insert(i_nb);
      }
    }

    while (!frontier.empty()) {

#if 0 // for serious debugging
      vector<int> clu;
      BOOST_FOREACH(Int2IntSet::value_type& pt_dir, pt2dirs) {
        clu.push_back(pt_dir.first);
      }
      MatrixXd sup_pd(clu.size(), n_dirs);
      for (int i=0; i < clu.size(); ++i) {
        for (int i_dir = 0; i_dir < n_dirs; ++i_dir) {
          sup_pd(i,i_dir) = pt2supports(clu[i], i_dir);
        }
      }
      for (int i_dir=0; i_dir < n_dirs; ++i_dir) {
        IntSet nearext;
        for (int i=0; i < clu.size(); ++i) {
          if (sup_pd.col(i_dir).maxCoeff() - sup_pd(i,i_dir) < thresh) {
            nearext.insert(clu[i]);
          }
        }
        assert( toSet(dir2supinfo[i_dir].inds) == nearext );
      }
      printf("ok!\n");
#endif

      int i_cur = frontier.front();
      frontier.pop();
//      printf("cur: %i\n", i_cur);
      DEBUG_PRINT("pt2dirs %s", Str(pt2dirs).c_str());

      bool reject = false;

      Int2Int pt2decrement;
      for (int i_dir = 0; i_dir < n_dirs; ++i_dir) {
        float cursup = pt2supports(i_cur, i_dir);
        SupInfo& si = dir2supinfo[i_dir];
        if (cursup > si.best) {
          for (int i=0; i < si.inds.size(); ++i) {
            float sup = si.sups[i];
            int i_pt = si.inds[i];
            if (cursup - sup > thresh) {
              pt2decrement[i_pt] = pt2decrement[i_pt] + 1;
              DEBUG_PRINT("decrementing %i (dir %i)\n", i_pt, i_dir);
            }
          }
        }
      }

      DEBUG_PRINT("pt2dec: %s", Str(pt2decrement).c_str());

      BOOST_FOREACH(const Int2Int::value_type& pt_dec, pt2decrement) {
        if (pt_dec.second == pt2dirs[pt_dec.first].size()) {
          reject = true;
          break;
        }
      }

      DEBUG_PRINT("reject? %i\n", reject);
      if (!reject) {
        pt2label[i_cur] = i_label;
        pt2dirs[i_cur] = IntSet();
        for (int i_dir = 0; i_dir < n_dirs; ++i_dir) {
          float cursup = pt2supports(i_cur, i_dir);
          if (cursup > dir2supinfo[i_dir].best - thresh) pt2dirs[i_cur].insert(i_dir);
        }

        for (int i_dir = 0; i_dir < n_dirs; ++i_dir) {
          float cursup = pt2supports(i_cur, i_dir);
          SupInfo& si = dir2supinfo[i_dir];
          if (cursup > si.best) {

            IntVec filtinds;
            FloatVec filtsups;
            for (int i = 0; i < si.inds.size(); ++i) {
              float sup = si.sups[i];
              int i_pt = si.inds[i];
              if (cursup - sup > thresh) {
                pt2dirs[i_pt].erase(i_dir);
              }
              else {
                filtinds.push_back(i_pt);
                filtsups.push_back(sup);
              }
            }
            si.inds = filtinds;
            si.sups = filtsups;
            si.inds.push_back(i_cur);
            si.sups.push_back(cursup);
            si.best = cursup;
          }
          else if (cursup > si.best - thresh) {
            si.inds.push_back(i_cur);
            si.sups.push_back(cursup);
          }
        }
        BOOST_FOREACH(const int& i_nb,  getNeighbors(*tree, i_cur, k_neighbs, 2*thresh)) {
          if (pt2label[i_nb]==UNLABELED && exclude_frontier.find(i_nb) == exclude_frontier.end()) {
            DEBUG_PRINT("adding %i to frontier\n", i_nb);
            frontier.push(i_nb);
            exclude_frontier.insert(i_nb);
          }
        }

      } // if !reject
      else {
      }

    } // while frontier nonempty

    if (indices != NULL) {
      indices->push_back(IntVec());
      BOOST_FOREACH(Int2IntSet::value_type& pt_dir, pt2dirs) {
        indices->back().push_back(pt_dir.first);
      }
    }
    if (hull_indices != NULL) {
      hull_indices->push_back(IntVec());
      BOOST_FOREACH(Int2IntSet::value_type& pt_dirs, pt2dirs) {
        BOOST_FOREACH(const int& dir, pt_dirs.second) {
          if (pt2supports(pt_dirs.first, dir) == dir2supinfo[dir].best) {
            hull_indices->back().push_back(pt_dirs.first);
            break;
          }
        }
      }
      DEBUG_PRINT("hull size: %i/%i\n", hull_indices->back().size(), pt2dirs.size());
    }

    ++i_label;
  }

}

}


//
// Copyright (c) Leonid Seniukov. All rights reserved.
// Licensed under the MIT license. See LICENSE file in the project root for details.
//

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/graph/graphviz.hpp>

#include "poseGraph/graphAlgorithms/GraphTraverser.h"

namespace gdr {

    std::vector<std::unique_ptr<ConnectedComponentPoseGraph>>
    GraphTraverser::splitGraphToConnectedComponents(const CorrespondenceGraph &correspondenceGraph) {

        int numberOfVerticesCG = correspondenceGraph.getNumberOfPoses();

        std::vector<int> componentNumberByPose;
        std::vector<std::vector<int>> components = bfsComputeConnectedComponents(correspondenceGraph,
                                                                                componentNumberByPose);

        for (int i = 0; i < components.size(); ++i) {

            for (const auto &pose: components[i]) {
                assert(componentNumberByPose[pose] == i);
            }
        }

        std::vector<std::unique_ptr<ConnectedComponentPoseGraph>> connectedComponents;

        // fill information about poses and change indices inside each components
        // so they are locally sequentially packed [0.. component.size() - 1]

        // components container
        std::vector<std::vector<VertexPose>> connectedComponentsVertices(components.size());

        // relative poses SE3 containers
        // each vector element represents one connected component connection (edges)
        // each vector of vectors is components.size() sized
        // and i-th vector containes edges from i-th pose (local index)
        std::vector<std::vector<std::vector<RelativeSE3>>> edgesOfComponentsByComponentsNumber(components.size());

        for (int componentNumber = 0; componentNumber < components.size(); ++componentNumber) {
            edgesOfComponentsByComponentsNumber[componentNumber].resize(components[componentNumber].size());
        }

        // contains pair {connected component number, "sequentially packed" index inside component}
        // for each pose
        std::vector<std::pair<int, int>> componentNumberAndLocalIndexByPoseGlobalIndex;

        for (int poseGlobalIndex = 0; poseGlobalIndex < numberOfVerticesCG; ++poseGlobalIndex) {

            VertexPose poseInsideConnectedComponent(correspondenceGraph.getVertex(poseGlobalIndex));

            int componentNumber = componentNumberByPose[poseGlobalIndex];
            int localIndexInsideComponent = connectedComponentsVertices[componentNumber].size();
            componentNumberAndLocalIndexByPoseGlobalIndex.emplace_back(
                    std::make_pair(componentNumber, localIndexInsideComponent));
            poseInsideConnectedComponent.setIndex(localIndexInsideComponent);

            connectedComponentsVertices[componentNumber].emplace_back(poseInsideConnectedComponent);
        }

        assert(componentNumberAndLocalIndexByPoseGlobalIndex.size() == numberOfVerticesCG);

        for (int globalPoseIndex = 0; globalPoseIndex < numberOfVerticesCG; ++globalPoseIndex) {
            const auto &infoAboutpose = componentNumberAndLocalIndexByPoseGlobalIndex[globalPoseIndex];
        }

        // recompute transformation matrices using new local indices
        for (int indexFrom = 0; indexFrom < numberOfVerticesCG; ++indexFrom) {
            for (const auto &transformation: correspondenceGraph.getConnectionsFromVertex(indexFrom)) {
                assert(transformation.getIndexFrom() == indexFrom);

                int indexTo = transformation.getIndexTo();
                int componentNumberIndexFrom = componentNumberAndLocalIndexByPoseGlobalIndex[indexFrom].first;
                int localIndexFrom = componentNumberAndLocalIndexByPoseGlobalIndex[indexFrom].second;

                int componentNumberIndexTo = componentNumberAndLocalIndexByPoseGlobalIndex[indexTo].first;
                int localIndexTo = componentNumberAndLocalIndexByPoseGlobalIndex[indexTo].second;

                assert(componentNumberIndexFrom == componentNumberIndexTo);

                const Sophus::SE3d &relativePoseSE3 = transformation.getRelativePoseSE3();
                RelativeSE3 localRelativePoseSE3(localIndexFrom,
                                                 localIndexTo,
                                                 transformation.getRelativePose());
                assert(componentNumberByPose[indexFrom] == componentNumberByPose[indexTo]);
                assert(componentNumberByPose[indexFrom] == componentNumberIndexTo);

                assert(localIndexFrom < edgesOfComponentsByComponentsNumber[componentNumberIndexFrom].size());
                assert(localRelativePoseSE3.getIndexFrom() == localIndexFrom);
                assert(localRelativePoseSE3.getIndexTo() == localIndexTo);
                edgesOfComponentsByComponentsNumber[componentNumberIndexTo][localIndexFrom].emplace_back(
                        localRelativePoseSE3);

            }
        }

        //fill information about observed points with local pose indices
        std::vector<std::vector<std::pair<keyPointImageAndLocalPointIndexAndKeyPointInfo,
                keyPointImageAndLocalPointIndexAndKeyPointInfo>>>
                inlierCorrespondencesPointsInsideComponentByComponentNumber(components.size());

        for (const auto &pairOfMatchedKeyPoints: correspondenceGraph.getInlierObservedPoints()) {

            int globalPoseIndexFirst = pairOfMatchedKeyPoints.first.first.first;
            int globalPoseIndexSecond = pairOfMatchedKeyPoints.second.first.first;

            int localPoseIndexFirst = componentNumberAndLocalIndexByPoseGlobalIndex[globalPoseIndexFirst].second;
            int localPoseIndexSecond = componentNumberAndLocalIndexByPoseGlobalIndex[globalPoseIndexSecond].second;
            int componentNumber = componentNumberByPose[globalPoseIndexFirst];

            const auto &pairFirst = pairOfMatchedKeyPoints.first;
            const auto &pairSecond = pairOfMatchedKeyPoints.second;

            KeyPointInfo keyPointInfoFirst = pairFirst.second;
            keyPointInfoFirst.setObservingPoseNumber(localPoseIndexFirst);
            KeyPointInfo keyPointInfoSecond = pairSecond.second;
            keyPointInfoSecond.setObservingPoseNumber(localPoseIndexSecond);

            std::pair<std::pair<int, int>, KeyPointInfo> pointInfoLocalFirst = {
                    {localPoseIndexFirst, pairFirst.first.second}, keyPointInfoFirst};
            std::pair<std::pair<int, int>, KeyPointInfo> pointInfoLocalSecond = {
                    {localPoseIndexSecond, pairSecond.first.second}, keyPointInfoSecond};

            assert(componentNumber == componentNumberAndLocalIndexByPoseGlobalIndex[globalPoseIndexSecond].first);

            inlierCorrespondencesPointsInsideComponentByComponentNumber[componentNumber].push_back(
                    {pointInfoLocalFirst, pointInfoLocalSecond});
        }

        std::vector<std::pair<int, int>> componentSizeAndComponentNumber;
        for (int i = 0; i < components.size(); ++i) {
            componentSizeAndComponentNumber.emplace_back(std::make_pair(components[i].size(), i));
        }

        std::stable_sort(componentSizeAndComponentNumber.begin(),
                         componentSizeAndComponentNumber.end(),
                         std::greater<>());
        std::unordered_map<int, int> numberWhenSortedBySizeByGlobalIndex;

        for (int i = 0; i < componentSizeAndComponentNumber.size(); ++i) {
            auto &sizeAndIndex = componentSizeAndComponentNumber[i];
            numberWhenSortedBySizeByGlobalIndex[sizeAndIndex.second] = i;
        }

        int addedVertices = 0;
        for (int componentNumber = 0; componentNumber < components.size(); ++componentNumber) {
            addedVertices += connectedComponentsVertices[componentNumber].size();
            int numberOfComponents = components.size();
            assert(numberOfComponents == connectedComponentsVertices.size());
            assert(numberOfComponents == edgesOfComponentsByComponentsNumber.size());
            assert(numberOfComponents == inlierCorrespondencesPointsInsideComponentByComponentNumber.size());
            std::string namePrefix =
                    "comp_" + std::to_string(numberWhenSortedBySizeByGlobalIndex[componentNumber]) + "_";

            connectedComponents.emplace_back(std::make_unique<
                    ConnectedComponentPoseGraph>(connectedComponentsVertices[componentNumber],
                                                 edgesOfComponentsByComponentsNumber[componentNumber],
                                                 KeyPointMatches(
                                                         inlierCorrespondencesPointsInsideComponentByComponentNumber[componentNumber]),
                                                 componentNumber));
        }
        assert(addedVertices == correspondenceGraph.getNumberOfPoses());

        std::stable_sort(connectedComponents.begin(), connectedComponents.end(),
                         [](const auto &lhs, const auto &rhs) {
                             return lhs->getNumberOfPoses() > rhs->getNumberOfPoses();
                         });

        return connectedComponents;
    }

    std::vector<std::vector<int>>
    GraphTraverser::bfsComputeConnectedComponents(const CorrespondenceGraph &correspondenceGraph,
                                                  std::vector<int> &componentNumberByPoseIndex) {

        class V {
        };

        class C {
        };

        typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS, V, C> PoseGraphForBfs;
        typedef boost::graph_traits<PoseGraphForBfs>::vertex_descriptor VertexDescriptorForBfs;
        PoseGraphForBfs poseGraphForBfs;
        std::vector<VertexDescriptorForBfs> verticesBoost;

        for (int poseNumber = 0; poseNumber < correspondenceGraph.getNumberOfPoses(); ++poseNumber) {
            verticesBoost.push_back(boost::add_vertex(poseGraphForBfs));
        }

        for (int i = 0; i < correspondenceGraph.getNumberOfPoses(); ++i) {
            for (const auto &edge: correspondenceGraph.getConnectionsFromVertex(i)) {
                assert(i == edge.getIndexFrom());

                if (edge.getIndexFrom() > edge.getIndexTo()) {
                    continue;
                }
                boost::add_edge(verticesBoost[edge.getIndexFrom()], verticesBoost[edge.getIndexTo()], poseGraphForBfs);
            }
        }

        std::vector<int> component(boost::num_vertices(poseGraphForBfs));
        size_t num_components = boost::connected_components(poseGraphForBfs, component.data());

        std::vector<std::vector<int>> components(num_components);

        for (int i = 0; i < component.size(); ++i) {
            components[component[i]].push_back(i);
        }

        componentNumberByPoseIndex = component;
        assert(!componentNumberByPoseIndex.empty());

        for (int componentNum = 0; componentNum < components.size(); ++componentNum) {
            for (const auto &pose: components[componentNum]) {
                assert(componentNumberByPoseIndex[pose] == componentNum);
            }
        }
        return components;
    }

    void GraphTraverser::bfsDrawToFile(const CorrespondenceGraph &correspondenceGraph,
                                       const std::string &outFile) {

        class V {
        };
        class C {
        };
        typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS, V, C> PoseGraphForBfs;
        typedef boost::graph_traits<PoseGraphForBfs>::vertex_descriptor VertexDescriptorForBfs;

        PoseGraphForBfs poseGraphForBfs;
        std::vector<VertexDescriptorForBfs> verticesBoost;

        for (const auto &pose: correspondenceGraph.getVertices()) {
            verticesBoost.push_back(boost::add_vertex(poseGraphForBfs));
        }

        for (int i = 0; i < correspondenceGraph.getNumberOfPoses(); ++i) {
            for (const auto &edge: correspondenceGraph.getConnectionsFromVertex(i)) {
                assert(i == edge.getIndexFrom());

                if (edge.getIndexFrom() > edge.getIndexTo()) {
                    continue;
                }
                boost::add_edge(verticesBoost[edge.getIndexFrom()], verticesBoost[edge.getIndexTo()], poseGraphForBfs);
            }
        }

        if (!outFile.empty()) {
            std::ofstream outf(outFile);
            boost::write_graphviz(outf, poseGraphForBfs);
        }
    }
}


/***********************************************************************
 *    created:    5/7/2011
 *    author:     Martin Preisler
 *************************************************************************/
/***************************************************************************
 *   Copyright (C) 2004 - 2011 Paul D Turner & The CEGUI Development Team
 *
 *   Permission is hereby granted, free of charge, to any person obtaining
 *   a copy of this software and associated documentation files (the
 *   "Software"), to deal in the Software without restriction, including
 *   without limitation the rights to use, copy, modify, merge, publish,
 *   distribute, sublicense, and/or sell copies of the Software, and to
 *   permit persons to whom the Software is furnished to do so, subject to
 *   the following conditions:
 *
 *   The above copyright notice and this permission notice shall be
 *   included in all copies or substantial portions of the Software.
 *
 *   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 *   EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 *   MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
 *   IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR
 *   OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
 *   ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
 *   OTHER DEALINGS IN THE SOFTWARE.
 ***************************************************************************/

#include "CEGUI/Vector.h"

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(Vector)

BOOST_AUTO_TEST_CASE(ConstructionAssignment2)
{
    CEGUI::Vector2f a(1, 0);
    CEGUI::Vector2f b(0, 1);
    CEGUI::Vector2f c(2, 2);
    CEGUI::Vector2f d(2, 1);
    CEGUI::Vector2f e(1, 2);

    BOOST_CHECK_EQUAL(a + b, CEGUI::Vector2f(1, 1));
    BOOST_CHECK_EQUAL(a - b, CEGUI::Vector2f(1, -1));

    BOOST_CHECK_EQUAL(a * c, CEGUI::Vector2f(2, 0));
    BOOST_CHECK_EQUAL(b * c, CEGUI::Vector2f(0, 2));
    BOOST_CHECK_EQUAL(c * d, CEGUI::Vector2f(4, 2));
    BOOST_CHECK_EQUAL(c * e, CEGUI::Vector2f(2, 4));

    BOOST_CHECK_EQUAL(a * 2, CEGUI::Vector2f(2, 0));
    BOOST_CHECK_EQUAL(b * 2, CEGUI::Vector2f(0, 2));
}

BOOST_AUTO_TEST_CASE(ConstructionAssignment3)
{
    CEGUI::Vector3f a(1, 0, 0);
    CEGUI::Vector3f b(0, 1, 0);
    CEGUI::Vector3f c(0, 0, 1);

    BOOST_CHECK_EQUAL(a + b, CEGUI::Vector3f(1, 1, 0));
    BOOST_CHECK_EQUAL(a - b, CEGUI::Vector3f(1, -1, 0));

    BOOST_CHECK_EQUAL(a * 2, CEGUI::Vector3f(2, 0, 0));
    BOOST_CHECK_EQUAL(b * 2, CEGUI::Vector3f(0, 2, 0));
    BOOST_CHECK_EQUAL(c * 2, CEGUI::Vector3f(0, 0, 2));

    BOOST_CHECK_EQUAL(CEGUI::Vector3f::one(), CEGUI::Vector3f::one_x() + CEGUI::Vector3f::one_y() + CEGUI::Vector3f::one_z());
}

BOOST_AUTO_TEST_CASE(Constants2)
{
    BOOST_CHECK_EQUAL(CEGUI::Vector2f::zero(), CEGUI::Vector2f(0, 0));
    BOOST_CHECK_EQUAL(CEGUI::Vector2f::one(), CEGUI::Vector2f(1, 1));
    BOOST_CHECK_EQUAL(CEGUI::Vector2f::one_x(), CEGUI::Vector2f(1, 0));
    BOOST_CHECK_EQUAL(CEGUI::Vector2f::one_y(), CEGUI::Vector2f(0, 1));

    BOOST_CHECK_EQUAL(CEGUI::UVector2::zero(), CEGUI::UVector2(CEGUI::UDim(0, 0), CEGUI::UDim(0, 0)));
    BOOST_CHECK_EQUAL(CEGUI::UVector2::one(), CEGUI::UVector2(CEGUI::UDim(1, 0), CEGUI::UDim(1, 0)));
    BOOST_CHECK_EQUAL(CEGUI::UVector2::one_x(), CEGUI::UVector2(CEGUI::UDim(1, 0), CEGUI::UDim(0, 0)));
    BOOST_CHECK_EQUAL(CEGUI::UVector2::one_y(), CEGUI::UVector2(CEGUI::UDim(0, 0), CEGUI::UDim(1, 0)));

    // test if it works with the UDim zero and relative
    BOOST_CHECK_EQUAL(CEGUI::UVector2::zero(), CEGUI::UVector2(CEGUI::UDim::zero(), CEGUI::UDim::zero()));
    BOOST_CHECK_EQUAL(CEGUI::UVector2::one(), CEGUI::UVector2(CEGUI::UDim::relative(), CEGUI::UDim::relative()));
    BOOST_CHECK_EQUAL(CEGUI::UVector2::one_x(), CEGUI::UVector2(CEGUI::UDim::relative(), CEGUI::UDim::zero()));
    BOOST_CHECK_EQUAL(CEGUI::UVector2::one_y(), CEGUI::UVector2(CEGUI::UDim::zero(), CEGUI::UDim::relative()));
}

BOOST_AUTO_TEST_CASE(Constants3)
{
    BOOST_CHECK_EQUAL(CEGUI::Vector3f::zero(), CEGUI::Vector3f(0, 0, 0));
    BOOST_CHECK_EQUAL(CEGUI::Vector3f::one(), CEGUI::Vector3f(1, 1, 1));

    BOOST_CHECK_EQUAL(CEGUI::Vector3f::one_x(), CEGUI::Vector3f(1, 0, 0));
    BOOST_CHECK_EQUAL(CEGUI::Vector3f::one_y(), CEGUI::Vector3f(0, 1, 0));
    BOOST_CHECK_EQUAL(CEGUI::Vector3f::one_z(), CEGUI::Vector3f(0, 0, 1));
}

BOOST_AUTO_TEST_SUITE_END()


// Copyright (c) 2016-2020 The ZCash developers
// Copyright (c) 2020 The KSOC developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "test/test_ksoc.h"

#include "sapling/address.hpp"
#include "sapling/util.h"
#include <boost/test/unit_test.hpp>
#include <univalue.h>

#include "data/sapling_key_components.json.h"

// In script_tests.cpp
extern UniValue read_json(const std::string& jsondata);

BOOST_FIXTURE_TEST_SUITE(sapling_keystore_tests, BasicTestingSetup)

BOOST_AUTO_TEST_CASE(saplingKeys) {
    // ["sk, ask, nsk, ovk, ak, nk, ivk, default_d, default_pk_d, note_v, note_r, note_cm, note_pos, note_nf"],
    UniValue sapling_keys = read_json(std::string(json_tests::sapling_key_components, json_tests::sapling_key_components + sizeof(json_tests::sapling_key_components)));

    // Skipping over comments in sapling_key_components.json file
    for (size_t i = 2; i < 12; i++) {
        uint256 skSeed, ask, nsk, ovk, ak, nk, ivk;
        skSeed.SetHex(sapling_keys[i][0].getValStr());
        ask.SetHex(sapling_keys[i][1].getValStr());
        nsk.SetHex(sapling_keys[i][2].getValStr());
        ovk.SetHex(sapling_keys[i][3].getValStr());
        ak.SetHex(sapling_keys[i][4].getValStr());
        nk.SetHex(sapling_keys[i][5].getValStr());
        ivk.SetHex(sapling_keys[i][6].getValStr());

        diversifier_t default_d;
        std::copy_n(ParseHex(sapling_keys[i][7].getValStr()).begin(), 11, default_d.begin());

        uint256 default_pk_d;
        default_pk_d.SetHex(sapling_keys[i][8].getValStr());

        auto sk = libzcash::SaplingSpendingKey(skSeed);

        // Check that expanded spending key from primitives and from sk are the same
        auto exp_sk_2 = libzcash::SaplingExpandedSpendingKey(ask, nsk, ovk);
        auto exp_sk = sk.expanded_spending_key();
        BOOST_CHECK(exp_sk == exp_sk_2);

        // Check that full viewing key derived from sk and expanded sk are the same
        auto full_viewing_key = sk.full_viewing_key();
        BOOST_CHECK(full_viewing_key == exp_sk.full_viewing_key());

        // Check that full viewing key from primitives and from sk are the same
        auto full_viewing_key_2 = libzcash::SaplingFullViewingKey(ak, nk, ovk);
        BOOST_CHECK(full_viewing_key == full_viewing_key_2);

        // Check that incoming viewing key from primitives and from sk are the same
        auto in_viewing_key = full_viewing_key.in_viewing_key();
        auto in_viewing_key_2 = libzcash::SaplingIncomingViewingKey(ivk);
        BOOST_CHECK(in_viewing_key == in_viewing_key_2);

        // Check that the default address from primitives and from sk method are the same
        auto default_addr = sk.default_address();
        auto addrOpt2 = in_viewing_key.address(default_d);
        BOOST_CHECK(addrOpt2);
        auto default_addr_2 = addrOpt2.value();
        BOOST_CHECK(default_addr == default_addr_2);

        auto default_addr_3 = libzcash::SaplingPaymentAddress(default_d, default_pk_d);
        BOOST_CHECK(default_addr_2 == default_addr_3);
        BOOST_CHECK(default_addr == default_addr_3);
    }
}

BOOST_AUTO_TEST_CASE(StoreAndRetrieveSaplingSpendingKey) {
    CBasicKeyStore keyStore;
    libzcash::SaplingExtendedSpendingKey skOut;
    libzcash::SaplingFullViewingKey fvkOut;
    libzcash::SaplingIncomingViewingKey ivkOut;

    std::vector<unsigned char, secure_allocator<unsigned char>> rawSeed(32);
    HDSeed seed(rawSeed);
    auto sk = libzcash::SaplingExtendedSpendingKey::Master(seed);
    auto fvk = sk.expsk.full_viewing_key();
    auto ivk = fvk.in_viewing_key();
    auto addr = sk.DefaultAddress();

    // Sanity-check: we can't get a key we haven't added
    BOOST_CHECK(!keyStore.HaveSaplingSpendingKey(fvk));
    BOOST_CHECK(!keyStore.GetSaplingSpendingKey(fvk, skOut));
    // Sanity-check: we can't get a full viewing key we haven't added
    BOOST_CHECK(!keyStore.HaveSaplingFullViewingKey(ivk));
    BOOST_CHECK(!keyStore.GetSaplingFullViewingKey(ivk, fvkOut));
    // Sanity-check: we can't get an incoming viewing key we haven't added
    BOOST_CHECK(!keyStore.HaveSaplingIncomingViewingKey(addr));
    BOOST_CHECK(!keyStore.GetSaplingIncomingViewingKey(addr, ivkOut));

    // When we specify the default address, we get the full mapping
    keyStore.AddSaplingSpendingKey(sk, addr);
    BOOST_CHECK(keyStore.HaveSaplingSpendingKey(fvk));
    BOOST_CHECK(keyStore.GetSaplingSpendingKey(fvk, skOut));
    BOOST_CHECK(keyStore.HaveSaplingFullViewingKey(ivk));
    BOOST_CHECK(keyStore.GetSaplingFullViewingKey(ivk, fvkOut));
    BOOST_CHECK(keyStore.HaveSaplingIncomingViewingKey(addr));
    BOOST_CHECK(keyStore.GetSaplingIncomingViewingKey(addr, ivkOut));
    BOOST_CHECK(sk == skOut);
    BOOST_CHECK(fvk == fvkOut);
    BOOST_CHECK(ivk == ivkOut);
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef MPLLIBS_METAMONAD_V1_MONOID_HPP
#define MPLLIBS_METAMONAD_V1_MONOID_HPP

// Copyright Abel Sinkovics (abel@sinkovics.hu)  2011.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <mpllibs/metamonad/v1/fwd/monoid.hpp>
#include <mpllibs/metamonad/v1/typeclass.hpp>
#include <mpllibs/metamonad/v1/mempty.hpp>
#include <mpllibs/metamonad/v1/mappend.hpp>
#include <mpllibs/metamonad/v1/lambda_c.hpp>
#include <mpllibs/metamonad/v1/lambda.hpp>
#include <mpllibs/metamonad/v1/syntax.hpp>
#include <mpllibs/metamonad/v1/name.hpp>
#include <mpllibs/metamonad/v1/lazy.hpp>
#include <mpllibs/metamonad/v1/lazy_protect_args.hpp>

#include <boost/mpl/reverse_fold.hpp>

namespace mpllibs
{
  namespace metamonad
  {
    namespace v1
    {
      template <class Tag>
      struct monoid
      {
        MPLLIBS_V1_TYPECLASS_EXPECT(mempty);
        MPLLIBS_V1_TYPECLASS_EXPECT(mappend);
        MPLLIBS_V1_TYPECLASS_EXPECT(mconcat);
      };

      template <class Tag>
      struct monoid_defaults : monoid<typeclass_expectations>
      {
        typedef
          lambda_c<l,
            lazy<
              boost::mpl::reverse_fold<
                l,
                lazy_protect_args<mpllibs::metamonad::v1::mempty<Tag> >,
                lambda<s, t,
                  syntax<mpllibs::metamonad::v1::mappend<Tag, s, t> >
                >,
                lambda_c<x, _, x>
              >
            >
          >
          mconcat;
      };
    }
  }
}

#endif