// Copyright (c) 2018 Graphcore Ltd. All rights reserved.

// Legacy code to test some BroadcastVectorInner vertices.
// This code is no longer used.

#define BOOST_TEST_MODULE AddToChannel2d

#include "../lib/popops/ExprOpUtil.hpp"
#include <boost/multi_array.hpp>
#include <functional>
#include <iostream>
#include <limits>
#include <poplar/Engine.hpp>
#include <poplibs_support/TestDevice.hpp>
#include <poplibs_test/Util.hpp>
#include <popops/codelets.hpp>
#include <poputil/TileMapping.hpp>
#include <poputil/VertexTemplates.hpp>

// Tolerances used in tests
#define FLOAT_REL_TOL 0.01
#define HALF_REL_TOL 0.1
#define FLOAT_ABS_TOL 1e-6
#define HALF_ABS_TOL 1e-5

using namespace poplar;
using namespace poplar::program;
using namespace poputil;
using namespace poplibs_test::util;
using namespace poplibs_support;

const OptionFlags options;

struct TestCase {
  poplar::Type type;
  std::vector<std::size_t> addendLen;
  std::vector<std::size_t> actsLen;
  bool subtract;
};

struct TestCaseData {
  // These are all the addends/acts concatenated together.
  std::vector<double> allAddends;
  std::vector<double> allActs;

  std::unique_ptr<char[]> rawAllAddends;
  std::unique_ptr<char[]> rawAllActs;
};

static bool addToChannel2DTests(const std::vector<TestCase> &cases) {

  const std::size_t overwriteLen = 32;

  auto device = createTestDevice(TEST_TARGET, 1, 1);
  const auto &target = device.getTarget();
  Graph graph(target);
  popops::addCodelets(graph);

  // One compute set, with a vertex for each test case.
  auto cs = graph.addComputeSet("cs");

  std::vector<std::pair<std::string, char *>> tmap;
  Sequence uploadProg, downloadProg;
  std::vector<TestCaseData> tcData(cases.size());

  for (std::size_t i = 0; i < cases.size(); ++i) {
    const auto &tc = cases[i];

    // Check the actsLen's are ok.
    if (tc.actsLen.size() != tc.addendLen.size())
      return false;

    for (unsigned a = 0; a < tc.actsLen.size(); ++a) {
      if (tc.actsLen[a] % tc.addendLen[a] != 0) {
        return false;
      }
    }

    std::size_t totalAddendLen =
        std::accumulate(tc.addendLen.begin(), tc.addendLen.end(), 0);
    std::size_t totalActsLen =
        std::accumulate(tc.actsLen.begin(), tc.actsLen.end(), 0);

    std::cout << "Test case [" << i << "]: "
              << " addendLen.size(): " << tc.addendLen.size()
              << " actsLen.size(): " << tc.actsLen.size()
              << " totalAddendLen: " << totalAddendLen
              << " totalActsLen: " << totalActsLen
              << " subtract: " << tc.subtract << " type: " << tc.type.toString()
              << "\n";

    std::string suffix = "_" + std::to_string(i);

    auto allAddends =
        graph.addVariable(tc.type, {totalAddendLen}, "allAddends" + suffix);
    graph.setTileMapping(allAddends, 0);
    auto allActs = graph.addVariable(tc.type, {totalActsLen + overwriteLen},
                                     "allActs" + suffix);
    graph.setTileMapping(allActs, 0);

    popops::expr::BinaryOpType op = tc.subtract
                                        ? popops::expr::BinaryOpType::SUBTRACT
                                        : popops::expr::BinaryOpType::ADD;
    auto templateVertexName =
        templateVertex("popops::BroadcastVectorInner2DInPlace", op, tc.type);
    auto v = graph.addVertex(cs, templateVertexName);

    // Connect the acts and addend subvectors.
    graph.setFieldSize(v["data"], tc.actsLen.size());
    graph.setFieldSize(v["B"], tc.actsLen.size());

    std::size_t actsPos = 0;
    std::size_t addendPos = 0;
    std::vector<unsigned> workList;
    workList.push_back(tc.actsLen.size() - 1);
    for (unsigned a = 0; a < tc.actsLen.size(); ++a) {
      graph.connect(v["data"][a],
                    allActs.slice(actsPos, actsPos + tc.actsLen[a]));
      graph.connect(v["B"][a],
                    allAddends.slice(addendPos, addendPos + tc.addendLen[a]));

      actsPos += tc.actsLen[a];
      addendPos += tc.addendLen[a];

      auto actsBlockCount = tc.actsLen[a] / tc.addendLen[a];
      uint16_t actsBlockCount16 = actsBlockCount;

      if (actsBlockCount16 != actsBlockCount)
        return false;
      workList.push_back(tc.scaleLen[a]);
      workList.push_back(actsBlockCount16);
    }

    graph.setTileMapping(v, 0);
    auto workListTensor =
        graph.addConstant(UNSIGNED_SHORT, {workList.size()}, workList.data());
    graph.setTileMapping(workListTensor, 0);
    graph.connect(v["workList"], workListTensor);
    tcData[i].rawAllAddends =
        allocateHostMemoryForTensor(allAddends, "allAddend" + suffix, graph,
                                    uploadProg, downloadProg, tmap);
    tcData[i].rawAllActs = allocateHostMemoryForTensor(
        allActs, "allActs" + suffix, graph, uploadProg, downloadProg, tmap);

    tcData[i].allAddends.resize(totalAddendLen);
    tcData[i].allActs.resize(totalActsLen + overwriteLen);

    std::mt19937 randomEngine;
    writeRandomValues(target, allAddends.elementType(),
                      tcData[i].allAddends.data(),
                      tcData[i].allAddends.data() + tcData[i].allAddends.size(),
                      -2., 2., randomEngine);
    writeRandomValues(target, allActs.elementType(), tcData[i].allActs.data(),
                      tcData[i].allActs.data() + tcData[i].allActs.size(), -2.,
                      2., randomEngine);

    copy(target, tcData[i].allAddends.data(), tcData[i].allAddends.size(),
         allAddends.elementType(), tcData[i].rawAllAddends.get());
    copy(target, tcData[i].allActs.data(), tcData[i].allActs.size(),
         allActs.elementType(), tcData[i].rawAllActs.get());
  }

  std::cout << "Executing engine\n";

  auto prog = Execute(cs);
  Engine engine(graph, Sequence(uploadProg, prog, downloadProg), options);

  attachStreams(engine, tmap);

  device.bind([&](const Device &d) {
    engine.load(d);
    engine.run(0);
  });

  std::cout << "Checking results\n";

  // Check the results for each test case.

  for (std::size_t i = 0; i < tcData.size(); ++i) {
    const auto &tc = cases[i];

    std::cout << "Checking case [" << i << "]\n";

    // Convert back to double.
    std::vector<double> allActsOut(tcData[i].allActs.size(), 0.0);

    copy(target, tc.type, tcData[i].rawAllActs.get(), allActsOut.data(),
         allActsOut.size());

    // Calculate the correct answer.
    auto allActsRef = tcData[i].allActs;

    std::size_t actsPos = 0;
    std::size_t addendPos = 0;
    for (unsigned a = 0; a < tc.actsLen.size(); ++a) {
      for (std::size_t k = 0; k < tc.actsLen[a]; ++k) {
        allActsRef[actsPos + k] +=
            tcData[i].allAddends[addendPos + (k % tc.addendLen[a])] *
            (tc.subtract ? -1 : 1);
      }
      actsPos += tc.actsLen[a];
      addendPos += tc.addendLen[a];
    }

    const double absoluteTolerance =
        tc.type == FLOAT ? FLOAT_ABS_TOL : HALF_ABS_TOL;
    const double relativeTolerance =
        tc.type == FLOAT ? FLOAT_REL_TOL : HALF_REL_TOL;

    auto matchesModel = checkIsClose(
        "out", allActsOut.data(), {allActsOut.size()}, allActsRef.data(),
        allActsOut.size(), relativeTolerance, absoluteTolerance);
    if (!matchesModel)
      return false;
  }

  return true;
}

void runAddToChannel2DTests(std::vector<TestCase> cases) {
  // Test ScaledAddToChannel
  BOOST_TEST(addToChannel2DTests(cases));

  // Test AddToChannel
  for (auto &tc : cases)
    tc.subtract = false;
  BOOST_TEST(addToChannel2DTests(cases));
}

const bool isNotSim = !isSimulator(TEST_TARGET);

BOOST_AUTO_TEST_CASE(AddToChannel2DTiny) {
  std::vector<TestCase> cases = {
      {HALF, {1, 4, 8, 5}, {15, 12, 32, 15}, true},
      {HALF, {1, 4, 8, 5}, {15, 12, 32, 15}, false},
      {FLOAT, {1, 4, 8, 5}, {15, 12, 32, 15}, true},
      {FLOAT, {1, 4, 8, 5}, {15, 12, 32, 15}, false},
  };
  runAddToChannel2DTests(cases);
}

BOOST_AUTO_TEST_CASE(AddToChannel2DSmall,
                     *boost::unit_test::precondition(enableIfNotSim())) {
  std::vector<TestCase> cases = {
      {HALF, {1, 4, 8, 12, 16}, {480, 480, 480, 480, 480}, true},
      {HALF, {1, 4, 8, 5, 8}, {15, 12, 40, 15, 168}, false},
      {FLOAT, {1, 4, 8, 12, 16}, {480, 480, 480, 480, 480}, false},
      {FLOAT, {1, 4, 8, 5, 8}, {15, 12, 40, 15, 168}, true},
  };
  runAddToChannel2DTests(cases);
}

std::size_t maxBlockCount() {
  // This is the maximum acts_block_count the vertex supports.
  return 4095;
}

BOOST_AUTO_TEST_CASE(AddToChannel2DLarge1_half,
                     *boost::unit_test::precondition(enableIfNotSim())) {
  std::vector<TestCase> cases = {
      {HALF, {1, 1}, {maxBlockCount(), 80}, false},
  };
  runAddToChannel2DTests(cases);
}

BOOST_AUTO_TEST_CASE(AddToChannel2DLarge8_half,
                     *boost::unit_test::precondition(enableIfNotSim())) {
  std::vector<TestCase> cases = {
      {HALF, {8, 8}, {8000, 80}, true},
  };
  runAddToChannel2DTests(cases);
}

BOOST_AUTO_TEST_CASE(AddToChannel2DLarge1_float,
                     *boost::unit_test::precondition(enableIfNotSim())) {
  std::vector<TestCase> cases = {
      {FLOAT, {1, 1}, {maxBlockCount(), 80}, true},
  };
  runAddToChannel2DTests(cases);
}

BOOST_AUTO_TEST_CASE(AddToChannel2DLarge8_float,
                     *boost::unit_test::precondition(enableIfNotSim())) {
  std::vector<TestCase> cases = {
      {FLOAT, {8, 8}, {8000, 80}, false},
  };
  runAddToChannel2DTests(cases);
}


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


#ifndef __HEAD_POSE_ESTIMATION
#define __HEAD_POSE_ESTIMATION

#include <dlib/opencv.h>
#include <dlib/image_processing.h>
#include <dlib/image_processing/frontal_face_detector.h>
#include <opencv2/core/core.hpp>

#include <vector>
#include <array>
#include <string>

// Anthropometric for male adult
// Relative position of various facial feature relative to sellion
// Values taken from https://en.wikipedia.org/wiki/Human_head
// X points forward
const static cv::Point3f P3D_SELLION(0., 0.,0.);
const static cv::Point3f P3D_RIGHT_EYE(-20., -65.5,-5.);
const static cv::Point3f P3D_LEFT_EYE(-20., 65.5,-5.);
const static cv::Point3f P3D_RIGHT_EAR(-100., -77.5,-6.);
const static cv::Point3f P3D_LEFT_EAR(-100., 77.5,-6.);
const static cv::Point3f P3D_NOSE(21.0, 0., -48.0);
const static cv::Point3f P3D_STOMMION(10.0, 0., -75.0);
const static cv::Point3f P3D_MENTON(0., 0.,-133.0);

static const int MAX_FEATURES_TO_TRACK=100;

// Interesting facial features with their landmark index
enum FACIAL_FEATURE {
    NOSE=30,
    RIGHT_EYE=36,
    LEFT_EYE=45,
    RIGHT_SIDE=0,
    LEFT_SIDE=16,
    EYEBROW_RIGHT=21,
    EYEBROW_LEFT=22,
    MOUTH_UP=51,
    MOUTH_DOWN=57,
    MOUTH_RIGHT=48,
    MOUTH_LEFT=54,
    SELLION=27,
    MOUTH_CENTER_TOP=62,
    MOUTH_CENTER_BOTTOM=66,
    MENTON=8
};

typedef cv::Matx44d head_pose;

class HeadPoseEstimation {

public:

    HeadPoseEstimation(const std::string& face_detection_model = "shape_predictor_68_face_landmarks.dat", float focalLength=455.);

    /** Returns the 2D position (in image coordinates) of the 68 facial features
     * detected by dlib (or an empty vector if no face is detected).
     */
    std::vector<std::vector<cv::Point>> update(cv::InputArray image);

    head_pose pose(size_t face_idx) const;

    std::vector<head_pose> poses() const;

    /** Returns an augmented image with the detected facial features and head pose drawn in.
     *
     * Leave either detected_features or detected_poses empty to skip drawing the respective detections.
     */
    cv::Mat drawDetections(const cv::Mat& original_image, const std::vector<std::vector<cv::Point>>& detected_features, const std::vector<head_pose>& detected_poses);

    float focalLength;
    float opticalCenterX;
    float opticalCenterY;

private:

    dlib::cv_image<dlib::bgr_pixel> current_image;

    dlib::frontal_face_detector detector;
    dlib::shape_predictor pose_model;

    std::vector<dlib::rectangle> faces;

    std::vector<dlib::full_object_detection> shapes;

    void drawFeatures(const std::vector<std::vector<cv::Point>>& detected_features, cv::Mat& result) const;

    void drawPose(const head_pose& detected_pose, size_t face_idx, cv::Mat& result) const;

    /** Return the point corresponding to the dictionary marker.
    */
    cv::Point2f coordsOf(size_t face_idx, FACIAL_FEATURE feature) const;

    /** Returns true if the lines intersect (and set r to the intersection
     *  coordinates), false otherwise.
     */
    bool intersection(cv::Point2f o1, cv::Point2f p1,
                      cv::Point2f o2, cv::Point2f p2,
                      cv::Point2f &r) const;

};

#endif // __HEAD_POSE_ESTIMATION


/*
 * This file is part of the DUDS project. It is subject to the BSD-style
 * license terms in the LICENSE file found in the top-level directory of this
 * distribution and at https://github.com/jjackowski/duds/blob/master/LICENSE.
 * No part of DUDS, including this file, may be copied, modified, propagated,
 * or distributed except according to the terms contained in the LICENSE file.
 *
 * Copyright (C) 2017  Jeff Jackowski
 */
#ifndef PLANETARY_HPP
#define PLANETARY_HPP

#include <duds/time/planetary/LeapSeconds.hpp>
#include <boost/date_time/gregorian/gregorian.hpp>
#include <duds/data/GenericValue.hpp>

namespace duds {

/**
 * Items that deal with time, a surprisingly difficult and annoying subject.
 * So difficult and annoying, I opted against an implementation close to
 * exitsing ones because those implementations and their conventions seem
 * to be part of the problem. Seconds will not be redefined, solar days
 * do not have a constant length, and a one second point in time will not
 * occur twice. The system here lives within that reality.
 * @author  Jeff Jackowski
 */
namespace time {

namespace planetary {

using std::unique_ptr;
/*
class PlanetaryTime {
	LeapSeconds leaps;
	int days;
};
*/

// move to earth.hpp
/**
 * Terran time stuffs.
 */
class Earth {
	LeapBounds<> bound;
public:
	Earth();
	LeapSeconds leaps;
	static const boost::gregorian::date &dateZero();
	static const boost::posix_time::ptime &timeZero();
	/**
	 * Makes the Earth planetary time object using the given zoneinfo file for
	 * data on leap seconds.
	 */
	static void make(const std::string &path = "/usr/share/zoneinfo-leaps/UTC");
	typedef duds::time::interstellar::Femtoseconds duration;
	typedef duration::rep rep;
	typedef duration::period period;
	typedef std::chrono::time_point<Earth, duration> time_point;
	static constexpr bool is_steady = false;
	time_point now() noexcept {
		// wrong
		return time_point(std::chrono::duration_cast<duration, rep, period>(
			std::chrono::system_clock::now().time_since_epoch()));
	}
	/**
	 * Converts from IST to UTC and provides a date result.
	 */
	boost::gregorian::date date(
		const duds::time::interstellar::SecondTime &t
	) const;
	template <class ISTime>
	/**
	 * Converts from IST to UTC and provides a date result.
	 */
	boost::gregorian::date date(const ISTime &t) const {
		return date(interstellar::SecondTime(
			std::chrono::duration_cast<interstellar::Seconds>(
				t.time_since_epoch()
			)
		));
	}
	/**
	 * Converts from IST to UTC and provides a std::time_t result.
	 */
	std::time_t timeUtc(
		const duds::time::interstellar::SecondTime &t
	) const;
	template <class ISTime>
	/**
	 * Converts from IST to UTC and provides a std::time_t result.
	 */
	std::time_t timeUtc(const ISTime &t) const {
		return timeUtc(interstellar::SecondTime(
			std::chrono::duration_cast<interstellar::Seconds>(
				t.time_since_epoch()
			)
		));
	}

	/**
	 * Converts from UTC to TAI and provides a date result.
	 */
	boost::gregorian::date dateUtcToTai(
		const duds::time::interstellar::SecondTime &t
		) const;
	template <class ISTime>
	/**
	 * Converts from IST to UTC and provides a date result.
	 */
	boost::gregorian::date dateUtcToTai(const ISTime &t) const {
		return dateUtcToTai(interstellar::SecondTime(
			std::chrono::duration_cast<interstellar::Seconds>(
				t.time_since_epoch()
			)
		));
	}
	/**
	 * Converts from IST to UTC and provides a ptime result.
	 */
	boost::posix_time::ptime posix(
		const duds::time::interstellar::MilliTime &t
	) const;
	template <class ISTime>
	/**
	 * Converts from IST to UTC and provides a ptime result.
	 */
	boost::posix_time::ptime posix(const ISTime &t) const {
		return posix(duds::time::interstellar::MilliTime(
			std::chrono::duration_cast<duds::time::interstellar::Milliseconds>(
				t.time_since_epoch()
			)
		));
	}
	/**
	 * Converts from UTC to TAI and provides a ptime result.
	 */
	boost::posix_time::ptime posixUtcToTai(
		const duds::time::interstellar::MilliTime &t
	) const;
	template <class ISTime>
	/**
	 * Converts from UTC to TAI and provides a ptime result.
	 */
	boost::posix_time::ptime posixUtcToTai(const ISTime &t) const {
		return posixUtcToTai(duds::time::interstellar::MilliTime(
			std::chrono::duration_cast<duds::time::interstellar::Milliseconds>(
				t.time_since_epoch()
			)
		));
	}
	// UTC

	/**
	 * Adds leap seconds to the given time in UTC, resulting in TAI.
	 * @param time  The time that needs leap seconds.
	 */
	template <class Clock, class Duration>
	void addLeapSeconds(
		duds::time::interstellar::TimePoint<Clock, Duration> &time
	) {
		//duds::time::interstellar::SecondTime comp = time;
		if (!bound.within(time)) {
			bound = leaps.getLeapBounds(time);
		}
		assert(bound.within(time));
		time += bound.leaps;
	}

	/**
	 * Converts a Gregorian calendar date into one of the time formats defined
	 * inside the duds::time::interstellar namespace.
	 */
	template <class ISTime>
	void date(ISTime &dest, const boost::gregorian::date &src) {
		dest = duds::time::interstellar::SecondTime(
			duds::time::interstellar::Seconds((src - dateZero()).days() * 86400)
		);
		duds::time::interstellar::Seconds ls = leaps.leapSeconds(dest);
		dest += ls;
	}
	/**
	 * Converts a POSIX time into one of the time formats defined
	 * inside the duds::time::interstellar namespace.
	 */
	template <class ISTime>
	void time(ISTime &dest, const boost::posix_time::ptime &src) {
		dest = duds::time::interstellar::MilliTime(
			duds::time::interstellar::Milliseconds(
				(src - timeZero()).total_milliseconds()
			)
		);
		duds::time::interstellar::Seconds ls = leaps.leapSeconds(dest);
		dest += ls;
	}
};

extern unique_ptr<Earth> earth;

/**
 * Converts without applying any modification to the time, so if the input
 * is in TAI, the result will be, too. Not a member of Earth to emphasize that
 * this is just a type conversion that does no adjustment of the time.
 */
template <class ISTime>
inline boost::gregorian::date ToDate(const ISTime &t) {
	return Earth::dateZero() + boost::gregorian::days(
		std::chrono::duration_cast<duds::time::interstellar::Seconds>(
			t.time_since_epoch()
		).count()
	);
}

/**
 * Converts without applying any modification to the time, so if the input
 * is in TAI, the result will be, too. Not a member of Earth to emphasize that
 * this is just a type conversion that does no adjustment of the time.
 */
template <class ISTime>
inline boost::posix_time::ptime ToPosix(const ISTime &t) {
	return Earth::timeZero() + boost::posix_time::milliseconds(
		std::chrono::duration_cast<duds::time::interstellar::Milliseconds>(
			t.time_since_epoch()
		).count()
	);
}

} } }

#endif        //  #ifndef PLANETARY_HPP


#include <aslam/backend/Optimizer2.hpp>
// std::partial_sum
#include <numeric>
#include <aslam/backend/ErrorTerm.hpp>
// M.inverse()
#include <Eigen/Dense>
#include <sm/eigen/assert_macros.hpp>
#include <sparse_block_matrix/linear_solver_dense.h>
#include <sparse_block_matrix/linear_solver_cholmod.h>
#ifndef QRSOLVER_DISABLED
#include <sparse_block_matrix/linear_solver_spqr.h>
#include <aslam/backend/SparseQrLinearSystemSolver.hpp>
#endif
#include <aslam/backend/sparse_matrix_functions.hpp>
#include <aslam/backend/BlockCholeskyLinearSystemSolver.hpp>
#include <aslam/backend/SparseCholeskyLinearSystemSolver.hpp>
#include <aslam/backend/DenseQrLinearSystemSolver.hpp>
#include <sm/PropertyTree.hpp>

template <typename T>
T getDeprecatedPropertyIfItExists(const sm::ConstPropertyTree& config, const std::string & name, const std::string & newName, T defaultValue, T (sm::ConstPropertyTree::* getter)(const std::string & key, T defaultValue) const){
  const T depV = (config.*getter)(name, defaultValue);
  const T v = (config.*getter)(newName, defaultValue);
  if(depV != defaultValue){
    std::cerr << "Property " << name << " is DEPREACTED! Use " << newName << " instead." << std::endl;
    if(v != defaultValue){
      SM_THROW(std::runtime_error, "Both properties " + name + " (deprecated) and " + newName + " are used together!");
    }
    return depV;
  }
  return v;
}

namespace aslam {
    namespace backend {

        void Optimizer2::Status::resetImplementation() {
          srv = SolutionReturnValue();
        }

        Optimizer2::Optimizer2(const Options& options) :
            _options(options)
        {
            initializeLinearSolver();
            initializeTrustRegionPolicy();
        }

        Optimizer2::Optimizer2(const sm::ConstPropertyTree& config, boost::shared_ptr<LinearSystemSolver> linearSystemSolver, boost::shared_ptr<TrustRegionPolicy> trustRegionPolicy) {
          Options options;
          options.convergenceDeltaError = getDeprecatedPropertyIfItExists(config, "convergenceDeltaJ", "convergenceDeltaError", options.convergenceDeltaError, static_cast<double(sm::ConstPropertyTree::*)(const std::string&, double) const>(&sm::ConstPropertyTree::getDouble));
          options.convergenceDeltaX = config.getDouble("convergenceDeltaX", options.convergenceDeltaX);
          options.maxIterations = config.getInt("maxIterations", options.maxIterations);
          options.doSchurComplement = config.getBool("doSchurComplement", options.doSchurComplement);
          options.verbose = config.getBool("verbose", options.verbose);
          options.linearSolverMaximumFails = config.getInt("linearSolverMaximumFails", options.linearSolverMaximumFails);
          options.numThreadsJacobian = getDeprecatedPropertyIfItExists(config, "nThreads", "numThreadsJacobian", (int)options.numThreadsJacobian, static_cast<int(sm::ConstPropertyTree::*)(const std::string&, int) const>(&sm::ConstPropertyTree::getInt));
          options.numThreadsError = config.getInt("numThreadsError", options.numThreadsError);
          options.linearSystemSolver = linearSystemSolver;
          options.trustRegionPolicy = trustRegionPolicy;
          _options = options;
          initializeLinearSolver();
          initializeTrustRegionPolicy();
          // USING C++11 would allow to do constructor delegation and more elegant code, i.e., directly call the upper constructor
        }

        Optimizer2::~Optimizer2()
        {
        }

        void Optimizer2::initializeTrustRegionPolicy()
        {
          if( !_options.trustRegionPolicy ) {
            _options.verbose && std::cout << "No trust region policy set in the options. Defaulting to levenberg_marquardt\n";
            _trustRegionPolicy.reset( new LevenbergMarquardtTrustRegionPolicy() );
          } else {
            _trustRegionPolicy = _options.trustRegionPolicy;
          }

          // \todo remove this check when the sparse qr solver supports an augmented diagonal
          if(_solver->name() == "sparse_qr" && _trustRegionPolicy->name() == "levenberg_marquardt") {
            _options.verbose && std::cout << "The sparse_qr solver is not compatible with levenberg_marquardt. Changing to the dog_leg trust region policy\n";
            _trustRegionPolicy.reset( new DogLegTrustRegionPolicy() );
          }

          _options.verbose && std::cout << "Using the " << _trustRegionPolicy->name() << " trust region policy\n";

        }

        void Optimizer2::initializeLinearSolver()
        {
          if( ! _options.linearSystemSolver ) {
            _options.verbose && std::cout << "No linear system solver set in the options. Defaulting to the sparse_cholesky solver\n";
            _solver.reset(new SparseCholeskyLinearSystemSolver());
          } else {
            _solver = _options.linearSystemSolver;
          }

          _options.verbose && std::cout << "Using the " << _solver->name() << " linear system solver\n";
        }

        void Optimizer2::initializeImplementation()
        {
            OptimizerProblemManagerBase::initializeImplementation();
            initializeLinearSolver();
            initializeTrustRegionPolicy();

            Timer initMx("Optimizer2: Initialize---Matrices");
            // Set up the block matrix structure.
            _solver->initMatrixStructure(getDesignVariables(), problemManager().getErrorTerms(), _trustRegionPolicy->requiresAugmentedDiagonal());
            initMx.stop();
            _options.verbose && std::cout << "Optimization problem initialized with " << problemManager().numDesignVariables() << " design variables and " << problemManager().getErrorTerms().size() << " error terms\n";
            _options.verbose && std::cout << "The Jacobian matrix is " << problemManager().getTotalDimSquaredErrorTerms() << " x " << problemManager().numOptParameters() << std::endl;
        }

        /*
        // returns true of stop!
        bool Optimizer2::evaluateStoppingCriterion(int iterations)
        {

        // as we have analytic Jacobians we can assume the precision to be:
        double epsilon = std::numeric_limits<double>::epsilon();

        double x_norm = ...;

        // the gradient: is simply the right hand side of GN:
        double grad_norm = _rhs.norm();
        double abs_J = fabs(_status.error);

        // the first condition:
        bool crit1 = grad_norm < sqrt(epsilon) * (1 + abs_J);

        bool crit2 = _dx.norm() < sqrt(epsilon) * (1 + x_norm);

        bool crit3 = fabs(_status.error - _p_J) < epsilon * (1 + abs_J);

        bool crit4 = iterations < _options.maxIterations;

        return (crit1 && crit2 && crit3) || crit4;

        }*/

      SolutionReturnValue Optimizer2::optimize()
      {
        OptimizerProblemManagerBase::optimize();
        return _status.srv;
      }

        void Optimizer2::optimizeImplementation()
        {
            Timer timeErr("Optimizer2: evaluate error", true);
            Timer timeSchur("Optimizer2: Schur complement", true);
            Timer timeBackSub("Optimizer2: Back substitution", true);
            Timer timeSolve("Optimizer2: Build and solve linear system", true);
            // Select the design variables and (eventually) the error terms involved in the optimization.
            SolutionReturnValue & srv = _status.srv;
            _status.numIterations = srv.iterations;

            _p_J = -1.0;

            // This sets _J
            timeErr.start();
            evaluateError(true);
            timeErr.stop();
            _p_J = _status.error;
            srv.JStart = _p_J;
            // *** while not done
            _options.verbose && std::cout << "[" << srv.iterations << ".0]: J: " << _status.error << std::endl;
            // Set up the estimation problem.
            double & deltaX = _status.maxDeltaX;
            deltaX = _options.convergenceDeltaX + 1.0;
            double & deltaJ = _status.deltaError;
            deltaJ = _options.convergenceDeltaError + 1.0;
            bool previousIterationFailed = false;
            bool linearSolverFailure = false;

            SM_ASSERT_TRUE(Exception, _solver.get() != NULL, "The solver is null");
            _trustRegionPolicy->setSolver(_solver);
            _trustRegionPolicy->optimizationStarting(_status.error);

            issueCallback<callback::event::OPTIMIZATION_INITIALIZED>();

            // Loop until convergence
            while (srv.iterations <  _options.maxIterations &&
                   srv.failedIterations < _options.maxIterations &&
                   ((deltaX > _options.convergenceDeltaX &&
                     fabs(deltaJ) > _options.convergenceDeltaError) ||
                    linearSolverFailure)) {

                timeSolve.start();
                bool solutionSuccess = _trustRegionPolicy->solveSystem(_status.error, previousIterationFailed, _options.numThreadsError, _dx);
                _status.numJacobianEvaluations++;
                SM_ASSERT_EQ(Exception, problemManager().numOptParameters(), size_t(_dx.size()), "_trustRegionPolicy->solveSystem yielded dx with wrong size!");
                timeSolve.stop();
                issueCallback<callback::event::LINEAR_SYSTEM_SOLVED>();

                if (!solutionSuccess) {
                    _options.verbose && std::cout << "[WARNING] System solution failed\n";
                    previousIterationFailed = true;
                    linearSolverFailure = true;
                    srv.failedIterations++;
                } else {
                    /// Apply the state update. _A, _b, _dx, and _H are passed in implicitly.
                    timeBackSub.start();
                    deltaX = applyStateUpdate();
                    timeBackSub.stop();
                    issueCallback<callback::event::DESIGN_VARIABLES_UPDATED>();
                    // This sets _J
                    timeErr.start();
                    evaluateError(true);
                    timeErr.stop();
                    deltaJ = _p_J - _status.error;
                    // This was a regression.
                    if( _trustRegionPolicy->revertOnFailure() )
                    {
                        if(deltaJ < 0.0)
                        {
                            _options.verbose && std::cout << "Last step was a regression. Reverting\n";
                            revertLastStateUpdate();
                            srv.failedIterations++;
                            previousIterationFailed = true;
                        }
                        else
                        {
                            _p_J = _status.error;
                            previousIterationFailed = false;
                        }
                    }
                    else
                    {
                        _p_J = _status.error;
                    }
                    srv.iterations++;
                    _status.numIterations = srv.iterations;

                    _options.verbose && std::cout << "[" << srv.iterations << "]: J: " << _status.error << ", dJ: " << deltaJ << ", deltaX: " << deltaX << ", ";
                    _options.verbose && _trustRegionPolicy->printState(std::cout);
                    _options.verbose && std::cout << std::endl;
                }
            } // if the linear solver failed / else
            srv.JFinal = _status.error = _p_J;
            srv.dXFinal = deltaX;
            srv.dJFinal = deltaJ;
            srv.linearSolverFailure = linearSolverFailure;

            //TODO make _status.convergence a set!
            if(srv.iterations >= _options.maxIterations){
              _status.convergence = MAX_ITERATIONS;
            } else if(linearSolverFailure || srv.failedIterations >= _options.maxIterations){
              _status.convergence = FAILURE;
            } else if (deltaX <= _options.convergenceDeltaX) {
              _status.convergence = DX;
            } else if (fabs(deltaJ) <= _options.convergenceDeltaError) {
              _status.convergence = DOBJECTIVE;
            }
        }

            DesignVariable* Optimizer2::designVariable(size_t i)
            {
                SM_ASSERT_LT_DBG(Exception, i, numDesignVariables(), "index out of bounds");
                return getDesignVariables().at(i);
            }

            size_t Optimizer2::numDesignVariables() const
            {
                return getDesignVariables().size();
            }

            double Optimizer2::applyStateUpdate()
            {
                // Apply the update to the dense state.
                int startIdx = 0;
                for (DesignVariable* d : getDesignVariables()) {
                    const int dbd = d->minimalDimensions();
                    Eigen::VectorXd dxS = _dx.segment(startIdx, dbd);
                    dxS *= d->scaling();
                    d->update(&dxS[0], dbd);
                    startIdx += dbd;
                }
                // Track the maximum delta
                // \todo: should this be some other metric?
                double deltaX = _dx.array().abs().maxCoeff();
                return deltaX;
            }

            void Optimizer2::revertLastStateUpdate()
            {
                for (DesignVariable * d : getDesignVariables()) {
                    d->revertUpdate();
                }
            }

            double Optimizer2::evaluateError(bool useMEstimator)
            {
              SM_ASSERT_TRUE(Exception, _solver.get() != NULL, "The solver is null");
              _status.error = _solver->evaluateError(_options.numThreadsError, useMEstimator, &_callbackManager);
              _status.numErrorEvaluations++;
              _callbackManager.issueCallback(callback::event::COST_UPDATED{_status.error, _p_J});
              return _status.error;
            }

            /// \brief return the reduced system dx
            const Eigen::VectorXd& Optimizer2::dx() const
            {
                return _dx;
            }

            /// The value of the objective function.
            double Optimizer2::J() const
            {
                return _status.error;
            }

            void Optimizer2::printTiming() const
            {
                sm::timing::Timing::print(std::cout);
            }

            void Optimizer2::checkProblemSetup()
            {
                // Check that all error terms are hooked up to design variables.
            }

            void Optimizer2::computeDiagonalCovariances(SparseBlockMatrix& outP, double lambda)
            {
                SM_THROW(Exception, "Broken");

                std::vector<std::pair<int, int> > blockIndices;
                for (size_t i = 0; i < getDesignVariables().size(); ++i) {
                    blockIndices.push_back(std::make_pair(i, i));
                }
                computeCovarianceBlocks(blockIndices, outP, lambda);
            }

    void Optimizer2::computeCovarianceBlocks(const std::vector<std::pair<int, int> > & /* blockIndices */, SparseBlockMatrix& /* outP */, double /* lambda */)
            {
                SM_THROW(Exception, "Broken");

            }

    void Optimizer2::computeCovariances(SparseBlockMatrix& /* outP */, double /* lambda */)
            {
                SM_THROW(Exception, "Broken");

            }

        void Optimizer2::computeHessian(SparseBlockMatrix& outH, double lambda)
            {

              boost::shared_ptr<BlockCholeskyLinearSystemSolver> solver_sp;
              solver_sp.reset(new BlockCholeskyLinearSystemSolver());
              // True here for creating the diagonal conditioning.
              solver_sp->initMatrixStructure(getDesignVariables(), problemManager().getErrorTerms(), true);

              _options.verbose && std::cout << "Setting the diagonal conditioner to: " << lambda << ".\n";
              evaluateError(false);
              solver_sp->setConstantConditioner(lambda);
              solver_sp->buildSystem(_options.numThreadsJacobian, false);
              _status.numJacobianEvaluations ++;
              solver_sp->copyHessian(outH);
            }

      const LinearSystemSolver * Optimizer2::getBaseSolver() const {
          return _solver.get();
      }

        const Matrix * Optimizer2::getJacobian() const {
            return _solver->Jacobian();
        }

        template <typename Event>
        void Optimizer2::issueCallback(){
          //TODO (HannesSommer) use ProceedInstruction value in the Optimizer
          _callbackManager.issueCallback(Event{_status.error, 0});
        }

        } // namespace backend
    } // namespace aslam


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

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.
// Copyright (c) 2013 Adam Wulkiewicz, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_EXTENSIONS_NSPHERE_STRATEGIES_CARTESIAN_POINT_IN_NSPHERE_HPP
#define BOOST_GEOMETRY_EXTENSIONS_NSPHERE_STRATEGIES_CARTESIAN_POINT_IN_NSPHERE_HPP

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/assert.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/strategies/covered_by.hpp>
#include <boost/geometry/strategies/within.hpp>

#include <boost/geometry/extensions/nsphere/views/center_view.hpp>

namespace boost { namespace geometry { namespace strategy
{

namespace within
{

struct point_nsphere_within_comparable_distance
{
    template <typename ComparableDistance, typename Radius>
    static inline bool apply(ComparableDistance const& ed_comp_dist
                           , Radius const& ing_radius)
    {
        return ed_comp_dist < ing_radius * ing_radius;
    }
};

struct point_nsphere_covered_by_comparable_distance
{
    template <typename ComparableDistance, typename Radius>
    static inline bool apply(ComparableDistance const& ed_comp_dist
                           , Radius const& ing_radius)
    {
        return ed_comp_dist <= ing_radius * ing_radius;
    }
};

template
<
    typename Point,
    typename NSphere,
    typename SubStrategy = point_nsphere_within_comparable_distance
>
struct point_in_nsphere
{
    static inline bool apply(Point const& point, NSphere const& nsphere)
    {
        return SubStrategy::apply(
                    geometry::comparable_distance(point, center_view<NSphere const>(nsphere)),
                    get_radius<0>(nsphere));
    }
};

//// For many geometry-in-nsphere, we do not have a strategy yet... but a default strategy should exist
//struct nsphere_dummy
//{
//    template <typename A, typename B>
//    static bool apply(A const& a, B const& b)
//    {
//        // Assertion if called
//        BOOST_GEOMETRY_ASSERT(false);
//        return false;
//    }
//};

} // namespace within

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

namespace within { namespace services
{

template <typename Point, typename NSphere>
struct default_strategy
    <
        point_tag, nsphere_tag,
        point_tag, nsphere_tag,
        cartesian_tag, cartesian_tag,
        Point, NSphere
    >
{
    typedef within::point_in_nsphere<Point, NSphere, within::point_nsphere_within_comparable_distance> type;
};

//template <typename AnyTag, typename AnyGeometry, typename NSphere>
//struct default_strategy
//    <
//        AnyTag, nsphere_tag,
//        AnyTag, nsphere_tag,
//        cartesian_tag, cartesian_tag,
//        AnyGeometry, NSphere
//    >
//{
//    typedef within::nsphere_dummy type;
//};

}} // namespace within::services

namespace covered_by { namespace services
{

template <typename Point, typename NSphere>
struct default_strategy
    <
        point_tag, nsphere_tag,
        point_tag, nsphere_tag,
        cartesian_tag, cartesian_tag,
        Point, NSphere
    >
{
    typedef within::point_in_nsphere<Point, NSphere, within::point_nsphere_covered_by_comparable_distance> type;
};

}} // namespace covered_by::services

#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}}} // namespace boost::geometry::strategy

#endif // BOOST_GEOMETRY_EXTENSIONS_NSPHERE_STRATEGIES_CARTESIAN_NSPHERE_IN_BOX_HPP


// --------------------------------------------------------------------------
//                   OpenMS -- Open-Source Mass Spectrometry
// --------------------------------------------------------------------------
// Copyright The OpenMS Team -- Eberhard Karls University Tuebingen,
// ETH Zurich, and Freie Universitaet Berlin 2002-2020.
//
// This software is released under a three-clause BSD license:
//  * Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//  * Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
//  * Neither the name of any author or any participating institution
//    may be used to endorse or promote products derived from this software
//    without specific prior written permission.
// For a full list of authors, refer to the file AUTHORS.
// --------------------------------------------------------------------------
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL ANY OF THE AUTHORS OR THE CONTRIBUTING
// INSTITUTIONS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
// OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
// OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
// ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// --------------------------------------------------------------------------
// $Maintainer: Hannes Roest $
// $Authors: Hannes Roest $
// --------------------------------------------------------------------------

#include <OpenMS/FILTERING/NOISEESTIMATION/SignalToNoiseEstimatorMedianRapid.h>

#include <numeric>

// array_wrapper needs to be included before it is used
// only in boost1.64+. See issue #2790
#if OPENMS_BOOST_VERSION_MINOR >= 64
#include <boost/serialization/array_wrapper.hpp>
#endif
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/variance.hpp>

namespace OpenMS
{

  void SignalToNoiseEstimatorMedianRapid::computeNoiseInWindows_(
      const std::vector<double>& mz_array, std::vector<double> int_array,
      std::vector<double> & result, double mz_start)
  {
    // PRECONDITION
    assert(mz_array.size() == int_array.size());
    assert(mz_array.size() > 2);

    // compute mean and standard deviation
    double sum = std::accumulate(int_array.begin(), int_array.end(), 0.0);
    double int_mean = sum / int_array.size();
    double sq_sum = std::inner_product(int_array.begin(), int_array.end(), int_array.begin(), 0.0);
    double int_stdev = std::sqrt(sq_sum / int_array.size() - int_mean * int_mean);

    std::vector<double>::const_iterator mz_start_it = mz_array.begin();
    std::vector<double>::const_iterator mz_end_it;
    std::vector<double>::iterator int_start_win = int_array.begin();
    std::vector<double>::iterator int_end_win = int_array.begin();
    for (size_t i = 0; i < result.size(); i++)
    {
      // Compute the the correct windows in m/z
      double mz_end = mz_start + window_length_;
      mz_end_it = std::lower_bound(mz_start_it, (std::vector<double>::const_iterator)mz_array.end(), mz_end);

      // Compute the the correct windows in intensity
      std::iterator_traits< std::vector<double>::const_iterator >::difference_type iterator_pos = std::distance(mz_start_it, mz_end_it);
      std::advance(int_end_win, iterator_pos);

      // compute median of all data between intensity start and intensity end
      double median = computeMedian_(int_start_win, int_end_win);
      result[i] = median;

      // Deal with a median of zero
      //
      // If we find a zero here, try to impute some value that might make sense as noise value ...
      // alternatively, one could also remove all zeros and compute the median on that
      if (result[i] == 0)
      {
        // Legacy implementation from SignalToNoiseEstimatorMedian
        //
        // max_intensity_ = gauss_global.mean + std::sqrt(gauss_global.variance) * auto_max_stdev_Factor_;
        // From the maximum intensity we can compute the value of the lowest
        // bin in the histogram of the SignalToNoiseEstimatorMedian algorithm:
        // maximum intensity divided by 60
        result[i] = (int_mean  + 3.0 * int_stdev) / 60;
      }

      mz_start_it = mz_end_it;
      int_start_win = int_end_win;
      mz_start += window_length_;
    }
  }

  double SignalToNoiseEstimatorMedianRapid::computeMedian_(std::vector<double>::iterator & first, std::vector<double>::iterator & last)
  {
    std::iterator_traits< std::vector<double>::const_iterator >::difference_type iterator_pos = std::distance(first, last);
    std::nth_element(first, first + iterator_pos / 2, last);

    double median;
    if (iterator_pos == 0)
    {
      median = 0.0;
    }
    else if (iterator_pos % 2 == 0)
    {
      // even case
      // compute the arithmetic mean between the two middle elements
      double f = *(first + iterator_pos / 2);
      std::nth_element(first, first + iterator_pos / 2 -1, last);
      double s = *(first + iterator_pos / 2 - 1);
      median = (f+s)/2.0;
    }
    else
    {
      // odd case
      median = *(first + iterator_pos / 2);
    }
    return median;
  }

} // namespace OpenMS