/*
 * Copyright 2009 The VOTCA Development Team (http://www.votca.org)
 *
 * 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.
 *
 */

// Standard includes
#include <fstream>
#include <iostream>

// Third party includes
#include <boost/program_options.hpp>

// Local VOTCA includes
#include <votca/csg/cgengine.h>
#include <votca/csg/version.h>

// Local internal includes
#include "rdf_calculator.h"

using namespace std;
using namespace votca::csg;
using namespace votca::tools;

#include <cstdlib>
#include <votca/csg/csgapplication.h>

// using namespace votca::tools;
using namespace std;
using namespace votca::csg;

class CsgStatApp : public CsgApplication {
 public:
  CsgStatApp() : write_every_(0) {}

  string ProgramName() override { return "csg_partial_rdf"; }
  void HelpText(ostream &out) override;

  bool DoTrajectory() override { return true; }
  bool DoMapping() override { return true; }
  bool DoMappingDefault(void) override { return false; }
  bool DoThreaded() override { return true; }
  bool SynchronizeThreads() override { return true; }
  void Initialize() override;
  bool EvaluateOptions() override;

  void BeginEvaluate(Topology *top, Topology *top_ref) override;
  void EndEvaluate() override;

  std::unique_ptr<CsgApplication::Worker> ForkWorker() override {
    return rdf_calculator_.ForkWorker();
  }

  void MergeWorker(CsgApplication::Worker *worker) override {
    rdf_calculator_.MergeWorker(worker);
  }

 public:
  RDFCalculator rdf_calculator_;
  votca::Index write_every_;
};

void CsgStatApp::HelpText(ostream &out) {
  out << "Calculate spatially confined rdfs";
}

void CsgStatApp::Initialize() {
  CsgApplication::Initialize();
  AddProgramOptions("Specific options")("options",
                                        boost::program_options::value<string>(),
                                        "  options file defining the rdfs")(
      "subvolume_radius", boost::program_options::value<double>(),
      "Rdf calc. in spherical subvolume of this radius (from center of box)")(
      "do-vol-corr", "Correct for subvolume truncation in rdf")(
      "write-every", boost::program_options::value<votca::Index>(&write_every_),
      " (UNIMPLEMENTED) write after every block of this length, "
      "if --blocking   is set, the averages are cleared after every output")(
      "do-blocks", "  write output for blocking analysis");
}

bool CsgStatApp::EvaluateOptions() {
  CsgApplication::EvaluateOptions();
  CheckRequired("options");
  CheckRequired("subvolume_radius");
  CheckRequired("trj", "no trajectory file specified");

  rdf_calculator_.LoadOptions(OptionsMap()["options"].as<string>());

  rdf_calculator_.SetSubvolRadius(
      OptionsMap()["subvolume_radius"].as<double>());

  rdf_calculator_.WriteEvery(write_every_);
  if (OptionsMap().count("do-blocks")) {
    rdf_calculator_.DoBlocks(true);
  }

  if (OptionsMap().count("do-vol-corr")) {
    rdf_calculator_.DoVolumeCorrection(true);
  }

  rdf_calculator_.Initialize();
  return true;
}

void CsgStatApp::BeginEvaluate(Topology *top, Topology *top_ref) {
  rdf_calculator_.BeginEvaluate(top, top_ref);
}

void CsgStatApp::EndEvaluate() { rdf_calculator_.EndEvaluate(); }

int main(int argc, char **argv) {
  CsgStatApp app;
  app.Exec(argc, argv);
}


#include "stdafx.h"
#include "pop.h"
#include "params.h"
#include "data.h"
#include "rnd.h"
#include "state.h"
#include "Line2Eqn.h"
#include "EvalEqnStr.h"
#include <unordered_map>
#if defined(_WIN32)
	#include <regex>
#else
	#include <boost/regex.hpp>
#endif
#include "FitnessEstimator.h"
#include "Fitness.h"

//void getEqnForm(std::string& eqn,std::string& eqn_form);
//float getCorr(vector<float>& output,vector<float>& target,float meanout,float meantarget,int off);
//int getComplexity(string& eqn);

//void getEqnForm(std::string& eqn,std::string& eqn_form)
//{
////replace numbers with the letter c
//#if defined(_WIN32)
//	std::regex e ("(([0-9]+)(\.)([0-9]+))|([0-9]+)");
//	std::basic_string<char> tmp = "c";
//	std::regex_replace (std::back_inserter(eqn_form), eqn.begin(), eqn.end(), e,tmp);
//#else
//	boost::regex e ("(([0-9]+)(\.)([0-9]+))|([0-9]+)");
//	eqn_form = boost::regex_replace(eqn,e,"c");
//#endif
//	//(\d+\.\d+)|(\d+)
//	//eqn_form = eqn;
//	//eqn_form=std::tr1::regex_replace(eqn,e,tmp.c_str(),std::tr1::regex_constants::match_default);
//	//std::string result;
//
//
//	//std::regex_replace(std::back_inserter(eqn_form),eqn.begin(),eqn.end(),e,"c",std::regex_constants::match_default);
//	//std::regex_replace(std::back_inserter(eqn_form),eqn.begin(),eqn.end(),e,"c",std::tr1::regex_constants::match_default
//    //std::cout << result;
//	//std::cout << eqn << "\t" << eqn_form <<"\n";
//}
//int getComplexity(string& eqn)
//{
//	int complexity=0;
//	char c;
//	for(int m=0;m<eqn.size();m++){
//		c=eqn[m];
//
//		if(c=='/')
//			complexity=complexity+2;
//		else if (c=='s'){
//			if(m+2<eqn.size()){
//				if ( eqn[m+1]=='i' && eqn[m+2] == 'n'){
//					complexity=complexity+3;
//					m=m+2;
//				}
//			}
//		}
//		else if (c=='c'){
//			if(m+2<eqn.size()){
//				if ( eqn[m+1]=='o' && eqn[m+2] == 's'){
//					complexity=complexity+3;
//					m=m+2;
//				}
//			}
//		}
//		else if (c=='e'){
//			if(m+2<eqn.size()){
//				if ( eqn[m+1]=='x' && eqn[m+2] == 'p'){
//					complexity=complexity+4;
//					m=m+2;
//				}
//			}
//		}
//		else if (c=='l'){
//			if(m+2<eqn.size()){
//				if ( eqn[m+1]=='o' && eqn[m+2] == 'g'){
//					complexity=complexity+4;
//					m=m+2;
//				}
//			}
//		}
//		else if (isalpha(c) && (m+1)<eqn.size()){
//			bool pass=true;
//			while ((m+1)<eqn.size() && pass){
//				if (isalpha(eqn[m+1])) m++;
//				else pass=0;
//			}
//			complexity++;
//		}
//		else
//			complexity++;
//	}
//
//	return complexity;
//}
//void eval(node& n,vector<float>& outstack)
//{
//	switch(n.type)
//	{
//	case 'n':
//		outstack.push_back(n.value);
//		break;
//	case 'v':
//		if (n.valpt==NULL)
//			cout<<"problem";
//		else
//			outstack.push_back(*n.valpt);
//		break;
//	case '+':
//		if(outstack.size()>=2){
//				float n1 = outstack.back(); outstack.pop_back();
//				float n2 = outstack.back(); outstack.pop_back();
//
//				outstack.push_back(n2+n1);
//		}
//		break;
//	case '-':
//		if(outstack.size()>=2){
//				float n1 = outstack.back(); outstack.pop_back();
//				float n2 = outstack.back(); outstack.pop_back();
//
//				outstack.push_back(n2-n1);
//		}
//		break;
//	case '*':
//		if(outstack.size()>=2){
//				float n1 = outstack.back(); outstack.pop_back();
//				float n2 = outstack.back(); outstack.pop_back();
//
//				outstack.push_back(n2*n1);
//		}
//		break;
//	case '/':
//		if(outstack.size()>=2){
//				float n1 = outstack.back(); outstack.pop_back();
//				float n2 = outstack.back(); outstack.pop_back();
//				if(abs(n1)<0.0001)
//					outstack.push_back(0);
//				else
//					outstack.push_back(n2/n1);
//		}
//		break;
//	case 's':
//		if(outstack.size()>=1){
//				float n1 = outstack.back(); outstack.pop_back();
//				outstack.push_back(sin(n1));
//		}
//		break;
//	case 'c':
//		if(outstack.size()>=1){
//				float n1 = outstack.back(); outstack.pop_back();
//				outstack.push_back(cos(n1));
//		}
//		break;
//	case 'e':
//		if(outstack.size()>=1){
//				float n1 = outstack.back(); outstack.pop_back();
//				outstack.push_back(exp(n1));
//		}
//		break;
//	case 'l':
//		if(outstack.size()>=1){
//				float n1 = outstack.back(); outstack.pop_back();
//				if (abs(n1)<0.0001)
//					outstack.push_back(0);
//				else
//					outstack.push_back(log(n1));
//		}
//		break;
//	}
//}


#include <boost/ut.hpp>// single header
// import boost.ut;        // single module (C++20)
#include "open_viii/archive/FI.hpp"
int
  main()
{
  using namespace boost::ut::literals;
  using namespace boost::ut::operators::terse;
  using namespace boost::ut;
  using namespace open_viii::archive;
  using namespace open_viii;
  [[maybe_unused]] suite tests = [] {
    static constexpr auto check_fi =
      [](const std::uint32_t &              size,
         const std::uint32_t &              offset,
         const open_viii::CompressionTypeT &compression_type) {
        const FI fi{ size, offset, compression_type };
        expect(eq(fi.uncompressed_size(), size));
        expect(eq(fi.offset(), offset));
        expect(eq(static_cast<std::uint32_t>(fi.compression_type()),
                  static_cast<std::uint32_t>(compression_type)));
      };
    static constexpr auto check_count = [](const std::size_t &expected_count) {
      expect(eq(FI::get_count(expected_count * FI::SIZE), expected_count));
    };
    "FI check if returned values = the values we put in."_test = [] {
      check_fi(0U, 1U, CompressionTypeT::none);
      check_fi(10U, 15U, CompressionTypeT::none);
      check_fi(65U, 16U, CompressionTypeT::none);
    };
    "FI check count"_test = [] {
      // theres a version of get count that takes a file path, it just gets the
      // filesize and passes it to the other function so I don't think I need to
      // write a test for it.
      expect(eq(FI::SIZE, sizeof(FI)));
      check_count(0U);
      check_count(1U);
      check_count(40U);
      check_count(60U);
      check_count(55U);
    };
    "FI entry offset"_test = [] {
      expect(eq(FI::get_fi_entry_offset(5U, 5U), 65U));
      expect(eq(FI::get_fi_entry_offset(45U, 3U), 543U));
      expect(eq(FI::get_fi_entry_offset(90U, 1U), 1081U));
      expect(eq(FI::get_fi_entry_offset(2U, 9U), 33U));
      expect(eq(FI::get_fi_entry_offset(1U, 0U), 12U));
      expect(eq(FI::get_fi_entry_offset(19U, 55U), 283U));
    };
    "FI append"_test = [] {
      std::vector<char> buffer{};
      append_entry(buffer, FI(5U, 10U));
      expect(eq(std::size(buffer), 12U));
      const auto local_fi = FI(buffer, 0U, 0U);
      expect(eq(local_fi.offset(), 10U));
      expect(eq(local_fi.uncompressed_size(), 5U));
    };
  };
}


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

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

#include <cmath>

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

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#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 <aslam/Time.hpp>

#include <aslam/cameras.hpp>

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

#include <aslam/calibration/exceptions/BadArgumentException.h>
#include <aslam/calibration/base/Timestamp.h>

namespace aslam {
  namespace calibration {

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

    CameraValidator::CameraValidator(const sm::PropertyTree& intrinsics,
        const Options& options) :
        _options(options),
        _maxXError(0.0),
        _maxYError(0.0) {
      initVisionFramework(intrinsics);
    }

    CameraValidator::CameraValidator(const sm::PropertyTree& intrinsics, const
        sm::PropertyTree& config) :
        _maxXError(0.0),
        _maxYError(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.sigma2 = config.getDouble("sigma2", _options.sigma2);
      _options.verbose = config.getBool("verbose", _options.verbose);

      // init vision framework
      initVisionFramework(intrinsics);
    }

    CameraValidator::~CameraValidator() {
    }

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

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

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

    const std::vector<CameraValidator::ObservationPtr>&
        CameraValidator::getObservations() const {
      return _observations;
    }

    Eigen::VectorXd CameraValidator::getReprojectionErrorMean() const {
      if (_reprojectionErrorsStatistics.getValid())
        return _reprojectionErrorsStatistics.getDistribution().getMean();
      else
        return Eigen::VectorXd::Zero(0);
    }

    Eigen::VectorXd CameraValidator::getReprojectionErrorVariance() const {
      if (_reprojectionErrorsStatistics.getValid())
        return _reprojectionErrorsStatistics.getDistribution().getCovariance().
          diagonal();
      else
        return Eigen::VectorXd::Zero(0);
    }

    Eigen::VectorXd CameraValidator::getReprojectionErrorStandardDeviation()
        const {
      if (_reprojectionErrorsStatistics.getValid())
        return getReprojectionErrorVariance().array().sqrt();
      else
        return Eigen::VectorXd::Zero(0);
    }

    double CameraValidator::getReprojectionErrorMaxXError() const {
      return _maxXError;
    }

    double CameraValidator::getReprojectionErrorMaxYError() const {
      return _maxYError;
    }

    void CameraValidator::getLastImage(cv::Mat& image) const {
      if (_observations.empty())
        return;
      auto observation = _observations.back();
      cv::Mat imageCopy(observation->image().rows,
        observation->image().cols, CV_8UC3);
      cv::cvtColor(observation->image(), imageCopy, CV_GRAY2RGB);
      auto T_t_c = observation->T_t_c();
      auto T_c_t = T_t_c.inverse();
      cv::Scalar green(0, 255, 0);
      cv::Scalar red(0, 0, 255);
      EstimatorML<NormalDistribution<2> > reprojectionErrorsStatistics;
      double maxXError = 0.0;
      double maxYError = 0.0;
      double errorNormSum = 0.0;
      double maxErrorNorm = 0.0;
      const int radius = 5;
      size_t numOutliers = 0;
      for (size_t i = 0; i < _calibrationTarget->size(); ++i) {
        auto targetPoint = sm::kinematics::toHomogeneous(
          _calibrationTarget->point(i));
        Eigen::Vector2d observedPoint;
        bool success = observation->imagePoint(i, observedPoint);
        if (!success)
          continue;
        Eigen::VectorXd predictedPoint;
        success = _geometry->vsHomogeneousToKeypoint(T_c_t * targetPoint,
          predictedPoint);
        if (!success)
          continue;
        cv::circle(imageCopy, cv::Point(cvRound(observedPoint(0)),
          cvRound(observedPoint(1))), radius, green, 1, CV_AA);
        cv::line(imageCopy, cv::Point(cvRound(observedPoint(0)) - radius,
          cvRound(observedPoint(1)) - radius), cv::Point(cvRound(
          observedPoint(0)) + radius, cvRound(observedPoint(1)) + radius),
          green, 1, CV_AA);
        cv::line(imageCopy, cv::Point(cvRound(observedPoint(0)) - radius,
          cvRound(observedPoint(1)) + radius), cv::Point(cvRound(
          observedPoint(0)) + radius, cvRound(observedPoint(1)) - radius),
          green, 1, CV_AA);
        cv::circle(imageCopy, cv::Point(cvRound(predictedPoint(0)),
          cvRound(predictedPoint(1))), radius, red, 1, CV_AA);
        cv::line(imageCopy, cv::Point(cvRound(predictedPoint(0)) - radius,
          cvRound(predictedPoint(1)) - radius), cv::Point(cvRound(
          predictedPoint(0)) + radius, cvRound(predictedPoint(1)) + radius),
          red, 1, CV_AA);
        cv::line(imageCopy, cv::Point(cvRound(predictedPoint(0)) - radius,
          cvRound(predictedPoint(1)) + radius), cv::Point(cvRound(
          predictedPoint(0)) + radius, cvRound(predictedPoint(1)) - radius),
          red, 1, CV_AA);
        const Eigen::Vector2d error = predictedPoint - observedPoint;
        const double errorNorm = error.norm();
        reprojectionErrorsStatistics.addPoint(error);
        if (std::fabs(error(0)) > maxXError)
          maxXError = std::fabs(error(0));
        if (std::fabs(error(1)) > maxYError)
          maxYError = std::fabs(error(1));
        errorNormSum += errorNorm;
        if (errorNorm > maxErrorNorm)
          maxErrorNorm = errorNorm;
        const double q = boost::math::quantile(
          boost::math::chi_squared_distribution<>(2), 0.975);
        const double md2 = (error.transpose() *
          (1.0 / _options.sigma2) * Eigen::Matrix2d::Identity() * error)(0, 0);
        if (md2 > q)
          numOutliers++;
      }
      std::stringstream stream;
      stream << "Reprojection error norm: avg = " << errorNormSum /
        _calibrationTarget->size() << "   max = " << maxErrorNorm;
      cv::putText(imageCopy, stream.str(), cv::Point(10, imageCopy.rows - 40),
        cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(255, 255, 255), 1, CV_AA);
      stream.str(std::string());
      stream << "Reprojection error: mean = ["
        << reprojectionErrorsStatistics.getDistribution().getMean().transpose()
        << "]   std = [" << reprojectionErrorsStatistics.
        getDistribution().getCovariance().diagonal().array().sqrt().transpose()
        << "]   max = [" << maxXError << " " << maxYError
        << "]   outliers = " << numOutliers;
      cv::putText(imageCopy, stream.str(), cv::Point(10, imageCopy.rows - 10),
        cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(255, 255, 255), 1, CV_AA);
      image = imageCopy;
    }

    const std::vector<Eigen::Vector2d>& CameraValidator::getErrors() const {
      return _errors;
    }

    const std::vector<double>& CameraValidator::getMahalanobisDistances()
        const {
      return _errorsMd2;
    }

    size_t CameraValidator::getNumOutliers(double p) const {
      const double q = boost::math::quantile(
        boost::math::chi_squared_distribution<>(2), p);
      return std::count_if(_errorsMd2.cbegin(), _errorsMd2.cend(), [&](decltype(
        *_errorsMd2.cbegin()) x){return x > q;});
    }

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

    void CameraValidator::initVisionFramework(const sm::PropertyTree&
        intrinsics) {
      // 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 (intrinsics.getString("projection/type") == "omni")
        _geometry =
          boost::make_shared<aslam::cameras::DistortedOmniCameraGeometry>(
          intrinsics);
      else if (intrinsics.getString("projection/type") == "pinhole")
        _geometry =
          boost::make_shared<aslam::cameras::DistortedPinholeCameraGeometry>(
          intrinsics);
      else
        throw BadArgumentException<std::string>(
          intrinsics.getString("projection/type"),
          "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);
    }

    bool CameraValidator::addImage(const cv::Mat& image, sm::timing::NsecTime
        timestamp) {
      // 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;
      }

      // transformation from camera to target
      auto T_t_c = observation->T_t_c();

      // transformation from target to camera
      auto T_c_t = T_t_c.inverse();

      // iterate over checkerboard corners
      for (size_t i = 0; i < _calibrationTarget->size(); ++i) {
        auto targetPoint = sm::kinematics::toHomogeneous(
          _calibrationTarget->point(i));
        Eigen::Vector2d observedPoint;
        bool success = observation->imagePoint(i, observedPoint);
        if (!success)
          continue;
        Eigen::VectorXd predictedPoint;
        success = _geometry->vsHomogeneousToKeypoint(T_c_t * targetPoint,
          predictedPoint);
        if (!success)
          continue;
        const Eigen::Vector2d error = predictedPoint - observedPoint;
        _reprojectionErrorsStatistics.addPoint(error);
        if (std::fabs(error(0)) > _maxXError)
          _maxXError = std::fabs(error(0));
        if (std::fabs(error(1)) > _maxYError)
          _maxYError = std::fabs(error(1));
        _errors.push_back(error);
        _errorsMd2.push_back((error.transpose() *
          (1.0 / _options.sigma2) * Eigen::Matrix2d::Identity() * error)(0, 0));
      }

      // store observation for later use if needed
      _observations.push_back(observation);

      return true;
    }

  }
}


#include "parser/json_parser.hpp"
#include "route_solver/log.hpp"

#include <boost/throw_exception.hpp>
#include <boost/filesystem/operations.hpp>
#include <boost/format.hpp>
#include <boost/lexical_cast.hpp>
#include <fstream>

namespace rto {

void setZoneCircle(const nlohmann::json& j, rs::Zone& zone)
{
	std::vector<double> vals;

	for (const auto& p : j["points"])
	{
		vals.push_back(p);
	}

	// TODO: check if coefficient is correct
	zone.setCircle(vals[2] / 111.32, rs::point2d(vals[0], vals[1]));
}

void setZonePolygonPoints(const nlohmann::json& j, rs::Zone& zone)
{
	std::vector<rs::point2d> points;

	for (const auto& f : j["points"])
	{
		float lat = 0;
		float lon = 0;
		int i = 0;

		for (const auto& a : f)
		{
			switch (i++)
			{
			case 0:
				lat = a;
				break;

			case 1:
				lon = a;
				break;

			default:
				BOOST_THROW_EXCEPTION(std::runtime_error("unexpected zone type"));
			}
		}

		points.push_back(rs::point2d(lat, lon));
	}

	if (points.size() > 1)
	{
		points.push_back(points.front());
	}

	zone.setPolygonPoints(std::move(points));
}

rs::Zone zoneFromJson(uint32_t id, const nlohmann::json& j)
{
	rs::Zone z(id);
	z.setExternalId(j["id"]);
	int zone_type_int = j["zonetype"];

	switch (zone_type_int)
	{
	case 1:
		setZonePolygonPoints(j, z);
		break;

	case 2:
		setZoneCircle(j, z);
		break;

	default:
		BOOST_THROW_EXCEPTION(std::runtime_error("Unsupported zone type"));
	}

	return z;
}

rs::TaskPoint taskPointFromJson(const nlohmann::json& j)
{
	return rs::TaskPoint(rs::point2d(j["lat"], j["lon"]), j["ts"], j["te"], j["dur"]);
}

template<class T>
T get_from_string(const std::string& name, const nlohmann::json& j)
{
	return boost::lexical_cast<T>(j[name].get<std::string>());
}

rs::Vehicle vehicleFromJson(std::uint32_t id, const nlohmann::json& j)
{
	rs::Vehicle v(id);
	v.setExternalId(j["id"]);
	v.setCapacity(get_from_string<float>("capacity", j));
	v.setLength(get_from_string<float>("length", j));
	v.setHeight(get_from_string<float>("height", j));
	v.setWidth(get_from_string<float>("width", j));
	v.setTripDuration(j["trip_duration"].get<double>());
	v.setTripLength(j["trip_length"].get<double>());
	v.setCostOfUsing(get_from_string<float>("cost_of_using", j));
	v.setCostPerKM(j["cost_per_km"].get<bool>());
	v.setTripMaxTasks(get_from_string<std::uint32_t>("trip_max_tasks", j));
	std::vector<std::uint32_t> wzones;

	for (const auto& z : j["working_zone"])
	{
		wzones.push_back(z);
	}

	v.setWorkZones(std::move(wzones));
	std::vector<std::uint32_t> rzones;

	for (const auto& z : j["restricted_zone"])
	{
		rzones.push_back(z);
	}

	v.setRestrictedZones(std::move(rzones));
	return v;
}

rs::Task taskFromJson(std::uint32_t id, const nlohmann::json& j)
{
	rs::Task t(id);
	t.setExternalId(j["id"]);
	t.setCapacity(j["capacity"].get<float>());
	t.setVolume(j["volume"].get<float>());
	t.setMaxsize(j["maxsize"].get<float>());
	t.setStart(taskPointFromJson(j["start"]));
	t.setEnd(taskPointFromJson(j["end"]));
	t.setVip((bool)j["vip"].get<int>());
	std::vector<std::uint16_t> types;

	for (const auto& e : j["types"])
	{
		types.push_back(e);
	}

	std::sort(types.begin(), types.end());
	t.setTypes(std::move(types));
	return t;
}

JsonParser JsonParser::fromString(const std::string& raw)
{
	return JsonParser(nlohmann::json::parse(raw));
}

JsonParser::JsonParser(const nlohmann::json& root)
{
	std::uint32_t zoneId = 0;

	for (const auto& j : root["zones"])
	{
		m_zones.emplace_back(zoneFromJson(zoneId++, j));
	}

	LOG_TRACE << "number of zones " << m_zones.size();
	std::uint32_t vehicleId = 0;

	for (const auto& j : root["vehicles"])
	{
		m_venchiles.emplace_back(vehicleFromJson(vehicleId++, j));
	}

	LOG_TRACE << "number of vehicles " << m_venchiles.size();
	std::uint32_t taskId = 0;

	for (const auto& j : root["tasks"])
	{
		m_tasks.emplace_back(taskFromJson(taskId++, j));
	}

	LOG_TRACE << "number of tasks " << m_tasks.size();
	const auto& settings = root["settings"];

	if (settings.empty())
	{
		return;
	}

	LOG_TRACE << "settings " << settings;
	auto get_if_found = [&](const std::string & key, auto & val)
	{
		auto it = settings.find(key);

		if (it == settings.end())
		{
			return;
		}

		val = *it;
	};
	get_if_found("mean_track_speed_km_in_hour", m_speedAprox);
	m_speedAprox = (m_speedAprox * 1000.0) / (60.0 * 60.0);
	get_if_found("colony_size_limit", m_abcParams.colonySize);
	get_if_found("trial_count_max_limit", m_abcParams.trialCountMax);
	get_if_found("max_time_in_sec", m_abcParams.maxTimeInSecHardLimit);
	get_if_found("max_inactive_time_in_sec", m_abcParams.maxTimeInSecNoNewBestSolution);
	get_if_found("partial_solution_enabled", m_partialSolutionEnabled);
	get_if_found("log_level", m_logLevel);
	get_if_found("multi_threaded", m_multiThreaded);
	m_accountId = root["account_id"];
}

double JsonParser::getSpeedAprox() const
{
	return m_speedAprox;
}

std::string JsonParser::getLogLevel() const
{
	return m_logLevel;
}

void JsonParser::setTasks(const std::vector<rs::Task>& tasks)
{
	m_tasks = tasks;
}

const ABCParams& JsonParser::getAbcParams() const
{
	return m_abcParams;
}

std::uint32_t JsonParser::getAccountId() const
{
	return m_accountId;
}

bool JsonParser::getPartialSolutionEnabled() const
{
	return m_partialSolutionEnabled;
}

bool JsonParser::getMultiThreaded() const
{
	return m_multiThreaded;
}

const std::vector<rs::Vehicle>& JsonParser::getVenchiles() const
{
	return m_venchiles;
}

const std::vector<rs::Zone>& JsonParser::getZones() const
{
	return m_zones;
}

const std::vector<rs::Task>& JsonParser::getTasks() const
{
	return m_tasks;
}

void JsonParser::produceInputDataCsvToFolder(const std::string& folder) const
{
	using namespace boost::filesystem;

	if (!boost::filesystem::exists(folder))
	{
		auto s = boost::format("folder %s doesn't exists") % folder;
		BOOST_THROW_EXCEPTION(std::runtime_error(s.str()));
	}

	path p(folder);
	std::map<std::uint32_t, std::vector<rs::point2d>> pointsByZones;

	for (const auto& z : m_zones)
	{
		for (const auto& pt : z.getPoints())
		{
			pointsByZones[z.getId()].push_back(pt);
		}
	}

	for (const auto& v : m_venchiles)
	{
		std::ofstream of;
		of.exceptions(std::ifstream::failbit | std::ifstream::badbit);
		auto v_file = (boost::format("venchile_%s.csv") % std::to_string(v.getId())).str();
		of.open((p / path(v_file)).string());
		of << "lat,lon,name" << std::endl;

		for (const auto& zoneId : v.getWorkZonesExternalIds())
		{
			for (const auto& pt : pointsByZones[zoneId])
			{
				of << pt.lat << "," << pt.lon << "," << "wz_" << zoneId << std::endl;
			}
		}
	}

	std::ofstream of;
	of.exceptions(std::ifstream::failbit | std::ifstream::badbit);
	of.open((p / "tasks.csv").string());

	for (const auto& t : m_tasks)
	{
		of << "lat,lon,name" << std::endl;
		of << t.getStart().point.lat << "," << t.getStart().point.lon << ",task_" << t.getExternalId() << "_start" << std::endl;
		of << t.getEnd().point.lat << "," << t.getEnd().point.lon << ",task_" << t.getExternalId() << "_end" << std::endl;
	}

	//std::map<std::uint32_t, workzone> zones;
	//std::vector<venchile> venchiles;
	//std::vector<task> tasks;
}

}


#ifndef NUMPYVIEW_INCL_HXX
#define	NUMPYVIEW_INCL_HXX

#include <boost/python.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
#include <numpy/noprefix.h>
#ifdef Bool
#undef Bool
#endif
#include <stddef.h>
#include <opengm/graphicalmodel/graphicalmodel.hxx>

//template <typename T>
//inline PyArray_TYPES typeEnumFromType(void);

namespace opengm{
namespace python{

using namespace boost::python;

//template<class T, size_t DIM>
//class NumpyViewAccessor1d;

template<class V,size_t DIM=0>
class NumpyView{
public:
   typedef V ValueType;
   typedef V  * CastPtrType;
   typedef int const * ShapePtrType;
   typedef typename marray::View< V ,false >::iterator IteratorType;
   typedef typename marray::View< V ,false >::const_iterator ConstIteratorType;
   typedef size_t const *  ShapeIteratorType;

   NumpyView():allocFromCpp_(false){

   }
   NumpyView( boost::python::object  obj):allocFromCpp_(false){
      boost::python::numeric::array array = boost::python::extract<boost::python::numeric::array > (obj);
      void * voidDataPtr=PyArray_DATA(array.ptr());
      CastPtrType dataPtr = static_cast<CastPtrType>(voidDataPtr);
      size_t dimension =static_cast<size_t>(PyArray_NDIM(array.ptr()));

      npy_intp * shapePtr = PyArray_DIMS(array.ptr());
      npy_intp * stridePtr = PyArray_STRIDES(array.ptr());
      opengm::FastSequence<size_t> mystrides(dimension);
      for(size_t i=0;i<dimension;++i){
         mystrides[i]=(stridePtr[i])/sizeof(V);
      }
      view_.assign(shapePtr,shapePtr+dimension,mystrides.begin(),dataPtr,marray::FirstMajorOrder);
   }

   NumpyView( boost::python::numeric::array  array):allocFromCpp_(false){
      void * voidDataPtr=PyArray_DATA(array.ptr());
      CastPtrType dataPtr = static_cast<CastPtrType>(voidDataPtr);
      size_t dimension =static_cast<size_t>(PyArray_NDIM(array.ptr()));
      npy_intp * shapePtr = PyArray_DIMS(array.ptr());
      npy_intp * stridePtr = PyArray_STRIDES(array.ptr());
      opengm::FastSequence<size_t> mystrides(dimension);
      for(size_t i=0;i<dimension;++i){
         mystrides[i]=(stridePtr[i])/sizeof(V);
      }
      view_.assign(shapePtr,shapePtr+dimension,mystrides.begin(),dataPtr,marray::FirstMajorOrder);
   }

   size_t size()const {return view_.size();}
   size_t dimension()const{return view_.dimension();}
   size_t shape(const size_t i)const{return view_.shape(i);}
    size_t const * shapeBegin()const{return view_.shapeBegin();}
    size_t const * shapeEnd()const{return view_.shapeEnd();}
   void error(const std::string &reason=std::string(" "))const{throw opengm::RuntimeError(reason);}
   //ShapeIteratorType shapeBegin()const{return view_.shapeBegin();}
   //ShapeIteratorType shapeEnd()const{return view_.shapeBegin();}

   template<class X0>
   const ValueType & operator()(X0 x0)const{
      return view_(x0);
   }
   const ValueType & operator()(const size_t x0,const size_t x1)const{
      return view_(x0,x1);
   }
   const ValueType & operator()(const size_t x0,const size_t x1,const size_t x2)const{
      return view_(x0,x1,x2);
   }
   const ValueType & operator()(const size_t x0,const size_t x1,const size_t x2,const size_t x3)const{
      return view_(x0,x1,x2,x3);
   }
   const ValueType & operator()(const size_t x0,const size_t x1,const size_t x2,const size_t x3,const size_t x4)const{
      return view_(x0,x1,x2,x3,x4);
   }
   /*
   const ValueType & operator()(const size_t x0,const size_t x1,const size_t x2,const size_t x3,const size_t x4,const size_t x5)const{
      return view_(x0,x1,x2,x3,x4,x5);
   }
   const ValueType & operator()(const size_t x0,const size_t x1,const size_t x2,const size_t x3,const size_t x4,const size_t x5,const size_t x6)const{
      return view_(x0,x1,x2,x3,x4,x5,x6);
   }
   const ValueType & operator()(const size_t x0,const size_t x1,const size_t x2,const size_t x3,const size_t x4,const size_t x5,const size_t x6,const size_t x7)const{
      return view_(x0,x1,x2,x3,x4,x5,x6,x7);
   }
   const ValueType & operator()(const size_t x0,const size_t x1,const size_t x2,const size_t x3,const size_t x4,const size_t x5,const size_t x6,const size_t x7,const size_t x8)const{
      return view_(x0,x1,x2,x3,x4,x5,x6,x7,x8);
   }
   const ValueType & operator()(const size_t x0,const size_t x1,const size_t x2,const size_t x3,const size_t x4,const size_t x5,const size_t x6,const size_t x7,const size_t x8,const size_t x9)const{
      return view_(x0,x1,x2,x3,x4,x5,x6,x7,x8,x9);
   }
   */
   template<class ITERATOR>
   const ValueType & operator[](ITERATOR  iterator)const{
      return view_(iterator);
   }

   template<class X0>
   ValueType & operator()(X0 x0){
      return view_(x0);
   }
   ValueType & operator()(const size_t x0,const size_t x1){
      return view_(x0,x1);
   }
   ValueType & operator()(const size_t x0,const size_t x1,const size_t x2){
      return view_(x0,x1,x2);
   }
   ValueType & operator()(const size_t x0,const size_t x1,const size_t x2,const size_t x3){
      return view_(x0,x1,x2,x3);
   }
   ValueType & operator()(const size_t x0,const size_t x1,const size_t x2,const size_t x3,const size_t x4){
      return view_(x0,x1,x2,x3,x4);
   }
   /*
   ValueType & operator()(const size_t x0,const size_t x1,const size_t x2,const size_t x3,const size_t x4,const size_t x5){
      return view_(x0,x1,x2,x3,x4,x5);
   }
   ValueType & operator()(const size_t x0,const size_t x1,const size_t x2,const size_t x3,const size_t x4,const size_t x5,const size_t x6){
      return view_(x0,x1,x2,x3,x4,x5,x6);
   }
   ValueType & operator()(const size_t x0,const size_t x1,const size_t x2,const size_t x3,const size_t x4,const size_t x5,const size_t x6,const size_t x7){
      return view_(x0,x1,x2,x3,x4,x5,x6,x7);
   }
   ValueType & operator()(const size_t x0,const size_t x1,const size_t x2,const size_t x3,const size_t x4,const size_t x5,const size_t x6,const size_t x7,const size_t x8){
      return view_(x0,x1,x2,x3,x4,x5,x6,x7,x8);
   }
   ValueType & operator()(const size_t x0,const size_t x1,const size_t x2,const size_t x3,const size_t x4,const size_t x5,const size_t x6,const size_t x7,const size_t x8,const size_t x9){
      return view_(x0,x1,x2,x3,x4,x5,x6,x7,x8,x9);
   }
   */
   template<class ITERATOR>
   ValueType & operator[](ITERATOR  iterator){
      return view_(iterator);
   }

   ConstIteratorType begin1d()const{
      return view_.begin();
   }
   ConstIteratorType end1d()const{
      return view_.end();
   }
   IteratorType begin1d(){
      return view_.begin();
   }
   IteratorType end1d(){
      return view_.end();
   }

   ConstIteratorType begin()const{
      return view_.begin();
   }
   ConstIteratorType end()const{
      return view_.end();
   }
   IteratorType begin(){
      return view_.begin();
   }
   IteratorType end(){
      return view_.end();
   }

   marray::View< V, false > getSliceView(size_t dimension, size_t sliceIndex) {
      // create base coordinate
      std::vector<size_t> baseIterator(view_.dimension(), 0);
      baseIterator[dimension] = sliceIndex;

      // create expected shape
      std::vector<size_t> shapeIterator(view_.shapeBegin(), view_.shapeEnd());
      shapeIterator[dimension] = 1;

      // return sub view of slice with reduced dimensions
      marray::View< V, false > new_view = view_.view(baseIterator.begin(), shapeIterator.begin());
      new_view.squeeze();
      return new_view;
   }

   marray::View< V ,false > view()const{
        return view_;
   }

   //boost::python::object arrayObject()const{
   //   return arrayObj_;
   //};
private:
   //boost::python::numeric::array arrayObj_;
   bool allocFromCpp_;
   marray::View< V ,false > view_;
   void * arrayData_;
};

}
}

#endif	/* NUMPYVIEW_INCL_HXX */


/* -*-c++-*--------------------------------------------------------------------
 * 2019 Bernd Pfrommer bernd.pfrommer@gmail.com
 */

#include "tagslam/factor/distance.h"
#include "tagslam/graph.h"
#include "tagslam/body.h"
#include "tagslam/logging.h"
#include "tagslam/xml.h"
#include <geometry_msgs/Point.h>
#include <XmlRpcException.h>
#include <string>
#include <sstream>
#include <boost/range/irange.hpp>

namespace tagslam {
  namespace factor {
    using boost::irange;

    Distance::Distance(double dist,  double noise,
                       const int corn1, const TagConstPtr &tag1,
                       const int corn2, const TagConstPtr &tag2,
                       const string  &name) :
      Factor(name, ros::Time(0)), distance_(dist), noise_(noise) {
      tag_[0] = tag1; tag_[1] = tag2;
      corner_[0] = corn1; corner_[1] = corn2;
    }

    VertexDesc Distance::addToGraph(const VertexPtr &vp, Graph *g) const {
      const ros::Time t0 = ros::Time(0);
      const VertexDesc vt1p = g->findTagPose(getTag(0)->getId());
      checkIfValid(vt1p, "tag pose 1 not found");
      const VertexDesc vt2p = g->findTagPose(getTag(1)->getId());
      checkIfValid(vt2p, "tag pose 2 not found");
      const VertexDesc vb1p = g->findBodyPose(t0,
                                              getTag(0)->getBody()->getName());
      checkIfValid(vb1p, "body pose 1 not found");
      const VertexDesc vb2p = g->findBodyPose(t0,
                                              getTag(1)->getBody()->getName());
      checkIfValid(vb2p, "body pose 2 not found");
      const VertexDesc fv = g->insertFactor(vp);
      g->addEdge(fv, vb1p, 0);
      g->addEdge(fv, vt1p, 1);
      g->addEdge(fv, vb2p, 2);
      g->addEdge(fv, vt2p, 3);
      return (fv);
    }

    void Distance::addToOptimizer(Graph *g) const {
      const VertexDesc v = g->find(this);
      checkIfValid(v, "factor not found");
      const std::vector<ValueKey> optKeys = g->getOptKeysForFactor(v, 4);
      const FactorKey fk = g->getOptimizer()->addDistanceMeasurement(
        getDistance(), getNoise(), getCorner(0),
        optKeys[0] /* T_w_b1 */,  optKeys[1] /* T_b1_o */,
        getCorner(1), optKeys[2] /* T_w_b2 */, optKeys[3] /* T_b2_o */);
      g->markAsOptimized(v, fk);
    }

    const Eigen::Vector3d Distance::getCorner(int idx) const {
      return (tag_[idx]->getObjectCorner(corner_[idx]));
    }

    double Distance::distance(
      const Transform &T_w_b1, const Transform &T_b1_o,
      const Transform &T_w_b2, const Transform &T_b2_o) const {
      const auto X1 = T_w_b1 * T_b1_o * getCorner(0);
      const auto X2 = T_w_b2 * T_b2_o * getCorner(1);
      return ((X2 - X1).norm());
    }

    DistanceFactorPtr
    Distance::parse(const string &name, XmlRpc::XmlRpcValue meas,
                    TagFactory *tagFactory) {
      try {
        const int tag1  = xml::parse<int>(meas, "tag1");
        const int tag2  = xml::parse<int>(meas, "tag2");
        const int c1    = xml::parse<int>(meas, "corner1");
        const int c2    = xml::parse<int>(meas, "corner2");
        const double d  = xml::parse<double>(meas, "distance");
        const double noise = xml::parse<double>(meas, "noise");

        TagConstPtr tag1Ptr = tagFactory->findTag(tag1);
        TagConstPtr tag2Ptr = tagFactory->findTag(tag2);
        if (!tag1Ptr || !tag2Ptr) {
          ROS_WARN_STREAM("ignoring distance factor with invalid "
                          " tags: " << name);
          return DistanceFactorPtr();
        }
        const DistanceFactorPtr fp(
          new factor::Distance(d, noise, c1, tag1Ptr, c2, tag2Ptr, name));
        return (fp);
      } catch (const XmlRpc::XmlRpcException &e) {
        BOMB_OUT("error parsing measurement:" + name);
      }
    }

    DistanceFactorPtrVec
    Distance::parse(XmlRpc::XmlRpcValue meas, TagFactory *tagFactory) {
      DistanceFactorPtrVec dv;
      if (meas.getType() == XmlRpc::XmlRpcValue::TypeInvalid) {
        throw std::runtime_error("invalid node type for measurement!");
      }
      for (const auto i: irange(0, meas.size())) {
        if (meas[i].getType() != XmlRpc::XmlRpcValue::TypeStruct) continue;
        for (XmlRpc::XmlRpcValue::iterator it = meas[i].begin();
             it != meas[i].end(); ++it) {
          if (it->second.getType() != XmlRpc::XmlRpcValue::TypeStruct) {
            continue;
          }
          DistanceFactorPtr d = parse(it->first, it->second, tagFactory);
          if (d) {
            dv.push_back(d);
          }
        }
      }
      return (dv);
    }

    string Distance::getLabel() const {
      std::stringstream ss;
      ss << name_;
      return (ss.str());
    }

    // static function!
    double Distance::getOptimized(const VertexDesc &v, const Graph &g) {
      if (!g.isOptimized(v)) {
        return (-1.0);
      }
      const auto p = std::dynamic_pointer_cast<const factor::Distance>(g[v]);
      if (!p) {
        BOMB_OUT("vertex is not distance: " << g[v]);
      }
      const std::vector<ValueKey> optKeys = g.getOptKeysForFactor(v, 4);
      const auto opt = g.getOptimizer();
      auto d = p->distance(opt->getPose(optKeys[0]), opt->getPose(optKeys[1]),
                           opt->getPose(optKeys[2]), opt->getPose(optKeys[3]));
      return (d);
    }
  } // namespace factor
}  // namespace tagslam


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

  Defines the as_logical meta-function

  @copyright 2016 NumScale SAS

  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_META_REAL_OF_HPP_INCLUDED
#define BOOST_SIMD_META_REAL_OF_HPP_INCLUDED

#include <boost/simd/config.hpp>
#include <boost/simd/detail/dispatch/meta/scalar_of.hpp>

namespace boost { namespace simd
{
  /*!
    @ingroup  group-api
    @brief    Compute the underlying real type of a given type

    For any given type @c T , computes a scalar underlying type which can represent a value
    from \f$\mathbb{R}\f$.

    @tparam T      Type to convert
  **/
  template<typename T>
  struct real_of : boost::dispatch::scalar_of<T>
  {};

  /*!
    @ingroup  group-api
    @brief Eager short-cut to real_of meta-function
  **/
  template<typename T> using real_of_t = typename real_of<T>::type;
} }

#endif


// Copyright 2015-2020 The ALMA Project Developers
//
// 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.

/// @file
/// Compute the thermal conductivity of Si under the RTA to test
/// that we can compute three-phonon scattering probabilities
/// correctly.

#include <iostream>
#include <functional>
#include <boost/filesystem.hpp>
#include <boost/mpi.hpp>
#include "gtest/gtest.h"
#include <utilities.hpp>
#include <vasp_io.hpp>
#include <qpoint_grid.hpp>
#include <processes.hpp>
#include <bulk_properties.hpp>

TEST(conductivity_case, conductivity_test) {
    boost::mpi::environment env;

    auto basedir = boost::filesystem::path(TEST_RESOURCE_DIR);
    auto si_dir = basedir / boost::filesystem::path("Si");
    auto poscar_path = si_dir / boost::filesystem::path("POSCAR");
    auto ifc_path = si_dir / boost::filesystem::path("FORCE_CONSTANTS");
    auto thirdorder_path =
        si_dir / boost::filesystem::path("FORCE_CONSTANTS_3RD");

    auto poscar = alma::load_POSCAR(poscar_path.string().c_str());
    auto syms = alma::Symmetry_operations(*poscar);
    auto force_constants =
        alma::load_FORCE_CONSTANTS(ifc_path.string().c_str(), *poscar, 5, 5, 5);
    auto thirdorder = alma::load_FORCE_CONSTANTS_3RD(
        thirdorder_path.string().c_str(), *poscar);

    auto grid = std::make_unique<alma::Gamma_grid>(
        *poscar, syms, *force_constants, 12, 12, 12);

    grid->enforce_asr();

    boost::mpi::communicator world;
    auto my_id = world.rank();
    auto processes = alma::find_allowed_threeph(*grid, world, 0.1);

    double reference[] = {262.594,
                          188.086,
                          147.672,
                          122.172,
                          104.504,
                          91.4766,
                          81.4379,
                          73.4452,
                          66.9194,
                          61.4835};

    std::vector<double> Ts;

    for (auto i = 0; i < 10; ++i)
        Ts.emplace_back(200. + 50. * i);

    // Precompute the matrix elements.
    for (std::size_t i = 0; i < processes.size(); ++i)
        processes[i].compute_vp2(*poscar, *grid, *thirdorder);
    auto pos = 0;

    for (auto T : Ts) {
        Eigen::ArrayXXd total_w0(
            alma::calc_w0_threeph(*grid, processes, T, world));

        if (my_id == 0) {
            auto kappa = alma::calc_kappa(*poscar, *grid, total_w0, T);
            EXPECT_TRUE(
                alma::almost_equal(reference[pos], kappa(0, 0), 1e-6, 5e-3));
            EXPECT_TRUE(
                alma::almost_equal(reference[pos], kappa(1, 1), 1e-6, 5e-3));
            EXPECT_TRUE(
                alma::almost_equal(reference[pos], kappa(2, 2), 1e-6, 5e-3));
            // Test the scalar calculation as well.
            Eigen::Vector3d direction{Eigen::Vector3d::Random()};
            auto kappa1d =
                alma::calc_kappa_1d(*poscar, *grid, total_w0, T, direction);
            direction.normalize();
            EXPECT_TRUE(alma::almost_equal(
                direction.dot(kappa * direction), kappa1d, 1e-8, 1e-3));
            kappa = syms.symmetrize_m<double>(kappa, true);
            kappa(0, 0) = kappa(1, 1) = kappa(2, 2) = 0.;
            EXPECT_TRUE(alma::almost_equal(kappa.norm(), 0., 1e-8, 0.));
            ++pos;
        }
    }
}


#ifndef PYTHONIC_INCLUDE_NUMPY_ARCTANH_HPP
#define PYTHONIC_INCLUDE_NUMPY_ARCTANH_HPP

#include "pythonic/include/utils/functor.hpp"
#include "pythonic/include/types/ndarray.hpp"
#include "pythonic/include/utils/numpy_traits.hpp"
#include <boost/simd/function/atanh.hpp>

PYTHONIC_NS_BEGIN

namespace numpy
{
#define NUMPY_NARY_FUNC_NAME arctanh
#define NUMPY_NARY_FUNC_SYM boost::simd::atanh
#include "pythonic/include/types/numpy_nary_expr.hpp"
}
PYTHONIC_NS_END

#endif


// -*- mode: c++ -*-
#include "twitter.hh"
#include "json.hh"
#include "pqjoin.hh"
#include "pqclient.hh"
#include "pqmulticlient.hh"
#include "memcacheadapter.hh"
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <math.h>
#include <sys/resource.h>
#include "time.hh"
namespace pq {
const char TwitterPopulator::tweet_data[] = "................................................................................................................................................................";

TwitterPopulator::TwitterPopulator(const Json& param)
    : nusers_(param["nusers"].as_i(5000)),
      push_(param["push"].as_b(false)),
      pull_(param["pull"].as_b(false)),
      log_(param["log"].as_b(false)),
      fetch_(param["fetch"].as_b(false)),
      synchronous_(param["synchronous"].as_b(false)),
      min_followers_(param["min_followers"].as_i(10)),
      min_subs_(param["min_subscriptions"].as_i(20)),
      max_subs_(param["max_subscriptions"].as_i(200)),
      shape_(param["shape"].as_d(55)),
      duration_(param["duration"].as_i(1000000)),
      ppost_(param["ppost"].as_i(2)),
      psubscribe_(param["psubscribe"].as_i(3)),
      celebrity_(param["celebrity"].as_i(0)),
      celebrity_type_(param["celebrity_type"].as_i(0)) {
}

uint32_t* TwitterPopulator::subscribe_probabilities(generator_type& gen) {
    uint32_t expected_subs = (min_subs_ + max_subs_) / 2;
    double* follow_shape = new double[nusers_];
    while (1) {
	double shape_effect = 0;
	for (uint32_t i = 0; i != nusers_; ++i) {
	    double x = pow((double) (i + 1) / nusers_, shape_);
	    follow_shape[i] = x;
	    shape_effect += x;
	}
        max_followers_ = (expected_subs - min_followers_) / (shape_effect / nusers_);
	if (max_followers_ <= nusers_ / 4)
            break;
        // change shape, try again
        shape_ = shape_ * 0.7;
        std::cerr << "RETRYING with shape " << shape_ << "...\n";
    }

    rng_type rng(gen);
    std::random_shuffle(follow_shape, follow_shape + nusers_, rng);

    uint32_t* sub_prob = new uint32_t[nusers_];
    assert(gen.min() == 0);
    double scale = gen.max() / (expected_subs * (double) nusers_);
    double pos = 0;
    for (uint32_t i = 0; i != nusers_; ++i) {
        pos += min_followers_ + follow_shape[i] * max_followers_;
        sub_prob[i] = (uint32_t) (pos * scale);
    }
    sub_prob[nusers_ - 1] = gen.max();

    delete[] follow_shape;
    return sub_prob;
}

void TwitterPopulator::create_subscriptions(generator_type& gen) {
    uint32_t* sub_prob = subscribe_probabilities(gen);
    uint32_t* subvec = new uint32_t[(nusers_ + 31) / 32];
    rng_type rng(gen);
    subs_.clear();
    std::vector<std::pair<uint32_t, uint32_t> > followers;

    for (uint32_t i = 0; i != nusers_; ++i) {
	memset(subvec, 0, sizeof(uint32_t) * ((nusers_ + 31) / 32));
	uint32_t nsubs = min_subs_ + rng(max_subs_ - min_subs_ + 1);
	for (uint32_t j = 0; j != nsubs; ++j) {
	    // pick follow
	    uint32_t other;
	    do {
		other = std::upper_bound(sub_prob, sub_prob + nusers_, gen()) - sub_prob;
	    } while (subvec[other / 32] & (1U << (other % 32)));
            subs_.push_back(std::make_pair(i, other));
            followers.push_back(std::make_pair(other, i));
            subvec[other / 32] |= 1U << (other % 32);
        }
    }

    delete[] sub_prob;
    delete[] subvec;

    followers_.clear();
    followers_.reserve(followers.size());
    follower_ptrs_.clear();
    follower_ptrs_.reserve(nusers_ + 1);

    std::sort(followers.begin(), followers.end());
    for (auto& sub : followers) {
        while (follower_ptrs_.size() <= sub.first)
            follower_ptrs_.push_back(followers_.size());
        followers_.push_back(sub.second);
    }
    while (follower_ptrs_.size() <= nusers_)
        follower_ptrs_.push_back(followers_.size());
}

void TwitterPopulator::print_subscription_statistics(std::ostream& stream) const {
    using namespace boost::accumulators;

    uint32_t* num_followers = new uint32_t[nusers_];
    memset(num_followers, 0, nusers_ * sizeof(uint32_t));
    for (auto& sub : subs_)
        ++num_followers[sub.second];
    std::sort(num_followers, num_followers + nusers_);

    accumulator_set<uint32_t, stats<tag::variance> > acc;
    for (uint32_t i = 0; i != nusers_; ++i)
        acc(num_followers[i]);

    stream << nusers_ << " USERS HAVE SUBSCRIBERS:\n"
           << "  zero: " << (std::upper_bound(num_followers, num_followers + nusers_, 0) - num_followers)
           << "  mean: " << mean(acc)
           << "  sdev: " << sqrt(variance(acc)) << "\n"
           << "  min " << num_followers[0]
           << ", 1% " << num_followers[(int) (0.01 * nusers_)]
           << ", 10% " << num_followers[(int) (0.10 * nusers_)]
           << ", 25% " << num_followers[(int) (0.25 * nusers_)]
           << ", 50% " << num_followers[(int) (0.50 * nusers_)]
           << ", 75% " << num_followers[(int) (0.75 * nusers_)]
           << ", 90% " << num_followers[(int) (0.90 * nusers_)]
           << ", 99% " << num_followers[(int) (0.99 * nusers_)]
           << ", max " << num_followers[nusers_ - 1] << "\n";

    delete[] num_followers;
}

typedef TwitterShim<DirectClient> local_shim_type;

tamed void run_twitter_local(TwitterPopulator& tp, Server& server) {
    tvars {
        DirectClient* client = new DirectClient(server);
        local_shim_type* shim = new local_shim_type(*client);
        TwitterRunner<local_shim_type>* tr = new TwitterRunner<local_shim_type>(*shim, tp);
    }

    twait { tr->populate(make_event()); }
    twait { tr->run(make_event()); }
    delete tr;
    delete shim;
    delete client;
    server.control(Json().set("quit", true));
}

typedef TwitterShim<MultiClient> remote_shim_type;

tamed void run_twitter_remote(TwitterPopulator& tp, int client_port,
                              const Hosts* hosts, const Hosts* dbhosts,
                              const Partitioner* part) {
    tvars {
        MultiClient* mc = new MultiClient(hosts, part, client_port);
        remote_shim_type* shim = new remote_shim_type(*mc);
        TwitterRunner<remote_shim_type>* tr = new TwitterRunner<remote_shim_type>(*shim, tp);
    }
    twait { mc->connect(make_event()); }
    twait { tr->populate(make_event()); }
    twait { tr->run(make_event()); }
    delete tr;
    delete shim;
    delete mc;
}

#if HAVE_MEMCACHED_PROTOCOL_BINARY_H
typedef pq::TwitterHashShim<pq::MemcacheMultiClient> memcache_shim_type;

tamed void run_twitter_memcache(TwitterPopulator& tp,
                                const Hosts* hosts, const Partitioner* part) {
    tvars {
        MemcacheMultiClient* mc = new MemcacheMultiClient(hosts, part);
        memcache_shim_type* shim = new memcache_shim_type(*mc);
        TwitterRunner<memcache_shim_type>* tr = new TwitterRunner<memcache_shim_type>(*shim, tp);
    }
    twait { mc->connect(make_event()); }
    twait { tr->populate(make_event()); }
    twait { tr->run(make_event()); }
    delete tr;
    delete shim;
    delete mc;
}
#else
void run_twitter_memcache(TwitterPopulator&, const Hosts*, const Partitioner*) {
    mandatory_assert(false && "Not configured for Memcache!");
}
#endif

} // namespace pq


#include <glad/glad.h>
#include <GLFW/glfw3.h>
#include <Eigen/Dense>
#include <stdio.h>
#include <iostream>
#include <fstream>
#include <string>
#include <cmath>
#include "matutils.hpp"
#include "shader.hpp"

#define STB_IMAGE_IMPLEMENTATION
#include "stb_image.h"

#define RES_PATH(FILE) (std::string(RES_DIR)+std::string("/")+std::string(FILE)).c_str()

// OK boi start banging https://learnopengl.com/
// https://www.glfw.org/docs/3.3/quick.html

void framebuffer_size_callback(GLFWwindow* window, int width, int height);

void framebuffer_size_callback(GLFWwindow* window, int width, int height)
{
    glViewport(0, 0, width, height);
}

void processInput(GLFWwindow *window)
{
    if (glfwGetKey(window, GLFW_KEY_ESCAPE) == GLFW_PRESS)
    {
        glfwSetWindowShouldClose(window, true);
    }
}

constexpr int window_width{800};
constexpr int window_height{600};

int main()
{
    printf("Starting the program\n");

    if (!glfwInit())
    {
        return -1;
    }

    // Set GLFW to 3.3
    glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3);
    glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 3);

    // Set profile (a compatibility thing)
    // Telling GLFW we want to use the core-profile means
    // we'll get access to a smaller subset of OpenGL features
    // without backwards-compatible features we no longer need
    glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE);

    GLFWwindow* window = glfwCreateWindow(window_width, window_height, "openGL is fun", NULL, NULL);
    if (window == nullptr)
    {
        std::cout << "Failed to create GLFW window" << std::endl;
        glfwTerminate();
        return -1;
    }

    glfwMakeContextCurrent(window);

    // Initialize GLAD before calling any openGL function
    // Load all OpenGL function pointers
    if (!gladLoadGLLoader((GLADloadproc)glfwGetProcAddress))
    {
        std::cout << "Failed to initialize GLAD" << std::endl;
        return -1;
    }

    // Set the viewport
    glViewport(0, 0, window_width, window_height);

    // Adjust the viewport if user resizes the window
    glfwSetFramebufferSizeCallback(window, framebuffer_size_callback);

    // Build and compile the shader program
    // TODO: use RES_PATH macro
    // TODO: move shaders to res
    Shader shader("vert.glsl", "frag.glsl");

    float vertices[] =
    {
        // positions                    // colors           // texture coords
        -1.0f,  -1.0f,  -1.0f, 1.0f,    0.0f, 0.0f, 0.0f,   1.0f, 1.0f,
        1.0f,   -1.0f,  -1.0f, 1.0f,    1.0f, 0.0f, 0.0f,   1.0, 0.0f,
        -1.0f,  1.0f,   -1.0f, 1.0f,    0.0f, 1.0f, 0.0f,   0.0f, 0.0f,
        1.0f,   1.0f,   -1.0f, 1.0f,    1.0f, 1.0f, 0.0f,   0.0f, 0.0f,

        -1.0f,  -1.0f,  1.0f, 1.0f,     0.0f, 0.0f, 1.0f,   1.0f, 1.0f,
        1.0f,   -1.0f,  1.0f, 1.0f,     1.0f, 0.0f, 1.0f,   1.0, 0.0f,
        -1.0f,  1.0f,   1.0f, 1.0f,     0.0f, 1.0f, 1.0f,   0.0f, 0.0f,
        1.0f,   1.0f,   1.0f, 1.0f,     1.0f, 1.0f, 1.0f,   0.0f, 0.0f
    };

    constexpr auto fovRad = M_PI * 0.5;
    constexpr auto near = 0.1;
    constexpr auto far = 10.0;
    const Mat4d perspective = perspectiveMatrix(fovRad, near, far);

    unsigned int texture;
    glGenTextures(1, &texture);
    glBindTexture(GL_TEXTURE_2D, texture);
    // set the texture wrapping/filtering options (on the currently bound texture object)
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);

    int width, height, nrChannels;
    unsigned char *imageData = stbi_load(RES_PATH("container.jpg"), &width, &height, &nrChannels, 0);
    if (imageData == nullptr)
    {
        printf("Image load failed\n");
        return 4;
    }
    glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, width, height, 0, GL_RGB, GL_UNSIGNED_BYTE, imageData);
    glGenerateMipmap(GL_TEXTURE_2D);
    stbi_image_free(imageData);

    unsigned int indices[] =
    {
        4, 7, 6, // front
        4, 5, 7, // front
        0, 6, 2, // left
        0, 4, 6, // left
        1, 3, 7, // right
        1, 7, 5, // right
        0, 2, 3, // back
        0, 3, 1, // back
        0, 1, 5, // bottom
        0, 5, 4, // bottom
        2, 7, 3, // top
        2, 6, 7 // top
    };

    // Reserve the buffer in GPU memory
    uint32_t VBO{0};
    uint32_t VAO{0};
    uint32_t EBO{0};
    glGenBuffers(1, &VBO);
    glGenVertexArrays(1, &VAO);
    glGenBuffers(1, &EBO);

    glBindVertexArray(VAO);

    // Copy the vertices to the array buffer
    glBindBuffer(GL_ARRAY_BUFFER, VBO);
    glBufferData(GL_ARRAY_BUFFER, sizeof(vertices), vertices, GL_STATIC_DRAW);

    // Copy our index array in a element buffer for OpenGL to use
    glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, EBO);
    glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(indices), indices, GL_STATIC_DRAW);

    // Tell OpenGL how to interpret vertex buffer data
    glVertexAttribPointer(0, 4, GL_FLOAT, GL_FALSE, 9 * sizeof(float), (void*)0);   // pos
    glEnableVertexAttribArray(0);
    glVertexAttribPointer(1, 3, GL_FLOAT, GL_FALSE, 9 * sizeof(float), (void*)(4 * sizeof(float))); // color
    glEnableVertexAttribArray(1);
    glVertexAttribPointer(2, 2, GL_FLOAT, GL_FALSE, 9 * sizeof(float), (void*)(7 * sizeof(float))); // tex
    glEnableVertexAttribArray(2);

    // note that this is allowed, the call to glVertexAttribPointer registered VBO
    // as the vertex attribute's bound vertex buffer object so afterwards we can safely unbind
    // glBindBuffer(GL_ARRAY_BUFFER, 0);

    // uncomment this call to draw in wireframe polygons.
    //glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);

    // Activate the shader program
    //glUseProgram(shaderProgram);
    shader.use();

    double t{0.0};
    constexpr auto fps{60.0};
    // Render loop
    while(!glfwWindowShouldClose(window))
    {
        processInput(window);

        glClearColor(0.2f, 0.3f, 0.3f, 1.0f);
        glEnable(GL_DEPTH_TEST);
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        float timeValue = glfwGetTime();
        float greenValue = 0.5f+0.5f*sin(timeValue*2.0f);
        shader.setFloat("pylly", greenValue);

        shader.setMat4f("perspective", perspective.cast<float>());

        // std::cout << camera << std::endl << std::endl;

        const Vec3d from{2.0, 1.0, 4.0};
        const Vec3d to{0.0, 0.0, 0.0};
        const Vec3d up{0.0, 1.0, 0.0};

        const Mat4d camera = cameraMatrix(from, to, up);
        shader.setMat4f("camera", camera.cast<float>());

        const Mat4d model = transformationMatrix(Vec3d(0.0, 0.0, 0.0), Vec3d(sin(t), 1.0 - sin(t), cos(t)));
        shader.setMat4f("model", model.cast<float>());

        // glDrawArrays(GL_TRIANGLES, 0, 36); // use this when not rendering from index buffer
        glDrawElements(GL_TRIANGLES, sizeof(indices) / sizeof(unsigned int), GL_UNSIGNED_INT, 0); // use this when rendering from index buffer

        glfwSwapBuffers(window);
        glfwPollEvents();

        t += 1.0 / fps;
    }

    // optional: de-allocate all resources once they've outlived their purpose:
    // ------------------------------------------------------------------------
    glDeleteVertexArrays(1, &VAO);
    glDeleteBuffers(1, &VBO);
    // glDeleteBuffers(1, &EBO);
    //glDeleteProgram(shaderProgram);
    shader.destroy();

    printf("Successfully exited the program!\n");

    glfwTerminate();
    return 0;
}


// -*- mode:c++;tab-width:2;indent-tabs-mode:t;show-trailing-whitespace:t;rm-trailing-spaces:t -*-
// vi: set ts=2 noet:
//
// (c) Copyright Rosetta Commons Member Institutions.
// (c) This file is part of the Rosetta software suite and is made available under license.
// (c) The Rosetta software is developed by the contributing members of the Rosetta Commons.
// (c) For more information, see http://www.rosettacommons.org. Questions about this can be
// (c) addressed to University of Washington UW TechTransfer, email: license@u.washington.edu.

#ifndef INCLUDED_riflib_types_hh
#define INCLUDED_riflib_types_hh

#include <scheme/types.hh>
#include <stdint.h>
#include <Eigen/Geometry>
#include <scheme/objective/voxel/VoxelArray.hh>

namespace devel {
namespace scheme {

	typedef ::Eigen::Transform<float,3,Eigen::AffineCompact> EigenXform;
	typedef ::scheme::objective::voxel::VoxelArray<3,float,float> VoxelArray;
	typedef VoxelArray* VoxelArrayPtr;
	using ::scheme::shared_ptr;
	using ::scheme::make_shared;
	using ::scheme::enable_shared_from_this;

}
}

#endif


/*
 *  Copyright (C) 2018 Naomasa Matsubayashi
 *  Licensed under MIT license, see file LICENSE in this source tree.
 */
#ifndef UWG_HMAC_HPP
#define UWG_HMAC_HPP

#include <type_traits>
#include <boost/container/static_vector.hpp>
#include <crypto++/blake2.h>
#include <uwg/is_continous_memory.hpp>

namespace uwg {
  class hmac {
  public:
    template< typename Key >
    hmac( const Key &key, std::enable_if_t< is_continous_memory_container_v< Key > >* = 0 ) {
      constexpr size_t key_length = 64u;
      boost::container::static_vector< unsigned char, key_length > ipad, key_;
      if( key.size() > key_length ) {
        hasher.Update( key.data(), key.size() );
        key_.resize( key_length, 0u );
        hasher.TruncatedFinal( key_.data(), key_.size() );
      }
      else key_.insert( key_.end(), key.begin(), key.end() );
      key_.resize( key_length, 0u );
      std::transform( key_.begin(), key_.end(), std::back_inserter( ipad ), []( unsigned char v ) { return 0x36 ^ v; } );
      std::transform( key_.begin(), key_.end(), std::back_inserter( opad ), []( unsigned char v ) { return 0x5c ^ v; } );
      std::fill( key_.begin(), key_.end(), 0u );
      hasher.Update( ipad.data(), ipad.size() );
      std::fill( ipad.begin(), ipad.end(), 0u );
    }
    template< typename In >
    auto update( const In &in ) ->
      std::enable_if_t< is_continous_memory_container_v< In >, hmac& > {
      hasher.Update( in.data(), in.size() );
      return *this;
    }
    template< typename Head, typename Next, typename ...Tail >
    auto update( const Head &head, const Next &next, const Tail&... tail ) ->
      std::enable_if_t< is_continous_memory_container_v< Head >, hmac& > {
      update( head );
      update( next, tail... );
      return *this;
    }
    hmac &update( const char *begin, const char *end ) {
      hasher.Update( reinterpret_cast< const unsigned char* >( begin ), std::distance( begin, end ) );
      return *this;
    }
    template< typename Out >
    auto get( Out &out ) ->
      std::enable_if_t< is_continous_memory_container_v< Out > > {
      constexpr size_t key_length = 64u;
      boost::container::static_vector< unsigned char, key_length > key_;
      key_.resize( hasher.DigestSize(), 0u );
      hasher.Final( key_.data() );
      hasher.Update( opad.data(), opad.size() );
      std::fill( opad.begin(), opad.end(), 0u );
      hasher.Update( key_.data(), key_.size() );
      std::fill( key_.begin(), key_.end(), 0u );
      out.resize( hasher.DigestSize(), 0u );
      hasher.TruncatedFinal( out.data(), out.size() );
    }
    template< typename Head, typename Next, typename ...Tail >
    auto get( Head &head, Next &next, Tail&... tail ) ->
      std::enable_if_t< is_continous_memory_container_v< Head > > {
      get( head );
      get( next, tail... );
    }
  private:
    CryptoPP::BLAKE2s hasher;
    boost::container::static_vector< unsigned char, 64u > opad;
  };
}

#endif


// CHAP - The Channel Annotation Package
//
// Copyright (c) 2016 - 2018 Gianni Klesse, Shanlin Rao, Mark S. P. Sansom, and
// Stephen J. Tucker
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.

#include <algorithm>
#include <iostream>
#include <functional>
#include <limits>
#include <ctime>

#include <boost/math/tools/minima.hpp>

#include <gromacs/pbcutil/pbc.h>
#include <gromacs/selection/nbsearch.h>
#include <gromacs/selection/selection.h>

#include "geometry/cubic_spline_interp_1D.hpp"
#include "geometry/cubic_spline_interp_3D.hpp"
#include "geometry/spline_curve_1D.hpp"
#include "geometry/spline_curve_3D.hpp"

#include "path-finding/molecular_path.hpp"

/*!
 * Constructor to generate a MolecularPath object from a set of centre line
 * points and corresponding radii.
 *
 * This uses CubicSplineInterp3D to perform cubic spline interpolation and
 * build a \f$ C^2 \f$-continuous curve connecting all given centre line
 * points. This uses the Euclidean distance between centre line points as
 * interpolation parameter.
 *
 * Subsequently, CubicSplineInterp1D is used to smoothly interpolate the radius
 * between the given support points. Consequently, the input vectors must have
 * the same number of elements. This uses the arc length distance between
 * centre line points as interpolation parameter.
 *
 * Finally, the centre-line curve is re-parameterised in terms of arc length
 * and the length of the path is computed as the arc length distance between
 * the first and last centre line points.
 */
MolecularPath::MolecularPath(std::vector<gmx::RVec> &pathPoints,
                             std::vector<real> &pathRadii)
{
    // assign internal containers for original path data:
    pathPoints_ = pathPoints;
    pathRadii_ = pathRadii;

    // construct centre line spline by interpolation of path points:
    CubicSplineInterp3D Interp3D;
    centreLine_ = Interp3D(pathPoints_, eSplineInterpBoundaryHermite);

    // get arc length at original control points:
    std::vector<real> arcLen = centreLine_.ctrlPointArcLength();
    openingLo_ = arcLen.front();
    openingHi_ = arcLen.back();
    length_ = std::abs(openingHi_ - openingLo_);

    // interpolate radius:
    CubicSplineInterp1D Interp1D;
    poreRadius_ =  Interp1D(arcLen, pathRadii_, eSplineInterpBoundaryHermite);

    // re-parameterise centre line spline by arc length:
    centreLine_.arcLengthParam();
}

/*!
 * Constructor for creating MolcularPath from a JSON document.
 */
MolecularPath::MolecularPath(
        const rapidjson::Document &doc)
    : centreLine_()
    , poreRadius_()
{
    // make sure document is valid object:
    if( !doc.IsObject() )
    {
        throw std::logic_error("JSON document passed to MolecularPath"
        "constructor is not a valid JSON object.");
    }

    // make sure radius spline data is given:
    if( !doc.HasMember("molPathRadiusSpline") )
    {
        throw std::logic_error("JSON document passed to MolecularPath"
        "constructor does not have required member molPathRadiusSpline.");
    }
    if( !doc["molPathRadiusSpline"].HasMember("knots") )
    {
        throw std::logic_error("Could not find knots in molPathRadiusSpline.");
    }
    if( !doc["molPathRadiusSpline"].HasMember("ctrl") )
    {
        throw std::logic_error("Could not find ctrl in molPathRadiusSpline.");
    }

    // make sure centre line spline data is given:
    if( !doc.HasMember("molPathCentreLineSpline") )
    {
        throw std::logic_error("JSON document passed to MolecularPath"
        "constructor does not have required member molPathCentreLineSpline.");
    }
    if( !doc["molPathCentreLineSpline"].HasMember("knots") )
    {
        throw std::logic_error("Could not find knots in molPathCentreLineSpline.");
    }
    if( !doc["molPathCentreLineSpline"].HasMember("ctrlX") )
    {
        throw std::logic_error("Could not find ctrlX in molPathCentreLineSpline.");
    }
    if( !doc["molPathCentreLineSpline"].HasMember("ctrlY") )
    {
        throw std::logic_error("Could not find ctrlY in molPathCentreLineSpline.");
    }
    if( !doc["molPathCentreLineSpline"].HasMember("ctrlZ") )
    {
        throw std::logic_error("Could not find ctrlZ in malPathCentreLineSpline.");
    }

    // make sure original points are present:
    if( !doc.HasMember("molPathOrigPoints") )
    {
        throw std::logic_error("JSON document passed to MolecularPath"
        "constructor does not have required member molPathOrigPoints.");
    }
    if( !doc["molPathOrigPoints"].HasMember("x") )
    {
        throw std::logic_error("Could not find x in molPathRadiusSpline.");
    }
    if( !doc["molPathOrigPoints"].HasMember("y") )
    {
        throw std::logic_error("Could not find y in molPathRadiusSpline.");
    }
    if( !doc["molPathOrigPoints"].HasMember("z") )
    {
        throw std::logic_error("Could not find z in molPathRadiusSpline.");
    }
    if( !doc["molPathOrigPoints"].HasMember("r") )
    {
        throw std::logic_error("Could not find r in molPathRadiusSpline.");
    }

    // extract original point and radius data from JSON:
    for(size_t i = 0; i < doc["molPathOrigPoints"]["r"].Size(); i++)
    {
         pathPoints_.push_back(
                gmx::RVec(doc["molPathOrigPoints"]["x"][i].GetDouble(),
                          doc["molPathOrigPoints"]["y"][i].GetDouble(),
                          doc["molPathOrigPoints"]["z"][i].GetDouble()));
         pathRadii_.push_back(doc["molPathOrigPoints"]["r"][i].GetDouble());
    }

    // extract pore radius spline from data:
    std::vector<real> poreRadiusKnots;
    std::vector<real> poreRadiusCtrlPoints;
    for(size_t i = 0; i < doc["molPathRadiusSpline"]["knots"].Size(); i++)
    {
        poreRadiusKnots.push_back(
                doc["molPathRadiusSpline"]["knots"][i].GetDouble() );
        poreRadiusCtrlPoints.push_back(
                doc["molPathRadiusSpline"]["ctrl"][i].GetDouble() );
    }

    // add duplicate knots at endpoints:
    int poreRadiusSplineDegree = 3; // TODO: should not be hardcoded
    poreRadiusKnots.insert(
            poreRadiusKnots.end(),
            poreRadiusSplineDegree - 1,
            poreRadiusKnots.back());
    poreRadiusKnots.insert(
            poreRadiusKnots.begin(),
            poreRadiusSplineDegree - 1,
            poreRadiusKnots.front());

    // create radius spline curve:
    poreRadius_ = SplineCurve1D(
            poreRadiusSplineDegree,
            poreRadiusKnots,
            poreRadiusCtrlPoints);

    // extract centre line spline from data:
    std::vector<real> centreLineKnots;
    std::vector<gmx::RVec> centreLineCtrlPoints;
    for(size_t i = 0; i < doc["molPathCentreLineSpline"]["knots"].Size(); i++)
    {
        centreLineKnots.push_back(
                doc["molPathCentreLineSpline"]["knots"][i].GetDouble() );
        centreLineCtrlPoints.push_back(
                gmx::RVec(doc["molPathCentreLineSpline"]["ctrlX"][i].GetDouble(),
                          doc["molPathCentreLineSpline"]["ctrlY"][i].GetDouble(),
                          doc["molPathCentreLineSpline"]["ctrlZ"][i].GetDouble()));
    }

    // add duplicate knots at endpoints:
    int centreLineSplineDegree = 3; // TODO: should not be hardcoded
    centreLineKnots.insert(
            centreLineKnots.end(),
            centreLineSplineDegree - 1,
            centreLineKnots.back());
    centreLineKnots.insert(
            centreLineKnots.begin(),
            centreLineSplineDegree - 1,
            centreLineKnots.front());

    // create centre line spline curve:
    centreLine_ = SplineCurve3D(
            centreLineSplineDegree,
            centreLineKnots,
            centreLineCtrlPoints);

    // set position of openings and pore length:
    openingLo_ = poreRadiusKnots.front();
    openingHi_ = poreRadiusKnots.back();
    length_ = openingHi_ - openingLo_;

    // sanity check:
    if( openingLo_ > openingHi_ )
    {
        throw std::logic_error("Pore opening coordinates out of order.");
    }
}

/*!
 * Destructor.
 */
MolecularPath::~MolecularPath()
{

}

/*!
 * Function for mapping a set of Cartesian positions onto the centre line
 * spline curve.
 *
 * The return value is a vector of points in spline coordinates ordered in the
 * same way as the input vector. Internally, this uses mapPosition() for each
 * input position.
 */
std::vector<gmx::RVec>
MolecularPath::mapPositions(const std::vector<gmx::RVec> &positions)
{
    // map all input positions onto centre line:
    std::vector<gmx::RVec> mappedPositions;
    mappedPositions.reserve(positions.size());
    for(auto pos : positions)
    {
        mappedPositions.push_back(centreLine_.cartesianToCurvilinear(pos));
    }

    // return mapped positions:
    return mappedPositions;
}

/*!
 * Maps all positions in a selection onto molecular pathway.
 *
 * This function does essentially the same as mapPositions, except that the
 * input may be provided as a selection of particles and the output will be
 * a map associating each refId in the selection with a set of curvilinear
 * coordinates.
 */
std::map<int, gmx::RVec>
MolecularPath::mapSelection(const gmx::Selection &mapSel)
{
    // build map of pathway mapped coordinates:
    std::map<int, gmx::RVec> mappedCoords;
    for(int i = 0; i < mapSel.posCount(); i++)
    {
        unsigned int idx = mapSel.position(i).refId();
        mappedCoords[idx] = centreLine_.cartesianToCurvilinear(
                mapSel.position(i).x());
    }

    // return mapped coordinates:
    return mappedCoords;
}

/*!
 * Checks if points described by a set of mapped coordinates lie within the
 * MolecularPath.
 *
 * The input is taken to be a map of points in centre line coordinates, i.e.
 * a tuple of \f$ (s_i, \rho_i, \phi_i) \f$ values for the \f$ i \f$ -th point,
 * where \f$ s \f$ is the distance along the centre line, \f$ \rho \f$ is the
 * (orthogonal) distance from the centre line, and \f$ \phi \f$ an angular
 * coordinate which is ignored in this function. To obtain a set of mapped
 * points the mapSelection() method can be used.
 *
 * The output is a map of booleans indicating whether or not a point lies
 * inside the MolecularPath. The integer key is the same as in the input map
 * and will usually correspond to the ID of a particle.
 *
 * To test whether a point lies within the MolecularPath, its distance from
 * the centre line is compared to to path radius at this point plus a margin,
 * i.e.
 *
 * \f[
 *      \gamma_i = \left\{
 *                 \begin{array}{ll}
 *                     1 & \text{ if } \rho_i < R(s_i) + m \\
 *                     0 & \text{ otherwise }
 *                 \end{array}
 *                 \right.
 * \f]
 *
 * with \f$ \gamma_i \f$ being a binary indicator function.
 */
std::map<int, bool>
MolecularPath::checkIfInside(const std::map<int, gmx::RVec> &mappedCoords,
                             real margin)
{
    // create map for check results:
    std::map<int, bool> isInside;

    for(auto it = mappedCoords.begin(); it != mappedCoords.end(); it++)
    {
        real evalPoint = it -> second[0];
        real thres = (poreRadius_.evaluate(evalPoint, 0)) + margin;
        // threshold needs to be squared here because radial coordinate is!
        isInside[it -> first] = (it -> second[1] < thres*thres);
    }

    // return assessment:
    return isInside;
}

/*!
 * Checks if a given set of coordinates lies within the pathway.
 */
std::map<int, bool>
MolecularPath::checkIfInside(const std::map<int, gmx::RVec> &mappedCoords,
                             real margin,
                             real sLo,
                             real sHi)
{
    // first make decision based on margin:
    std::map<int, bool> isInside = checkIfInside(mappedCoords, margin);

    // now erase all elements that do not fall in given range:
    for(auto it = isInside.begin(); it != isInside.end(); it++)
    {
        // obtain mapped s value:
        real s = mappedCoords.at(it->first)[0];

        // does it fall in target interval?
        if( s < sLo || s > sHi )
        {
            it -> second = false;
        }
    }

    // return resulting map:
    return isInside;
}

/*!
 * Adds a scalar property to the MolecularPath. Note that property names must
 * be unique and already existing properties will be overwritten.
 */
void
MolecularPath::addScalarProperty(
        std::string name,
        SplineCurve1D property,
        bool divergent)
{
    properties_[name] = std::pair<SplineCurve1D, bool>(property, divergent);
}

/*!
 * Returns map of scalar properties associated with the MolecularPath.
 */
std::map<std::string, std::pair<SplineCurve1D, bool>>
MolecularPath::scalarProperties() const
{
    return properties_;
}

/*!
 * Simple getter function for access to original path points used to construct
 * the path.
 */
std::vector<gmx::RVec>
MolecularPath::pathPoints()
{
    return pathPoints_;
}

/*!
 * Simple getter function for access to original radii used to construct the
 * path.
 */
std::vector<real>
MolecularPath::pathRadii()
{
    return pathRadii_;
}

/*!
 * Returns a copy of the internal pore radius spline.
 */
SplineCurve1D
MolecularPath::pathRadius()
{
    return poreRadius_;
}

/*!
 * Returns a copy of the internal centre line spline.
 */
SplineCurve3D
MolecularPath::centreLine()
{
    return centreLine_;
}

/*!
 * Returns length of the pathway, defined as the the arc length distance
 * between the first and last control point
 */
real
MolecularPath::length() const
{
    return length_;
}

/*!
 * Returns the pore radius \f$ R(s) \f$ at a given value of the centre line's
 * spline parameter \f$ s \f$.
 */
real
MolecularPath::radius(real s)
{
    return poreRadius_.evaluate(s, 0);
}

/*!
 * Returns coordinates of the lower opening of the pore.
 */
real
MolecularPath::sLo()
{
    return openingLo_;
}

/*!
 * Returns the coordinates of the upper opening of the pore.
 */
real
MolecularPath::sHi()
{
    return openingHi_;
}

/*!
 * Getter method for access to the radius spline's knot vector. This returns
 * the complete knot vector including duplicate points at the ends.
 */
std::vector<real>
MolecularPath::poreRadiusKnots() const
{
    return poreRadius_.knotVector();
}

/*!
 * Getter method for access to the radius spline's knot vector. This does strip
 * the knot vector of repeated knots at end points so that the resulting vector
 * has as many elements as the vector of control points.
 */
std::vector<real>
MolecularPath::poreRadiusUniqueKnots() const
{
    std::vector<real> allKnots = poreRadius_.knotVector();
    std::vector<real> uniqueKnots(
        allKnots.begin() + poreRadius_.degree() - 1,
        allKnots.end() - poreRadius_.degree() + 1);
    return uniqueKnots;
}

/*!
 * Getter method for access to the radius spline's control points.
 */
std::vector<real>
MolecularPath::poreRadiusCtrlPoints() const
{
    return poreRadius_.ctrlPoints();
}

/*!
 * Getter method for access to the centre line spline's knot vector.
 * This returns  the complete knot vector including duplicate points at the
 * ends.
 */
std::vector<real>
MolecularPath::centreLineKnots() const
{
    return centreLine_.knotVector();
}

/*!
 * Getter method for access to the centre line spline's knot vector. This does
 * strip the knot vector of repeated knots at end points so that the resulting
 * vector has as many elements as the vector of control points.
 */
std::vector<real>
MolecularPath::centreLineUniqueKnots() const
{
    std::vector<real> allKnots = centreLine_.knotVector();
    std::vector<real> uniqueKnots(
            allKnots.begin() + centreLine_.degree() - 1,
            allKnots.end() - centreLine_.degree() + 1);
    return uniqueKnots;
}

/*!
 * Getter method for access to the centre line spline's control points.
 */
std::vector<gmx::RVec>
MolecularPath::centreLineCtrlPoints() const
{
    return centreLine_.ctrlPoints();
}

/*!
 * Finds the minimum radius of the path and the location along the centre line
 * (in the current parameterisation) of this minimum.
 *
 * This is achieved by first sampling a set of trial points no more than 0.1
 * nm apart along the centreline and evaluating the path radius at all of these
 * points. The position of the minimum radius sample point is then used to
 * create input to Brent's minimisation algorithm. The initial bracketing
 * interval for Brent's algorithm is taken as one sampling point above and
 * below the minimum radius sampling point, if this point lies somewhere in
 * between the the centre line's endpoints. If the minimum radius sampling
 * point falls on either endpoint of the centeline, this point itself is taken
 * as one of the bracketing interval's limits.
 *
 * Note that Brent's algorithm is currently limited to a hardcoded limit of
 * 100 iterations.
 */
std::pair<real, real>
MolecularPath::minRadius()
{
    // internal parameters:
    real maxSampleDist = 0.1;
    boost::uintmax_t maxIter = 100;

    // draw radius samples along path:
    int nSamples = std::ceil(length_/maxSampleDist);
    std::vector<real> s = sampleArcLength(nSamples, 0.0);
    std::vector<real> r = sampleRadii(s);

    // find smallest sample radius:
    auto itMin = std::min_element(r.begin(), r.end());
    int idxMin = std::distance(r.begin(), itMin);

    // determine bracketing interval:
    real sMin = s[idxMin - 1];
    real sMax = s[idxMin + 1];
    if( itMin == r.begin() )
    {
        sMin = s[idxMin];
        sMax = s[idxMin + 1];
    }
    else if( itMin == r.end() )
    {
        sMin = s[idxMin - 1];
        sMax = s[idxMin];
    }

    // return minimum and arg min:
    return boost::math::tools::brent_find_minima(
            std::bind(&MolecularPath::radius, this, std::placeholders::_1),
            sMin,
            sMax,
            std::numeric_limits<real>::digits,
            maxIter);
}

/*!
 *  Returns the volume of the path defined as the volume of the spline tube
 *  between the upper and lower opening of the path, i.e.
 *
 *  \f[
 *
 *      V = \pi \int_{s_0}^{s_1} \left( R(s) \right)^2 ds
 *
 *  \f]
 *
 *  where \f$ R(s) \f$ denotes the radius at a given point along the spline.
 *
 *  This integral is solved numerically by applying a sixth order Newton-Cotes
 *  scheme (Weddle's rule) in each interval between two subsequent knots. Since
 *  the path radius is \f$ \mathcal{O}(s^3) \f$ by construction, the integrand
 *  is guaranteed to be of order \f$ \mathcal{O}(s^6) \f$ so the integration
 *  is exact.
 *
 *  The volume is nonetheless an estimate due to (i) the cross-sectional area
 *  of a path not being truly circular and (ii) the dependency of radius on
 *  centre line parameter not being \f$ \mathcal{O}(s^3) \f$ necessarily. The
 *  former effect is likely stronger so that the volume estimate should be
 *  viewed as a lower bound.
 */
real
MolecularPath::volume()
{
    // initialise number of intervals larger than number of spline intervals:
    // (this ensures that function is polynomial on each interval)
    size_t numIntervals = (poreRadius_.nKnots()) - 2*poreRadius_.degree() - 1;

    // integration interval:
    real h = length_/numIntervals;

    // sample path radii:
    int numPoints = 6*numIntervals + 1;
    std::vector<real> radii = sampleRadii(numPoints, 0.0);
    std::vector<real> s = sampleArcLength(numPoints, 0.0);

    // calculate squared radii:
    std::vector<real> sqRadii;
    sqRadii.reserve(radii.size());
    for(size_t i = 0; i < radii.size(); i++)
    {
        sqRadii.push_back(radii[i]*radii[i]);
    }

    // evaluate integral using fine grained support points:
    real integral = 0.0;
    for(size_t i = 0; i < numIntervals; i++)
    {
        // index to vector of radii:
        int idx = 6*i;

        // integrate this interval using Weddle's Rule:
        integral +=  41.0*sqRadii[idx] +
                    216.0*sqRadii[idx+1] +
                     27.0*sqRadii[idx+2] +
                    272.0*sqRadii[idx+3] +
                     27.0*sqRadii[idx+4] +
                    216.0*sqRadii[idx+5] +
                     41.0*sqRadii[idx+6];
    }
    integral *= h/840.0;

    // multiply in constant factor:
    integral *= PI_;

    // return value of integral:
    return integral;
}

/*!
 * Returns a vector of equally spaced arc length points that extends a
 * specified distance beyond the openings of the pore.
 */
std::vector<real>
MolecularPath::sampleArcLength(size_t nPoints,
                               real extrapDist) const
{
    // get spacing of points in arc length:
    real arcLenStep = sampleArcLenStep(nPoints, extrapDist);

    // evaluate spline to obtain sample points:
    std::vector<real> arcLengthSample;
    arcLengthSample.reserve(nPoints);
    for(size_t i = 0; i < nPoints; i++)
    {
        // calculate evaluation point:
        arcLengthSample.push_back( openingLo_ - extrapDist + i*arcLenStep );
    }

    // return vector of points:
    return arcLengthSample;
}

/*!
 * Returns a vector of point on the molecular path's centre line. The points
 * will be equally spaced in arc length and sampling will extend beyond the
 * pore openings for the specified distance. All points are given in
 * Cartesian coordinates.
 */
std::vector<gmx::RVec>
MolecularPath::samplePoints(size_t nPoints,
                            real extrapDist)
{
    // sample equidistant arc length values:
    std::vector<real> arcLengthSteps = sampleArcLength(nPoints, extrapDist);

    // return vector of points at these values:
    return samplePoints(arcLengthSteps);
}

/*!
 * Returns a vector of points on the path's centre line at the given arc length
 * parameter values. All points are given in Cartesian coordinates.
 */
std::vector<gmx::RVec>
MolecularPath::samplePoints(std::vector<real> arcLengthSample)
{
    // evaluate spline to obtain sample points:
    std::vector<gmx::RVec> points;
    points.reserve(arcLengthSample.size());
    for(size_t i = 0; i < arcLengthSample.size(); i++)
    {
        // evaluate spline at this point:
        points.push_back( centreLine_.evaluate(arcLengthSample[i], 0) );
    }

    // return vector of points:
    return points;
}

/*!
 * Returns vector of \p nPoints tangents to the centre line. The samples are
 * taken from equidistant points along the spline, extending \p extrapDist into
 * the extrapolation range on either side.
 */
std::vector<gmx::RVec>
MolecularPath::sampleTangents(size_t nPoints, real extrapDist)
{
    // sample equidistant arc length values:
    std::vector<real> arcLengthSteps = sampleArcLength(nPoints, extrapDist);

    // return vector of tangents at these values:
    return sampleTangents(arcLengthSteps);
}

/*!
 * Returns vector of tangents to the centre line. These are calculated at the
 * evaluation points given on \p arcLengthSample.
 */
std::vector<gmx::RVec>
MolecularPath::sampleTangents(std::vector<real> arcLengthSample)
{
    // evaluate spline to obtain sample points:
    std::vector<gmx::RVec> tangents;
    tangents.reserve(arcLengthSample.size());
    for(size_t i = 0; i < arcLengthSample.size(); i++)
    {
        // evaluate spline at this point:
        tangents.push_back( centreLine_.tangentVec(arcLengthSample[i]) );
    }

    // return vector of points:
    return tangents;
}

/*!
 * Returns vector of \p nPoints tangents to the centre line. The samples are
 * taken from equidistant points along the spline, extending \p extrapDist into
 * the extrapolation range on either side. All tangents are explicitly
 * normalised in this function.
 */

std::vector<gmx::RVec>
MolecularPath::sampleNormTangents(size_t nPoints, real extrapDist)
{
    // sample equidistant arc length values:
    std::vector<real> arcLengthSteps = sampleArcLength(nPoints, extrapDist);

    // return vector of tangents at these values:
    std::vector<gmx::RVec> tangents = sampleTangents(arcLengthSteps);

    // normalise all tangent vectors:
    std::vector<gmx::RVec>::iterator it;
    for(it = tangents.begin(); it != tangents.end(); it++)
    {
        unitv(*it, *it);
    }

    return tangents;
}

/*!
 * Returns vector of tangents to the centre line. These are calculate at the
 * evaluation points given on \p arcLengthSample. All tangents are explicitly
 * normalised in this function.
 */
std::vector<gmx::RVec>
MolecularPath::sampleNormTangents(std::vector<real> arcLengthSample)
{
    // evaluate spline to obtain sample points:
    std::vector<gmx::RVec> tangents;
    tangents.reserve(arcLengthSample.size());
    for(size_t i = 0; i < arcLengthSample.size(); i++)
    {
        // evaluate spline at this point:
        tangents.push_back( centreLine_.tangentVec(arcLengthSample[i]) );

        // normalise tangent vector:
        unitv(tangents.back(), tangents.back());
    }

    // return vector of points:
    return tangents;
}

/*!
 * \todo This needs to be implemented.
 */
std::vector<gmx::RVec>
MolecularPath::sampleNormals(size_t nPoints, real extrapDist)
{
    // sample equidistant arc length values:
    std::vector<real> arcLengthSteps = sampleArcLength(nPoints, extrapDist);

    // return vector of normals at these values:
    return sampleNormals(arcLengthSteps);
}

/*!
 * \todo This needs to be implemented.
 */
std::vector<gmx::RVec>
MolecularPath::sampleNormals(std::vector<real> /* arcLengthSample */)
{
    // evaluate spline to obtain sample points:
    std::vector<gmx::RVec> normals;

    // return vector of points:
    return normals;
}

/*!
 * Returns a vector of radius values at equally spaced points a long the path.
 * Sampling extends the specified distance beyond the openings of the pore.
 */
std::vector<real>
MolecularPath::sampleRadii(size_t nPoints,
                           real extrapDist)
{
    // get spacing of points in arc length:
    real arcLenStep = sampleArcLenStep(nPoints, extrapDist);

    // evaluate spline to obtain sample points:
    std::vector<real> radii;
    for(size_t i = 0; i < nPoints; i++)
    {
        // calculate evaluation point:
        real evalPoint = openingLo_ - extrapDist + i*arcLenStep;

        // evaluate spline at this point:
        radii.push_back( poreRadius_.evaluate(evalPoint, 0) );
    }

    // return vector of points:
    return radii;
}

/*!
 * Returns a vector of radius values at the given arc length parameter values.
 */
std::vector<real>
MolecularPath::sampleRadii(std::vector<real> arcLengthSample)
{
    // evaluate spline to obtain sample points:
    std::vector<real> radii;
    for(size_t i = 0; i < arcLengthSample.size(); i++)
    {
        // evaluate spline at this point:
        radii.push_back( poreRadius_.evaluate(arcLengthSample[i], 0) );
    }

    // return vector of points:
    return radii;
}

/*!
 * Shift the s-coordinate by the given number.
 */
void
MolecularPath::shift(const gmx::RVec &shift)
{
    // shift both the centre line and the radius spline:
    centreLine_.shift(shift);
    poreRadius_.shift(shift);

    // adjust convenience variables defined in MolecularPath itself:
    openingLo_ -= shift[SS];
    openingHi_ -= shift[SS];
}

/*!
 * Auxiliary function that computes the step length along the arc for sampling
 * a given property at \f$ N \f$ points and reaching \f$ d \f$ into the
 * extrapolation region at either side of the pore. The step length is computed
 * as
 *
 * \f[
 *      \Delta s = \frac{L + 2 d}{N - 1}
 * \f]
 *
 * where \f$ L \f$ denotes the length of the path between its first and last
 * control points.
 */
real
MolecularPath::sampleArcLenStep(size_t nPoints, real extrapDist) const
{
    // get spacing of points in arc length:
    return (this -> length() + 2.0*extrapDist)/(nPoints - 1);
}


// Copyright (c) 2014 The Bitcoin Core developers
// Copyright (c) 2014-2015 The Dash developers
// Copyright (c) 2015-2017 The PIVX developers
// Copyright (c) 2018      The MTNC developers
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "primitives/transaction.h"
#include "main.h"

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(main_tests)

void CheckBlockValue(int nHeight, CAmount nSubsidy, CAmount nExpectedValue) {
    int nTail = nHeight - nHeight / 21600 * 21600;
    if (nHeight > Params().LAST_POW_BLOCK() && nTail >= 0 && nTail < 15) {
        BOOST_CHECK(nSubsidy == 1000 * COIN);
    } else {
        BOOST_CHECK(nSubsidy == nExpectedValue);
    }
}

BOOST_AUTO_TEST_CASE(subsidy_limit_test)
{
    CAmount nSum = 0;
    for (int nHeight = 0; nHeight < 1; nHeight += 1) {
        /* premine in block 1 (700,000 MTNC) */
        CAmount nSubsidy = GetBlockValue(nHeight);
        CheckBlockValue(nHeight, nSubsidy, 7000000 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = 1; nHeight < 2000; nHeight += 1) {
        /* premine in block 1 (700,000 MTNC) */
        CAmount nSubsidy = GetBlockValue(nHeight);
        CheckBlockValue(nHeight, nSubsidy, 1 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = 2000; nHeight < 300000; nHeight += 1) {
        /* PoW Phase and POS Phase One */
        CAmount nSubsidy = GetBlockValue(nHeight);
        CheckBlockValue(nHeight, nSubsidy, 100 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = 300000; nHeight < 1000000; nHeight += 1) {
        /* PoS Phase Two */
        CAmount nSubsidy = GetBlockValue(nHeight);
        CheckBlockValue(nHeight, nSubsidy, 50 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = 1000000; nHeight < 2000000; nHeight += 1) {
        /* POS Phase Three */
        CAmount nSubsidy = GetBlockValue(nHeight);
        CheckBlockValue(nHeight, nSubsidy, 25 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = 2000000; nHeight < 3000000; nHeight += 1) {
        /* POS Phase Four */
        CAmount nSubsidy = GetBlockValue(nHeight);
        CheckBlockValue(nHeight, nSubsidy, 12.5 * COIN);
        nSum += nSubsidy;
    }

    // nHeight >= 3000000, nSubsidy=6.25
    BOOST_CHECK(nSum == 11128837400000000ULL);
}

BOOST_AUTO_TEST_SUITE_END()