#include "catch.hpp"

#include <cmath>
#include <cstdio>
#include <iostream>
#include <sstream>
#include <vector>

#include <Eigen/Core>
#include <Eigen/SparseCore>

#include <SmurffCpp/Utils/MatrixUtils.h>
#include <SmurffCpp/Configs/MatrixConfig.h>

using namespace smurff;

static NoiseConfig fixed_ncfg(NoiseTypes::fixed);

TEST_CASE("MatrixConfig::MatrixConfig(std::uint64_t nrow, std::uint64_t ncol, const std::vector<double>& values, const NoiseConfig& noiseConfig)")
{
   std::vector<double> actualMatrixConfigValues = { 1, 5, 9, 2, 6, 10, 3, 7, 11, 4, 8, 12 };
   MatrixConfig actualMatrixConfig(3, 4, actualMatrixConfigValues, fixed_ncfg);
   Eigen::MatrixXd actualMatrix = matrix_utils::dense_to_eigen(actualMatrixConfig);

   Eigen::MatrixXd expectedMatrix(3, 4);
   expectedMatrix(0, 0) = 1; expectedMatrix(0, 1) = 2; expectedMatrix(0, 2) = 3; expectedMatrix(0, 3) = 4;
   expectedMatrix(1, 0) = 5; expectedMatrix(1, 1) = 6; expectedMatrix(1, 2) = 7; expectedMatrix(1, 3) = 8;
   expectedMatrix(2, 0) = 9; expectedMatrix(2, 1) = 10; expectedMatrix(2, 2) = 11; expectedMatrix(2, 3) = 12;

   REQUIRE(matrix_utils::equals(actualMatrix, expectedMatrix));
}

TEST_CASE("MatrixConfig(std::uint64_t nrow, std::uint64_t ncol, std::vector<double>&& values, const NoiseConfig& noiseConfig)")
{
   std::vector<double> actualMatrixConfigValues = { 1, 5, 9, 2, 6, 10, 3, 7, 11, 4, 8, 12 };
   MatrixConfig actualMatrixConfig(3, 4, std::move(actualMatrixConfigValues), fixed_ncfg);
   Eigen::MatrixXd actualMatrix = matrix_utils::dense_to_eigen(actualMatrixConfig);

   Eigen::MatrixXd expectedMatrix(3, 4);
   expectedMatrix(0, 0) = 1; expectedMatrix(0, 1) = 2; expectedMatrix(0, 2) = 3; expectedMatrix(0, 3) = 4;
   expectedMatrix(1, 0) = 5; expectedMatrix(1, 1) = 6; expectedMatrix(1, 2) = 7; expectedMatrix(1, 3) = 8;
   expectedMatrix(2, 0) = 9; expectedMatrix(2, 1) = 10; expectedMatrix(2, 2) = 11; expectedMatrix(2, 3) = 12;

   REQUIRE(actualMatrixConfigValues.data() == NULL);
   REQUIRE(matrix_utils::equals(actualMatrix, expectedMatrix));
}

TEST_CASE("MatrixConfig(std::uint64_t nrow, std::uint64_t ncol, std::shared_ptr<std::vector<double> > values, const NoiseConfig& noiseConfig)")
{
   std::shared_ptr<std::vector<double> > actualMatrixConfigValues =
      std::make_shared<std::vector<double> >(std::initializer_list<double>({ 1, 5, 9, 2, 6, 10, 3, 7, 11, 4, 8, 12 }));
   MatrixConfig actualMatrixConfig(3, 4, actualMatrixConfigValues, fixed_ncfg);
   Eigen::MatrixXd actualMatrix = matrix_utils::dense_to_eigen(actualMatrixConfig);

   Eigen::MatrixXd expectedMatrix(3, 4);
   expectedMatrix(0, 0) = 1; expectedMatrix(0, 1) = 2; expectedMatrix(0, 2) = 3; expectedMatrix(0, 3) = 4;
   expectedMatrix(1, 0) = 5; expectedMatrix(1, 1) = 6; expectedMatrix(1, 2) = 7; expectedMatrix(1, 3) = 8;
   expectedMatrix(2, 0) = 9; expectedMatrix(2, 1) = 10; expectedMatrix(2, 2) = 11; expectedMatrix(2, 3) = 12;

   REQUIRE(matrix_utils::equals(actualMatrix, expectedMatrix));
}

TEST_CASE("MatrixConfig(std::uint64_t nrow, std::uint64_t ncol, const std::vector<std::uint32_t>& rows, const std::vector<std::uint32_t>& cols, const std::vector<double>& values, const NoiseConfig& noiseConfig)")
{
   std::vector<std::uint32_t> actualMatrixConfigRows = { 0, 0, 0, 0, 2, 2, 2, 2};
   std::vector<std::uint32_t> actualMatrixConfigCols = { 0, 1, 2, 3, 0, 1, 2, 3 };
   std::vector<double> actualMatrixConfigValues      = { 1, 2, 3, 4, 9, 10, 11, 12 };
   MatrixConfig actualMatrixConfig(3, 4, actualMatrixConfigRows, actualMatrixConfigCols, actualMatrixConfigValues, fixed_ncfg, false);
   Eigen::SparseMatrix<double> actualMatrix = matrix_utils::sparse_to_eigen(actualMatrixConfig);

   Eigen::SparseMatrix<double> expectedMatrix(3, 4);
   std::vector<Eigen::Triplet<double> > expectedMatrixTriplets;
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 0, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 1, 2));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 2, 3));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 3, 4));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 0, 9));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 1, 10));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 2, 11));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 3, 12));
   expectedMatrix.setFromTriplets(expectedMatrixTriplets.begin(), expectedMatrixTriplets.end());

   REQUIRE(matrix_utils::equals(actualMatrix, expectedMatrix));
}

TEST_CASE("MatrixConfig(std::uint64_t nrow, std::uint64_t ncol, std::vector<std::uint32_t>&& rows, std::vector<std::uint32_t>&& cols, std::vector<double>&& values, const NoiseConfig& noiseConfig)")
{
   std::vector<std::uint32_t> actualMatrixConfigRows = { 0, 0, 0, 0, 2, 2, 2, 2};
   std::vector<std::uint32_t> actualMatrixConfigCols = { 0, 1, 2, 3, 0, 1, 2, 3 };
   std::vector<double> actualMatrixConfigValues      = { 1, 2, 3, 4, 9, 10, 11, 12 };
   MatrixConfig actualMatrixConfig(3, 4, std::move(actualMatrixConfigRows), std::move(actualMatrixConfigCols), std::move(actualMatrixConfigValues), fixed_ncfg, false);
   Eigen::SparseMatrix<double> actualMatrix = matrix_utils::sparse_to_eigen(actualMatrixConfig);

   Eigen::SparseMatrix<double> expectedMatrix(3, 4);
   std::vector<Eigen::Triplet<double> > expectedMatrixTriplets;
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 0, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 1, 2));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 2, 3));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 3, 4));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 0, 9));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 1, 10));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 2, 11));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 3, 12));
   expectedMatrix.setFromTriplets(expectedMatrixTriplets.begin(), expectedMatrixTriplets.end());

   REQUIRE(actualMatrixConfigRows.data() == NULL);
   REQUIRE(actualMatrixConfigCols.data() == NULL);
   REQUIRE(actualMatrixConfigValues.data() == NULL);
   REQUIRE(matrix_utils::equals(actualMatrix, expectedMatrix));
}

TEST_CASE("MatrixConfig(std::uint64_t nrow, std::uint64_t ncol, std::shared_ptr<std::vector<std::uint32_t> > rows, std::shared_ptr<std::vector<std::uint32_t> > cols, std::shared_ptr<std::vector<double> > values, const NoiseConfig& noiseConfig)")
{
   std::shared_ptr<std::vector<std::uint32_t> > actualMatrixConfigRows =
      std::make_shared<std::vector<std::uint32_t> >(std::initializer_list<std::uint32_t>({ 0, 0, 0, 0, 2, 2, 2, 2 }));
   std::shared_ptr<std::vector<std::uint32_t> > actualMatrixConfigCols =
      std::make_shared<std::vector<std::uint32_t> >(std::initializer_list<std::uint32_t>({ 0, 1, 2, 3, 0, 1, 2, 3}));
   std::shared_ptr<std::vector<double> > actualMatrixConfigValues =
      std::make_shared<std::vector<double> >(std::initializer_list<double>({ 1, 2, 3, 4, 9, 10, 11, 12 }));
   MatrixConfig actualMatrixConfig(3, 4, actualMatrixConfigRows, actualMatrixConfigCols, actualMatrixConfigValues, fixed_ncfg, false);
   Eigen::SparseMatrix<double> actualMatrix = matrix_utils::sparse_to_eigen(actualMatrixConfig);

   Eigen::SparseMatrix<double> expectedMatrix(3, 4);
   std::vector<Eigen::Triplet<double> > expectedMatrixTriplets;
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 0, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 1, 2));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 2, 3));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 3, 4));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 0, 9));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 1, 10));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 2, 11));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 3, 12));
   expectedMatrix.setFromTriplets(expectedMatrixTriplets.begin(), expectedMatrixTriplets.end());

   REQUIRE(matrix_utils::equals(actualMatrix, expectedMatrix));
}

TEST_CASE("MatrixConfig(std::uint64_t nrow, std::uint64_t ncol, const std::vector<std::uint32_t>& rows, const std::vector<std::uint32_t>& cols, const NoiseConfig& noiseConfig)")
{
   Eigen::SparseMatrix<double> expectedMatrix(3, 4);
   std::vector<Eigen::Triplet<double> > expectedMatrixTriplets;
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 0, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 1, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 2, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 3, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 0, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 1, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 2, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 3, 1));
   expectedMatrix.setFromTriplets(expectedMatrixTriplets.begin(), expectedMatrixTriplets.end());

   std::vector<std::uint32_t> actualMatrixConfigRows = { 0, 0, 0, 0, 2, 2, 2, 2 };
   std::vector<std::uint32_t> actualMatrixConfigCols = { 0, 1, 2, 3, 0, 1, 2, 3 };
   MatrixConfig actualMatrixConfig(3, 4, actualMatrixConfigRows, actualMatrixConfigCols, fixed_ncfg, false);
   Eigen::SparseMatrix<double> actualMatrix = matrix_utils::sparse_to_eigen(actualMatrixConfig);

   REQUIRE(matrix_utils::equals(actualMatrix, expectedMatrix));
}

TEST_CASE("MatrixConfig(std::uint64_t nrow, std::uint64_t ncol, std::vector<std::uint32_t>&& rows, std::vector<std::uint32_t>&& cols, const NoiseConfig& noiseConfig)")
{
   Eigen::SparseMatrix<double> expectedMatrix(3, 4);
   std::vector<Eigen::Triplet<double> > expectedMatrixTriplets;
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 0, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 1, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 2, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 3, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 0, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 1, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 2, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 3, 1));
   expectedMatrix.setFromTriplets(expectedMatrixTriplets.begin(), expectedMatrixTriplets.end());

   std::vector<std::uint32_t> actualMatrixConfigRows = { 0, 0, 0, 0, 2, 2, 2, 2 };
   std::vector<std::uint32_t> actualMatrixConfigCols = { 0, 1, 2, 3, 0, 1, 2, 3 };
   MatrixConfig actualMatrixConfig(3, 4, std::move(actualMatrixConfigRows), std::move(actualMatrixConfigCols), fixed_ncfg, false);
   Eigen::SparseMatrix<double> actualMatrix = matrix_utils::sparse_to_eigen(actualMatrixConfig);

   REQUIRE(actualMatrixConfigRows.data() == NULL);
   REQUIRE(actualMatrixConfigCols.data() == NULL);
   REQUIRE(matrix_utils::equals(actualMatrix, expectedMatrix));
}

TEST_CASE("MatrixConfig(std::uint64_t nrow, std::uint64_t ncol, std::shared_ptr<std::vector<std::uint32_t> > rows, std::shared_ptr<std::vector<std::uint32_t> > cols, const NoiseConfig& noiseConfig)")
{
   Eigen::SparseMatrix<double> expectedMatrix(3, 4);
   std::vector<Eigen::Triplet<double> > expectedMatrixTriplets;
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 0, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 1, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 2, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 3, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 0, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 1, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 2, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 3, 1));
   expectedMatrix.setFromTriplets(expectedMatrixTriplets.begin(), expectedMatrixTriplets.end());

   std::shared_ptr<std::vector<std::uint32_t> > actualMatrixConfigRows =
      std::make_shared<std::vector<std::uint32_t> >(std::initializer_list<uint32_t>({ 0, 0, 0, 0, 2, 2, 2, 2 }));
   std::shared_ptr<std::vector<std::uint32_t> > actualMatrixConfigCols =
      std::make_shared<std::vector<std::uint32_t> >(std::initializer_list<uint32_t>({ 0, 1, 2, 3, 0, 1, 2, 3 }));
   MatrixConfig actualMatrixConfig(3, 4, actualMatrixConfigRows, actualMatrixConfigCols, fixed_ncfg, false);
   Eigen::SparseMatrix<double> actualMatrix = matrix_utils::sparse_to_eigen(actualMatrixConfig);

   REQUIRE(matrix_utils::equals(actualMatrix, expectedMatrix));
}

//---

TEST_CASE("MatrixConfig(std::uint64_t nrow, std::uint64_t ncol, const std::vector<std::uint32_t>& columns, const std::vector<double>& values, const NoiseConfig& noiseConfig)")
{
   std::vector<std::uint32_t> actualMatrixConfigColumns = {
         0, 1, 2, 0, 1, 2, 0, 1, 2, 0, 1, 2,
         0, 0, 0, 1, 1, 1, 2, 2, 2, 3, 3, 3
   };
   std::vector<double> actualMatrixConfigValues = { 1, 5, 9, 2, 6, 10, 3, 7, 11, 4, 8, 12 };

   MatrixConfig actualMatrixConfig(3, 4, actualMatrixConfigColumns, actualMatrixConfigValues, fixed_ncfg, false);
   Eigen::MatrixXd actualMatrix = matrix_utils::sparse_to_eigen(actualMatrixConfig);

   Eigen::MatrixXd expectedMatrix(3, 4);
   expectedMatrix(0, 0) = 1; expectedMatrix(0, 1) = 2; expectedMatrix(0, 2) = 3; expectedMatrix(0, 3) = 4;
   expectedMatrix(1, 0) = 5; expectedMatrix(1, 1) = 6; expectedMatrix(1, 2) = 7; expectedMatrix(1, 3) = 8;
   expectedMatrix(2, 0) = 9; expectedMatrix(2, 1) = 10; expectedMatrix(2, 2) = 11; expectedMatrix(2, 3) = 12;

   REQUIRE(matrix_utils::equals(actualMatrix, expectedMatrix));
}

TEST_CASE("MatrixConfig(std::uint64_t nrow, std::uint64_t ncol, std::vector<std::uint32_t>&& columns, std::vector<double>&& values, const NoiseConfig& noiseConfig)")
{
   std::vector<std::uint32_t> actualMatrixConfigColumns = {
      0, 1, 2, 0, 1, 2, 0, 1, 2, 0, 1, 2,
      0, 0, 0, 1, 1, 1, 2, 2, 2, 3, 3, 3
   };
   std::vector<double> actualMatrixConfigValues = { 1, 5, 9, 2, 6, 10, 3, 7, 11, 4, 8, 12 };

   MatrixConfig actualMatrixConfig(3, 4, std::move(actualMatrixConfigColumns), std::move(actualMatrixConfigValues), fixed_ncfg, false);
   Eigen::MatrixXd actualMatrix = matrix_utils::sparse_to_eigen(actualMatrixConfig);

   Eigen::MatrixXd expectedMatrix(3, 4);
   expectedMatrix(0, 0) = 1; expectedMatrix(0, 1) = 2; expectedMatrix(0, 2) = 3; expectedMatrix(0, 3) = 4;
   expectedMatrix(1, 0) = 5; expectedMatrix(1, 1) = 6; expectedMatrix(1, 2) = 7; expectedMatrix(1, 3) = 8;
   expectedMatrix(2, 0) = 9; expectedMatrix(2, 1) = 10; expectedMatrix(2, 2) = 11; expectedMatrix(2, 3) = 12;

   REQUIRE(actualMatrixConfigColumns.data() == NULL);
   REQUIRE(actualMatrixConfigValues.data() == NULL);
   REQUIRE(matrix_utils::equals(actualMatrix, expectedMatrix));
}

TEST_CASE("MatrixConfig(std::uint64_t nrow, std::uint64_t ncol, std::shared_ptr<std::vector<std::uint32_t> > columns, std::shared_ptr<std::vector<double> > values, const NoiseConfig& noiseConfig)")
{
   std::shared_ptr<std::vector<std::uint32_t> > actualMatrixConfigColumns =
      std::make_shared<std::vector<std::uint32_t> >(
         std::initializer_list<std::uint32_t>{
            0, 1, 2, 0, 1, 2, 0, 1, 2, 0, 1, 2,
            0, 0, 0, 1, 1, 1, 2, 2, 2, 3, 3, 3
            }
         );
   std::shared_ptr<std::vector<double> > actualMatrixConfigValues =
      std::make_shared<std::vector<double> >(
         std::initializer_list<double>{ 1, 5, 9, 2, 6, 10, 3, 7, 11, 4, 8, 12 }
      );
   MatrixConfig actualMatrixConfig(3, 4, actualMatrixConfigColumns, actualMatrixConfigValues, fixed_ncfg, false);
   Eigen::MatrixXd actualMatrix = matrix_utils::sparse_to_eigen(actualMatrixConfig);

   Eigen::MatrixXd expectedMatrix(3, 4);
   expectedMatrix(0, 0) = 1; expectedMatrix(0, 1) = 2; expectedMatrix(0, 2) = 3; expectedMatrix(0, 3) = 4;
   expectedMatrix(1, 0) = 5; expectedMatrix(1, 1) = 6; expectedMatrix(1, 2) = 7; expectedMatrix(1, 3) = 8;
   expectedMatrix(2, 0) = 9; expectedMatrix(2, 1) = 10; expectedMatrix(2, 2) = 11; expectedMatrix(2, 3) = 12;

   REQUIRE(matrix_utils::equals(actualMatrix, expectedMatrix));
}


/***************************************************************************
 *   Copyright (C) by GFZ Potsdam                                          *
 *                                                                         *
 *   You can redistribute and/or modify this program under the             *
 *   terms of the SeisComP Public License.                                 *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   SeisComP Public License for more details.                             *
 ***************************************************************************/

#define SEISCOMP_COMPONENT MLh

#include <seiscomp3/logging/log.h>
#include <seiscomp3/processing/amplitudes/MLh.h>

#include <boost/bind.hpp>
#include <cstdio>

namespace Seiscomp {
namespace Processing {

namespace {

AmplitudeProcessor::AmplitudeValue average(
	const AmplitudeProcessor::AmplitudeValue &v0,
	const AmplitudeProcessor::AmplitudeValue &v1)
{
	AmplitudeProcessor::AmplitudeValue v;
	// Average both values
	v.value = (v0.value + v1.value) * 0.5;

	// Compute lower and upper uncertainty
	double v0l = v0.value;
	double v0u = v0.value;
	double v1l = v1.value;
	double v1u = v1.value;

	if ( v0.lowerUncertainty ) v0l -= *v0.lowerUncertainty;
	if ( v0.upperUncertainty ) v0u += *v0.upperUncertainty;
	if ( v1.lowerUncertainty ) v1l -= *v1.lowerUncertainty;
	if ( v1.upperUncertainty ) v1u += *v1.upperUncertainty;

	double l = 0, u = 0;

	l = std::max(l, v.value - v0l);
	l = std::max(l, v.value - v0u);
	l = std::max(l, v.value - v1l);
	l = std::max(l, v.value - v1u);

	u = std::max(l, v0l - v.value);
	u = std::max(l, v0u - v.value);
	u = std::max(l, v1l - v.value);
	u = std::max(l, v1u - v.value);

	v.lowerUncertainty = l;
	v.upperUncertainty = u;

	return v;
}

AmplitudeProcessor::AmplitudeTime average(
	const AmplitudeProcessor::AmplitudeTime &t0,
	const AmplitudeProcessor::AmplitudeTime &t1)
{
	AmplitudeProcessor::AmplitudeTime t;
	t.reference = Core::Time((double(t0.reference) + double(t1.reference)) * 0.5);

	// Compute lower and upper uncertainty
	Core::Time t0b = t0.reference + Core::TimeSpan(t0.begin);
	Core::Time t0e = t0.reference + Core::TimeSpan(t0.end);
	Core::Time t1b = t1.reference + Core::TimeSpan(t1.begin);
	Core::Time t1e = t1.reference + Core::TimeSpan(t1.end);

	Core::Time minTime = t.reference;
	Core::Time maxTime = t.reference;

	minTime = std::min(minTime, t0b);
	minTime = std::min(minTime, t0e);
	minTime = std::min(minTime, t1b);
	minTime = std::min(minTime, t1e);

	maxTime = std::max(maxTime, t0b);
	maxTime = std::max(maxTime, t0e);
	maxTime = std::max(maxTime, t1b);
	maxTime = std::max(maxTime, t1e);

	t.begin = (double)(minTime - t.reference);
	t.end = (double)(maxTime - t.reference);

	return t;
}

AmplitudeProcessor::AmplitudeValue gmean(
	const AmplitudeProcessor::AmplitudeValue &v0,
	const AmplitudeProcessor::AmplitudeValue &v1)
{
	AmplitudeProcessor::AmplitudeValue v;
	// Average both values
	v.value = sqrt(v0.value * v1.value);

	// Compute lower and upper uncertainty
	double v0l = v0.value;
	double v0u = v0.value;
	double v1l = v1.value;
	double v1u = v1.value;

	if ( v0.lowerUncertainty ) v0l -= *v0.lowerUncertainty;
	if ( v0.upperUncertainty ) v0u += *v0.upperUncertainty;
	if ( v1.lowerUncertainty ) v1l -= *v1.lowerUncertainty;
	if ( v1.upperUncertainty ) v1u += *v1.upperUncertainty;

	double l = 0, u = 0;

	l = std::max(l, v.value - v0l);
	l = std::max(l, v.value - v0u);
	l = std::max(l, v.value - v1l);
	l = std::max(l, v.value - v1u);

	u = std::max(l, v0l - v.value);
	u = std::max(l, v0u - v.value);
	u = std::max(l, v1l - v.value);
	u = std::max(l, v1u - v.value);

	v.lowerUncertainty = l;
	v.upperUncertainty = u;

	return v;
}

}

IMPLEMENT_SC_CLASS_DERIVED(AmplitudeProcessor_ML2h, AbstractAmplitudeProcessor_ML, "AmplitudeProcessor_ML");
REGISTER_AMPLITUDEPROCESSOR(AmplitudeProcessor_ML2h, "ML");

AmplitudeProcessor_MLh::AmplitudeProcessor_MLh()
: AbstractAmplitudeProcessor_ML("ML") {}

AmplitudeProcessor_ML2h::AmplitudeProcessor_ML2h()
: Processing::AmplitudeProcessor("ML") {
	setSignalEnd(150.);
	setMinSNR(0);
	// Maximum distance is 8 degrees
	setMaxDist(8);
	// Maximum depth is 80 km
	setMaxDepth(80);

	setUsedComponent(Horizontal);

	_combiner = TakeAverage;

	_ampN.setUsedComponent(FirstHorizontal);
	_ampE.setUsedComponent(SecondHorizontal);

	_ampE.setPublishFunction(boost::bind(&AmplitudeProcessor_ML2h::newAmplitude, this, _1, _2));
	_ampN.setPublishFunction(boost::bind(&AmplitudeProcessor_ML2h::newAmplitude, this, _1, _2));

	// Propagate configuration to single processors
	_ampN.setConfig(config());
	_ampE.setConfig(config());
}

AmplitudeProcessor_ML2h::AmplitudeProcessor_ML2h(const Core::Time &trigger)
: Processing::AmplitudeProcessor(trigger, "ML") {
	setSignalEnd(150.);
	setMinSNR(0);
	// Maximum distance is 8 degrees
	setMaxDist(8);
	// Maximum depth is 80 km
	setMaxDepth(80);

	setUsedComponent(Horizontal);

	_combiner = TakeAverage;

	_ampN.setUsedComponent(FirstHorizontal);
	_ampE.setUsedComponent(SecondHorizontal);

	_ampE.setPublishFunction(boost::bind(&AmplitudeProcessor_ML2h::newAmplitude, this, _1, _2));
	_ampN.setPublishFunction(boost::bind(&AmplitudeProcessor_ML2h::newAmplitude, this, _1, _2));

	// Propagate configuration to single processors
	_ampN.setConfig(config());
	_ampE.setConfig(config());

	_ampN.setTrigger(trigger);
	_ampE.setTrigger(trigger);
}

const AmplitudeProcessor *AmplitudeProcessor_ML2h::componentProcessor(Component comp) const {
	switch ( comp ) {
		case FirstHorizontalComponent:
			return &_ampN;
		case SecondHorizontalComponent:
			return &_ampE;
		default:
			break;
	}

	return NULL;
}

const DoubleArray *AmplitudeProcessor_ML2h::processedData(Component comp) const {
	switch ( comp ) {
		case FirstHorizontalComponent:
			return _ampN.processedData(comp);
		case SecondHorizontalComponent:
			return _ampE.processedData(comp);
		default:
			break;
	}

	return NULL;
}

void AmplitudeProcessor_ML2h::reprocess(OPT(double) searchBegin, OPT(double) searchEnd) {
	setStatus(WaitingForData, 0);
	_ampN.setConfig(config());
	_ampE.setConfig(config());

	_results[0] = _results[1] = Core::None;

	_ampN.reprocess(searchBegin, searchEnd);
	_ampE.reprocess(searchBegin, searchEnd);

	if ( !isFinished() ) {
		if ( _ampN.status() > Finished )
			setStatus(_ampN.status(), _ampN.statusValue());
		else
			setStatus(_ampE.status(), _ampE.statusValue());
	}
}

int AmplitudeProcessor_ML2h::capabilities() const {
	return _ampN.capabilities() | Combiner;
}

AmplitudeProcessor::IDList
AmplitudeProcessor_ML2h::capabilityParameters(Capability cap) const {
	if ( cap == Combiner ) {
		IDList params;
		params.push_back("Average");
		params.push_back("Max");
		params.push_back("Min");
		params.push_back("Geometric mean");
		return params;
	}

	return _ampN.capabilityParameters(cap);
}

bool AmplitudeProcessor_ML2h::setParameter(Capability cap, const std::string &value) {
	if ( cap == Combiner ) {
		if ( value == "Min" ) {
			_combiner = TakeMin;
			return true;
		}
		else if ( value == "Max" ) {
			_combiner = TakeMax;
			return true;
		}
		else if ( value == "Average" ) {
			_combiner = TakeAverage;
			return true;
		}
		else if ( value == "Geometric mean" ) {
			_combiner = TakeGeometricMean;
			return true;
		}

		return false;
	}

	_ampN.setParameter(cap, value);
	return _ampE.setParameter(cap, value);
}

bool AmplitudeProcessor_ML2h::setup(const Settings &settings) {
	// Copy the stream configurations (gain, orientation, responses, ...) to
	// the horizontal processors
	_ampN.streamConfig(FirstHorizontalComponent) = streamConfig(FirstHorizontalComponent);
	_ampE.streamConfig(SecondHorizontalComponent) = streamConfig(SecondHorizontalComponent);

	_combiner = TakeAverage;

	try {
		std::string s = settings.getString("amplitudes." + _type + ".combiner");
		if ( s == "average" )
			_combiner = TakeAverage;
		else if ( s == "max" )
			_combiner = TakeMax;
		else if ( s == "min" )
			_combiner = TakeMin;
		else if ( s == "geometric_mean" )
			_combiner = TakeGeometricMean;
		else {
			SEISCOMP_ERROR("%s: invalid combiner type for station %s.%s: %s",
			               _type.c_str(),
			               settings.networkCode.c_str(), settings.stationCode.c_str(),
			               s.c_str());
			return false;
		}
	}
	catch ( ... ) {}

	if ( !AmplitudeProcessor::setup(settings) ) return false;

	// Setup each component
	if ( !_ampN.setup(settings) || !_ampE.setup(settings) ) return false;

	return true;
}

void AmplitudeProcessor_ML2h::setTrigger(const Core::Time& trigger) {
	AmplitudeProcessor::setTrigger(trigger);
	_ampE.setTrigger(trigger);
	_ampN.setTrigger(trigger);
}

void AmplitudeProcessor_ML2h::computeTimeWindow() {
	// Copy configuration to each component
	_ampN.setConfig(config());
	_ampE.setConfig(config());

	_ampE.computeTimeWindow();
	_ampN.computeTimeWindow();
	setTimeWindow(_ampE.timeWindow() | _ampN.timeWindow());
}

double AmplitudeProcessor_ML2h::timeWindowLength(double distance_deg) const {
	double endN = _ampN.timeWindowLength(distance_deg);
	double endE = _ampE.timeWindowLength(distance_deg);
	_ampN.setSignalEnd(endN);
	_ampE.setSignalEnd(endE);
	return std::max(endN, endE);
}

void AmplitudeProcessor_ML2h::reset() {
	AmplitudeProcessor::reset();

	_results[0] = _results[1] = Core::None;

	_ampE.reset();
	_ampN.reset();
}

void AmplitudeProcessor_ML2h::close() {
	// TODO: Check for best available amplitude here
}

bool AmplitudeProcessor_ML2h::feed(const Record *record) {
	// Both processors finished already?
	if ( _ampE.isFinished() && _ampN.isFinished() ) return false;

	// Did an error occur?
	if ( status() > WaveformProcessor::Finished ) return false;

	if ( record->channelCode() == _streamConfig[FirstHorizontalComponent].code() ) {
		if ( !_ampN.isFinished() ) {
			_ampN.feed(record);
			if ( _ampN.status() == InProgress )
				setStatus(WaveformProcessor::InProgress, _ampN.statusValue());
			else if ( _ampN.isFinished() && _ampE.isFinished() ) {
				if ( !isFinished() ) {
					if ( _ampE.status() == Finished )
						setStatus(_ampN.status(), _ampN.statusValue());
					else
						setStatus(_ampE.status(), _ampE.statusValue());
				}
			}
		}
	}
	else if ( record->channelCode() == _streamConfig[SecondHorizontalComponent].code() ) {
		if ( !_ampE.isFinished() ) {
			_ampE.feed(record);
			if ( _ampE.status() == InProgress )
				setStatus(WaveformProcessor::InProgress, _ampE.statusValue());
			else if ( _ampE.isFinished() && _ampN.isFinished() ) {
				if ( !isFinished() ) {
					if ( _ampN.status() == Finished )
						setStatus(_ampE.status(), _ampE.statusValue());
					else
						setStatus(_ampN.status(), _ampN.statusValue());
				}
			}
		}
	}

	return true;
}

bool AmplitudeProcessor_ML2h::computeAmplitude(const DoubleArray &data,
                                               size_t i1, size_t i2,
                                               size_t si1, size_t si2,
                                               double offset,
                                               AmplitudeIndex *dt,
                                               AmplitudeValue *amplitude,
                                               double *period, double *snr) {
	return false;
}

void AmplitudeProcessor_ML2h::newAmplitude(const AmplitudeProcessor *proc,
                                           const AmplitudeProcessor::Result &res) {

	if ( isFinished() ) return;

	int idx = 0;

	if ( proc == &_ampE ) {
		idx = 0;
	}
	else if ( proc == &_ampN ) {
		idx = 1;
	}

	_results[idx] = ComponentResult();
	_results[idx]->value = res.amplitude;
	_results[idx]->time = res.time;
	_results[idx]->snr = res.snr;

	if ( _results[0] && _results[1] ) {
		setStatus(Finished, 100.);
		Result newRes;
		newRes.record = res.record;

		switch ( _combiner ) {
			case TakeAverage:
				newRes.amplitude = average(_results[0]->value, _results[1]->value);
				newRes.time = average(_results[0]->time, _results[1]->time);
				newRes.component = Horizontal;
				break;
			case TakeGeometricMean:
				newRes.amplitude = gmean(_results[0]->value, _results[1]->value);
				newRes.time = average(_results[0]->time, _results[1]->time);
				newRes.component = Horizontal;
				break;
			case TakeMin:
				if ( _results[0]->value.value <= _results[1]->value.value ) {
					newRes.amplitude = _results[0]->value;
					newRes.time = _results[0]->time;
					newRes.component = _ampE.usedComponent();
				}
				else {
					newRes.amplitude = _results[1]->value;
					newRes.time = _results[1]->time;
					newRes.component = _ampN.usedComponent();
				}
				break;
			case TakeMax:
				if ( _results[0]->value.value >= _results[1]->value.value ) {
					newRes.amplitude =  _results[0]->value;
					newRes.time = _results[0]->time;
					newRes.component = _ampE.usedComponent();
				}
				else {
					newRes.amplitude =  _results[1]->value;
					newRes.time = _results[1]->time;
					newRes.component = _ampN.usedComponent();
				}
				break;
		};

		newRes.period = -1;
		newRes.snr = std::min(_results[0]->snr, _results[1]->snr);
		emitAmplitude(newRes);
	}
}

}
}


#include <algorithm>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/register/linestring.hpp>
#include <boost/geometry/geometries/register/point.hpp>
#include <NTL/mat_ZZ.h>
#include <NTL/LLL.h>
#include <QVector2D>
#include "functions.hpp"
#include "LatticeFitter.hpp"

BOOST_GEOMETRY_REGISTER_POINT_2D_GET_SET(
    QPoint, int, boost::geometry::cs::cartesian,
    x, y, setX, setY)

BOOST_GEOMETRY_REGISTER_LINESTRING_TEMPLATED(QVector)
BOOST_GEOMETRY_REGISTER_LINESTRING(QPolygon)

namespace
{

template <typename Geometry, typename Point>
void translate(Geometry& g, const Point& d)
{
  boost::geometry::for_each_point(g, [&d](Point& p) {
    boost::geometry::add_point(p, d);
  });
};

template <typename Point>
Point extend_vector(Point origin, Point base_vector, const int scalar)
{
  // just extend the origin in the base vector's direction
  boost::geometry::multiply_value(base_vector, scalar * 5);
  boost::geometry::add_point(origin, base_vector);
  return origin;
}

template <typename Point, typename Line>
void translate_and_intersect(
    const Point& base_vector,
    Line extended_line,
    const Line& image,
    Line& output)
{
  // do the first intersection
  Line intersection_points;
  boost::geometry::intersection(extended_line, image, intersection_points);

  // it has to be 2 intersection points to be a valid line
  while (intersection_points.size() > 1) {
    // output.insert(
    //     output.end(),
    //     intersection_points.begin(),
    //     intersection_points.end());
    output.append(intersection_points);

    // translate "cutting" line with base vector
    translate(extended_line, base_vector);

    // and repeat until no valid lines can be created
    boost::geometry::clear(intersection_points);
    boost::geometry::intersection(extended_line, image, intersection_points);
  }
}

template <typename Point, typename Ring>
QVector<Point> grid_base_image_intersections(
    const Point& origin,
    Point base_vector_u,
    Point base_vector_v,
    const Ring& image,
    const int edge)
{
  using boost::geometry::multiply_value;

  QVector<Point> line;

  // STEP 1: ------------------------------------------------------------------
  // Obtain a long line laying on our base vector.
  line.push_back(extend_vector(origin, base_vector_u, edge));

  // Flip base vector direction
  multiply_value(base_vector_u, -1);
  line.push_back(extend_vector(origin, base_vector_u, edge));

  // STEP 2: ------------------------------------------------------------------
  // translate the line in both directions, and calculate intersections
  // with the image.
  QVector<Point> result;
  translate_and_intersect(base_vector_v, line, image, result);

  // flip base vector direction
  multiply_value(base_vector_v, -1);

  // pre-translate, as the two intersection points of the base vector's line
  // have already been found
  translate(line, base_vector_v);
  translate_and_intersect(base_vector_v, line, image, result);

  return result;
}

} // end unnamed namespace

QPixmap mat2QPixmap(const cv::Mat& image)
{
  cv::Mat rgb;
  cv::cvtColor(image, rgb, CV_BGR2RGB);
  return QPixmap::fromImage(
           QImage(static_cast<uchar*>(rgb.data),
                  rgb.cols, rgb.rows, rgb.step, QImage::Format_RGB888));
}

QPixmap mat2QPixmapGray(const cv::Mat& image)
{
  return QPixmap::fromImage(
           QImage(static_cast<uchar*>(image.data),
                  image.cols, image.rows, image.step, QImage::Format_Grayscale8));
}

void applyThreshold(const cv::Mat& image, cv::Mat& dest, double value)
{
  cv::threshold(image, dest, value, 255.0, cv::THRESH_BINARY);
}

QPolygon convexHull(const std::vector<cv::Point2f>& points)
{
  std::vector<cv::Point2f> hull;
  cv::convexHull(points, hull);

  QPolygon poly;

  for (const auto& point : hull)
    poly.append(QPoint(point.x, point.y));

  return poly;
}

QVector<QPoint> generateGrid(
    const Lattice& lattice,
    int width, int height)
{
  QPoint origin (
    NTL::to_int(lattice.origin[0]),
    NTL::to_int(lattice.origin[1])
  );

  QPoint baseVectorU {
    NTL::to_int(lattice.bases(1, 1)),
    NTL::to_int(lattice.bases(1, 2))
  };

  QPoint baseVectorV {
    NTL::to_int(lattice.bases(2, 1)),
    NTL::to_int(lattice.bases(2, 2))
  };

  QVector<QPoint> image_rectangle {
    { 0, 0 },
    { 0, height },
    { width, height },
    { width, 0 },
    { 0, 0 },
  };

  // get intersection points with first base vector
  auto lines_u = grid_base_image_intersections(
      origin, baseVectorU, baseVectorV, image_rectangle, width);

  // and just swap the base vectors for the other set of points
  auto lines_v = grid_base_image_intersections(
      origin, baseVectorV, baseVectorU, image_rectangle, height);

  QVector<QPoint> intersection_points;
  intersection_points.resize(lines_u.size() + lines_v.size());
  intersection_points.append(lines_u);
  intersection_points.append(lines_v);

  return intersection_points;
}


/*
 *          Copyright Andrey Semashev 2007 - 2015.
 * Distributed under the Boost Software License, Version 1.0.
 *    (See accompanying file LICENSE_1_0.txt or copy at
 *          http://www.boost.org/LICENSE_1_0.txt)
 */
/*!
 * \file   alignment_gap_between.hpp
 * \author Andrey Semashev
 * \date   20.11.2007
 *
 * \brief  This header is the Boost.Log library implementation, see the library documentation
 *         at http://www.boost.org/doc/libs/release/libs/log/doc/html/index.html.
 */

#ifndef BOOST_LOG_ALIGNMENT_GAP_BETWEEN_HPP_INCLUDED_
#define BOOST_LOG_ALIGNMENT_GAP_BETWEEN_HPP_INCLUDED_

#include <boost/log/detail/config.hpp>
#include <boost/type_traits/alignment_of.hpp>
#include <boost/log/detail/header.hpp>

namespace boost {

BOOST_LOG_OPEN_NAMESPACE

namespace aux {

//! The metafunction computes the minimal gap between objects t1 and t2 of types T1 and T2
//! that would be needed to maintain the alignment of t2 if it's placed right after t1
template< typename T1, typename T2 >
struct alignment_gap_between
{
    enum _
    {
        T2_alignment = boost::alignment_of< T2 >::value,
        tail_size = sizeof(T1) % T2_alignment,
        value = tail_size > 0 ? T2_alignment - tail_size : 0
    };
};

} // namespace aux

BOOST_LOG_CLOSE_NAMESPACE // namespace log

} // namespace boost

#include <boost/log/detail/footer.hpp>

#endif // BOOST_LOG_ALIGNMENT_GAP_BETWEEN_HPP_INCLUDED_


/// @file
/// @copyright The code is licensed under the BSD License
///            <http://opensource.org/licenses/BSD-2-Clause>,
///            Copyright (c) 2013-2015 Alexandre Hamez.
/// @author Alexandre Hamez

#include <boost/variant.hpp>

#include "mc/classic/dead_states.hh"
#include "mc/classic/place_bound.hh"
#include "mc/classic/reachability.hh"
#include "mc/classic/reachability_ast.hh"
#include "mc/classic/reachability_eval.hh"
#include "mc/shared/step.hh"

namespace pnmc { namespace mc { namespace classic {

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

void
reachability(const order& o, const properties::formulae& f, results& res)
{
  const bool deadlock = res.dead_states
                      ? *res.dead_states != zero()
                      : false;

  const auto bounds = place_bound(o, *res.states, f.places_bounds);
  const auto dead_transitions = [&]
  {
    if (res.dead_transitions) {return *res.dead_transitions;}
    else                      {return std::set<std::string>();}
  }();

  for (const auto& formula : f.booleans)
  {
    if (not res.reachability)
    {
      res.reachability.emplace();
    }
    const auto ast = make_ast(formula.expression, deadlock, dead_transitions, bounds);
    res.reachability->emplace_back(formula.id, eval(ast, *res.states));
  }

  for (const auto& formula : f.integers)
  {
    if (not res.evaluations)
    {
      res.evaluations.emplace();
    }
    const auto ast = make_ast(formula.expression, bounds);
    res.evaluations->emplace_back(formula.id, eval(ast, *res.states));
  }
}

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

}}} // namespace pnmc::mc::classic


//
// Copyright 2012 Christian Henning
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
#ifndef BOOST_GIL_EXTENSION_TOOLBOX_COLOR_SPACES_HSV_HPP
#define BOOST_GIL_EXTENSION_TOOLBOX_COLOR_SPACES_HSV_HPP

#include <boost/numeric/conversion/cast.hpp>

#include <boost/gil/color_convert.hpp>
#include <boost/gil/typedefs.hpp>
#include <boost/gil/detail/mp11.hpp>

#include <algorithm>
#include <cmath>

namespace boost{ namespace gil {

/// \addtogroup ColorNameModel
/// \{
namespace hsv_color_space
{
/// \brief Hue
struct hue_t {};
/// \brief Saturation
struct saturation_t{};
/// \brief Value
struct value_t {};
}
/// \}

/// \ingroup ColorSpaceModel
using hsv_t = mp11::mp_list
<
    hsv_color_space::hue_t,
    hsv_color_space::saturation_t,
    hsv_color_space::value_t
>;

/// \ingroup LayoutModel
using hsv_layout_t = layout<hsv_t>;

BOOST_GIL_DEFINE_ALL_TYPEDEFS(32f, float32_t, hsv)

/// \ingroup ColorConvert
/// \brief RGB to HSV
template <>
struct default_color_converter_impl< rgb_t, hsv_t >
{
   template <typename P1, typename P2>
   void operator()( const P1& src, P2& dst ) const
   {
      using namespace hsv_color_space;

      // only float32_t for hsv is supported
      float32_t temp_red   = channel_convert<float32_t>( get_color( src, red_t()   ));
      float32_t temp_green = channel_convert<float32_t>( get_color( src, green_t() ));
      float32_t temp_blue  = channel_convert<float32_t>( get_color( src, blue_t()  ));

      float32_t hue, saturation, value;

      float32_t min_color = (std::min)( temp_red, (std::min)( temp_green, temp_blue ));
      float32_t max_color = (std::max)( temp_red, (std::max)( temp_green, temp_blue ));

      value = max_color;

      float32_t diff = max_color - min_color;

      if( max_color < 0.0001f )
      {
         saturation = 0.f;
      }
      else
      {
         saturation = diff / max_color;
      }

      if( saturation < 0.0001f )
      {
         //it doesn't matter what value it has
         hue = 0.f;
      }
      else
      {
         if( (std::abs)( boost::numeric_cast<float32_t>(temp_red - max_color) ) < 0.0001f )
         {
            hue = ( temp_green - temp_blue )
                / diff;
         }
         else if( temp_green >= max_color ) // means == but >= avoids compiler warning; color is never greater than max
         {
            hue = 2.f + ( temp_blue - temp_red )
                / diff;
         }
         else
         {
            hue = 4.f + ( temp_red - temp_green )
                / diff;
         }

         //to bring it to a number between 0 and 1
         hue /= 6.f;

         if( hue < 0.f )
         {
            hue++;
         }
      }

      get_color( dst, hue_t() )        = hue;
      get_color( dst, saturation_t() ) = saturation;
      get_color( dst, value_t() )      = value;
   }
};

/// \ingroup ColorConvert
/// \brief HSV to RGB
template <>
struct default_color_converter_impl<hsv_t,rgb_t>
{
   template <typename P1, typename P2>
   void operator()( const P1& src, P2& dst) const
   {
      using namespace hsv_color_space;

      float32_t red, green, blue;

      //If saturation is 0, the color is a shade of gray
      if (std::abs(get_color(src, saturation_t())) < 0.0001f)
      {
         // If saturation is 0, the color is a shade of gray
         red   = get_color( src, value_t() );
         green = get_color( src, value_t() );
         blue  = get_color( src, value_t() );
      }
      else
      {
         float32_t frac, p, q, t, h;
         uint32_t i;

         //to bring hue to a number between 0 and 6, better for the calculations
         h = get_color( src, hue_t() );
         h *= 6.f;

         i = static_cast<uint32_t>(floor(h));

         frac = h - i;

         p = get_color( src, value_t() )
           * ( 1.f - get_color( src, saturation_t() ));

         q = get_color( src, value_t() )
           * ( 1.f - ( get_color( src, saturation_t() ) * frac ));

         t = get_color( src, value_t() )
           * ( 1.f - ( get_color( src, saturation_t() ) * ( 1.f - frac )));

         switch( i )
         {
            case 0:
            {
               red   = get_color( src, value_t() );
               green = t;
               blue  = p;

               break;
            }

            case 1:
            {
               red   = q;
               green = get_color( src, value_t() );
               blue  = p;

               break;
            }

            case 2:
            {
               red   = p;
               green = get_color( src, value_t() );
               blue  = t;

               break;
            }

            case 3:
            {
               red   = p;
               green = q;
               blue  = get_color( src, value_t() );

               break;
            }

            case 4:
            {
               red   = t;
               green = p;
               blue  = get_color( src, value_t() );

               break;
            }

            case 5:
            {
               red   = get_color( src, value_t() );
               green = p;
               blue  = q;

               break;
            }

         }
      }

      get_color(dst,red_t())  =
         channel_convert<typename color_element_type< P2, red_t >::type>( red );
      get_color(dst,green_t())=
         channel_convert<typename color_element_type< P2, green_t >::type>( green );
      get_color(dst,blue_t()) =
         channel_convert<typename color_element_type< P2, blue_t >::type>( blue );
   }
};

} // namespace gil
} // namespace boost

#endif


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

// Copyright (c) 2007-2011 Barend Gehrels, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_OVERLAY_GET_RELATIVE_ORDER_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_OVERLAY_GET_RELATIVE_ORDER_HPP

#include <boost/geometry/algorithms/distance.hpp>

#include <boost/geometry/strategies/intersection.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace overlay
{

/*!
    \brief Get relative order
    \details Can indicate which of two segments R and S,
        both crossing a common segment P, comes first.
        If the two segments cross P very close (e.g. in a spike),
        the distance between the intersection points can be zero,
        but we still need to know which comes first.
        Therefore, it is useful that using sides we are able to discover this.
 */
template <typename Point1>
struct get_relative_order
{
    typedef strategy_intersection
        <
            typename cs_tag<Point1>::type,
            Point1,
            Point1,
            Point1
        > si;

    typedef typename si::side_strategy_type strategy;

    template <typename Point>
    static inline int value_via_product(Point const& ti, Point const& tj,
                    Point const& ui, Point const& uj, int factor)
    {
        int const side_ti_u = strategy::apply(ti, tj, ui);
        int const side_tj_u = strategy::apply(ti, tj, uj);

#ifdef BOOST_GEOMETRY_DEBUG_RELATIVE_ORDER
        std::cout << (factor == 1  ? " r//s " :  " s//r ")
            << side_ti_u << " / " << side_tj_u;
#endif

        return side_ti_u * side_tj_u >= 0
            ? factor * (side_ti_u != 0 ? side_ti_u : side_tj_u)
            : 0;
    }

    static inline int apply(
                Point1 const& pi, Point1 const& pj,
                Point1 const& ri, Point1 const& rj,
                Point1 const& si, Point1 const& sj)
    {
        int const side_ri_p = strategy::apply(pi, pj, ri);
        int const side_si_p = strategy::apply(pi, pj, si);

#ifdef BOOST_GEOMETRY_DEBUG_RELATIVE_ORDER
        int const side_rj_p = strategy::apply(pi, pj, rj);
        int const side_sj_p = strategy::apply(pi, pj, sj);
        std::cout << "r//p: " << side_ri_p << " / " << side_rj_p;
        std::cout << " s//p: " << side_si_p << " / " << side_sj_p;
#endif

        int value = value_via_product(si, sj, ri, rj, 1);
        if (value == 0)
        {
            value = value_via_product(ri, rj, si, sj, -1);
        }

        int const order = side_ri_p * side_ri_p * side_si_p * value;

#ifdef BOOST_GEOMETRY_DEBUG_RELATIVE_ORDER
        std::cout
            << " o: " << order
            << std::endl << std::endl;
#endif

        return order;
    }
};

}} // namespace detail::overlay
#endif //DOXYGEN_NO_DETAIL

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_OVERLAY_GET_RELATIVE_ORDER_HPP


/*! \file	calib.hpp
 *  \brief	Declarations for the CALIBRATOR node.
*/

#ifndef THERMALVIS_CALIB_HPP
#define THERMALVIS_CALIB_HPP

#include "calibrator_config.hpp"

#include "core/tools.hpp"
#include "core/program.hpp"

#ifdef _BUILD_FOR_ROS_
#include "calibratorConfig.h"
#include "core/ros_resources.hpp"
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <dynamic_reconfigure/server.h>
#endif

#include "core/general_resources.hpp"

#include <signal.h>

#include "core/launch.hpp"
#include "core/improc.hpp"
#include "core/features.hpp"
#include "calibrator/calibration.hpp"
#include "calibrator/intrinsics.hpp"
#include "calibrator/extrinsics.hpp"

#ifdef _USE_BOOST_
#include <boost/filesystem.hpp>
#include <boost/thread/thread.hpp>
#ifndef _BUILD_FOR_ROS_
#include <boost/algorithm/string/replace.hpp>
#endif
#endif

#include <opencv/cv.h>
#include <opencv/highgui.h>
#include "opencv2/calib3d/calib3d.hpp"

#define DEFAULT_TIMER_PERIOD	0.05

//HGH
#define PATTERN_CODE_INVALID -1
#define PATTERN_CODE_FIND_TRACK 0
#define PATTERN_CODE_TRACK_FIND 1
#define PATTERN_CODE_FIND 2

const char __PROGRAM__[] = "thermalvis-calibrator";

/// \brief		Stores configuration information for the streamer/image-processing routine
class calibratorData : public calibratorSharedData, public commonData, public calibratorLaunchOnlyData {
	friend class xmlParameters;
	friend class calibratorNode;
#ifndef _BUILD_FOR_ROS_
	friend class calibratorConfig;
#endif

protected:
	// ...

public:
	// Camera models
	cv::Mat K[MAX_ALLOWABLE_CAMS];
	cv::Mat distCoeffs[MAX_ALLOWABLE_CAMS];

	string read_addr;

	calibratorData();

	bool assignFromXml(xmlParameters& xP);

#ifdef _BUILD_FOR_ROS_
	bool obtainStartingData(ros::NodeHandle& nh);
#endif
	bool processStartingData();
};

#ifndef _BUILD_FOR_ROS_
/// \brief		Substitute for ROS live configuration adjustments
class calibratorConfig : public calibratorRealtimeData {
public:
	calibratorConfig() { }
	bool assignStartingData(calibratorData& startupData);

};
#endif

/// \brief		Critical frame data regarding whether it contains a pattern valid for use in calibration
struct validPattern {
    int  validFrameID;
    int  patternID;
    bool patternFound;
    bool isValidFrame;
};

// example: http://ua-ros-pkg.googlecode.com/svn/trunk/arrg/ua_vision/object_tracking/src/object_tracker_node.cpp
/// \brief		Manages the calibration procedures
class calibratorNode : public GenericOptions {
private:

#ifdef _BUILD_FOR_ROS_
	ros::Timer timer;
	image_transport::CameraPublisher debug_pub[MAX_ALLOWABLE_CAMS];
	image_transport::CameraSubscriber camera_sub[MAX_ALLOWABLE_CAMS];
	cv_bridge::CvImagePtr cv_ptr[MAX_ALLOWABLE_CAMS];
	sensor_msgs::Image msg_debug[MAX_ALLOWABLE_CAMS];
	ros::NodeHandle private_node_handle;
	dynamic_reconfigure::Server<thermalvis::calibratorConfig> server;
	dynamic_reconfigure::Server<thermalvis::calibratorConfig>::CallbackType f;
#else
	cv::Mat cv_ptr[MAX_ALLOWABLE_CAMS];
#endif

	cv::Mat prevMat[MAX_ALLOWABLE_CAMS], grayMat[MAX_ALLOWABLE_CAMS];

	struct timeval cycle_timer;
	double elapsedTime;
	double avgTime;

	struct timeval tracking_timer;
	double elapsedTrackingTime;
	double frameTrackingTime;

	unsigned int validSets;

	struct timeval vacant_timer, elapsed_timer;
	double vacantInputTime, elapsedInputTime;

	Mat map1[MAX_ALLOWABLE_CAMS], map2[MAX_ALLOWABLE_CAMS];
	int topValidHeight, botValidHeight, leftValid[MAX_ALLOWABLE_CAMS], rightValid[MAX_ALLOWABLE_CAMS];

	vector<unsigned int> patternIndices[MAX_ALLOWABLE_CAMS];
	vector<Mat> displayImages[MAX_ALLOWABLE_CAMS];

	vector<ros::Time> times[MAX_ALLOWABLE_CAMS];

	vector<unsigned int> validPairs[MAX_ALLOWABLE_CAMS];

	bool infoProcessed[MAX_ALLOWABLE_CAMS];
	bool stillCollecting;

	double alpha;

	Mat default_R;

	Mat cameraMatrices[MAX_ALLOWABLE_CAMS];
	Mat distCoeffVecs[MAX_ALLOWABLE_CAMS];

	Rect roi[MAX_ALLOWABLE_CAMS];
	vector<Point2f> rectangleBounds, newRecBounds;
	Mat rectCamMat[MAX_ALLOWABLE_CAMS];
	vector<Point2f> leftLinePoints, rightLinePoints;

	Size imSize[MAX_ALLOWABLE_CAMS];
	Mat newCamMat[MAX_ALLOWABLE_CAMS];

	double reprojectionError_intrinsics[MAX_ALLOWABLE_CAMS];
	double extendedReprojectionError_intrinsics[MAX_ALLOWABLE_CAMS];

	vector<unsigned char> duplicateFlags[MAX_ALLOWABLE_CAMS];

	vector<ros::Time> patternTimestamps[MAX_ALLOWABLE_CAMS];

	vector<int> subselectedTags_intrinsics[MAX_ALLOWABLE_CAMS];

	string topic[MAX_ALLOWABLE_CAMS];

	unsigned int publishCount;

	// Extrinsics:
	Mat R_[MAX_ALLOWABLE_CAMS], P_[MAX_ALLOWABLE_CAMS];
	Mat E[MAX_ALLOWABLE_CAMS], F[MAX_ALLOWABLE_CAMS], Q[MAX_ALLOWABLE_CAMS];      // Between first camera and all other cameras
	Mat R[MAX_ALLOWABLE_CAMS], Rv[MAX_ALLOWABLE_CAMS], T[MAX_ALLOWABLE_CAMS];  // Rotations/translations between first camera and all other cameras
	Mat R2[MAX_ALLOWABLE_CAMS], T2[MAX_ALLOWABLE_CAMS];       // Rotations/translations between all other cameras

	double stereoError;
	double extendedExtrinsicReprojectionError;

	bool alphaChanged;

	calibratorData configData;

	sensor_msgs::CameraInfo debug_camera_info[MAX_ALLOWABLE_CAMS];

	char debug_pub_name[MAX_ALLOWABLE_CAMS][256];

	char nodeName[256];

	Mat newImage, lastImage;

	unsigned int checkIndex[MAX_ALLOWABLE_CAMS];
	unsigned int frameCount[MAX_ALLOWABLE_CAMS], undistortionCount, rectificationCount, totalFrameCount[2];

	std::vector<cv::Point3f> row;

	vector<vector<Point2f> > pointSets[MAX_ALLOWABLE_CAMS], candidateSets[MAX_ALLOWABLE_CAMS], testingSets[MAX_ALLOWABLE_CAMS];
	vector<vector<Point2f> > extrinsicsPointSets[MAX_ALLOWABLE_CAMS], extrinsicCandidateSets[MAX_ALLOWABLE_CAMS], extrinsicTestingSets[MAX_ALLOWABLE_CAMS];

    //HGH
    Mat lastMat[MAX_ALLOWABLE_CAMS];
    vector<Point2f> cornerSets[MAX_ALLOWABLE_CAMS];
    vector<unsigned int> frameCounts[MAX_ALLOWABLE_CAMS];
    bool  doVerify;

#ifdef _USE_BOOST_
	boost::shared_mutex _access;
#endif

	bool readyForOutput;

	vector<Point2f> cornerSet[MAX_ALLOWABLE_CAMS];

    //HGH
    void preprocessImage(Mat src, Mat &dst, double a, double b, bool normaliz, bool negative );

public:
	///brief	Constructor. Must receive configuration data.
#ifdef _BUILD_FOR_ROS_
	calibratorNode(ros::NodeHandle& nh, calibratorData startupData);
#else
	calibratorNode(calibratorData startupData);
#endif

	void performIntrinsicCalibration();
	void performExtrinsicCalibration();

	///brief	Initial receipt of an image WITHOUT camera info.
#ifdef _BUILD_FOR_ROS_
	void handle_camera(const sensor_msgs::ImageConstPtr& msg_ptr, const sensor_msgs::CameraInfoConstPtr& info_msg, const unsigned int camera_index = 0);
#else
	void handle_camera(const cv::Mat& inputImage, const sensor_msgs::CameraInfo *info_msg, const unsigned int camera_index = 0);
#endif

	void create_virtual_pairs();

	void assignDebugCameraInfo();

//#ifdef _BUILD_FOR_ROS_
//	void publishUndistorted(const ros::TimerEvent& event);
//#else
	void publishUndistorted();
//#endif

//#ifdef _BUILD_FOR_ROS_
//	void publishRectified(const ros::TimerEvent& event);
//#else
	void publishRectified();
//#endif

	void startUndistortionPublishing();

	void startRectificationPublishing();

	bool isStillCollecting();
	bool wantsToUndistort();
	bool wantsToRectify();
	void writeResults();
	bool wantsIntrinsics();
	bool wantsExtrinsics();
        void updatePairs();

        //HGH
        void determineValidPairs();
        void evaluateFrames();

	void getAverageTime();
	void getFrameTrackingTime();
	void assignIntrinsics();

	void preparePatternSubsets();
	void prepareExtrinsicPatternSubsets();

	///brief	Processes online changes in node configuration and applies them.
#ifdef _BUILD_FOR_ROS_
	void serverCallback(thermalvis::calibratorConfig &config, uint32_t level);
#else
	void serverCallback(calibratorConfig &config);
#endif

	void updateMap();

	bool findPattern(const Mat& im, vector<Point2f>& dst, Mat& prev, const int cameraNumber);

	void updateIntrinsicMap(unsigned int idx);
	void set_ready_for_output();

	void prepareForTermination();

        //HGH
        bool isVerifying();

	///brief	Periodically checks for some things...
#ifdef _BUILD_FOR_ROS_
    void timerCallback(const ros::TimerEvent& e);
#else
	void loopCallback();
#endif

};

//boost::shared_ptr < calibratorNode > *globalNodePtr;

#endif


/*
 * MHSamplingWeightFunction.cpp
 *
 *  Created on: Jul 30, 2012
 *      Author: jchen
 */

#include "MHSamplingWeightFunction.h"

/*
 * MHSamplingWeightFunction.cc
 *
 *  Created on: Apr 26, 2012
 *      Author: jchen
 */

#include "MHSamplingWeightFunction.h"
#include "patNBParameters.h"
#include <boost/algorithm/string.hpp>
#include "patNetworkBase.h"
#include "patRouter.h"
#include "patNode.h"
#include "patMultiModalPath.h"
#include  <math.h>
using namespace boost::algorithm;

MHSamplingWeightFunction::MHSamplingWeightFunction(
		const map<ARC_ATTRIBUTES_TYPES, double>& link_coef,
		const double &link_scale, const double &ps_scale,
		const double obs_scale) :
		patLinkAndPathCost::patLinkAndPathCost(link_coef, link_scale, ps_scale), m_obs_scale(
				obs_scale), m_path_probas(NULL), m_function_type(
				patNBParameters::the()->mh_function_type) {

}

MHSamplingWeightFunction* MHSamplingWeightFunction::clone() const {
	return new MHSamplingWeightFunction(*this);
}
MHSamplingWeightFunction::MHSamplingWeightFunction(
		const MHSamplingWeightFunction& another) :
		patLinkAndPathCost::patLinkAndPathCost(another), m_function_type(
				another.m_function_type) {
	m_path_probas = another.m_path_probas;
	m_obs_scale = another.m_obs_scale;

}
MHSamplingWeightFunction::~MHSamplingWeightFunction() {

}

double MHSamplingWeightFunction::computeObsWeight(
		const patMultiModalPath& path) const {

	double obs_weight = 0.0;
	if (m_path_probas != NULL) {

//		cout << m_path_probas->size() << endl;
		map<patMultiModalPath, double>::const_iterator find_path =
				m_path_probas->find(path);
		if (find_path != m_path_probas->end()) {
//			cout << m_obs_scale<<"*"<<find_path->second<<endl;

			if (m_function_type == 1) {
				obs_weight = log(1.0 + m_obs_scale * find_path->second); //option 1
			} else if (m_function_type == 2) {
				obs_weight = m_obs_scale * find_path->second; //option 2
			} else {
				throw RuntimeException("Wrong  mh function type");
			}

		}
	}
//	cout << "obs weight: " << obs_weight << endl;

	return obs_weight;
}
double MHSamplingWeightFunction::logWeigthOriginal(
		const patMultiModalPath& path) const {

	double pathCost = getCost(path);

	pathCost += computeObsWeight(path);
	return pathCost; // - this.nodeLoopScale * nodeLoopCnt);
}

void MHSamplingWeightFunction::setPathProbas(
		const map<const patMultiModalPath, double>* path_probas) {
	m_path_probas = path_probas;
}

double MHSamplingWeightFunction::getObsWeightScale() const {
	return m_obs_scale;
}

patMultiModalPath MHSamplingWeightFunction::getMostLikelyPath(const patMultiModalPath& sp_path) const {

	double highest_proba = 0.0;
	patMultiModalPath highest_path;

	double average_proba = 0.0;
	double average_cost = 0.0;

	if (m_path_probas != NULL) {
		for (map<patMultiModalPath, double>::const_iterator path_iter =
				m_path_probas->begin(); path_iter != m_path_probas->end();
				++path_iter) {
			if (path_iter->first.getUpNode() == sp_path.getUpNode()
					&& path_iter->first.getDownNode()
							== sp_path.getDownNode()) {
				average_proba += path_iter->second;
				average_cost += getCost(path_iter->first);
//			cout<<"path proba: "<<path_iter->second<<endl;
				if (path_iter->second > highest_proba) {
					highest_proba = path_iter->second;
					highest_path = path_iter->first;
				}
			}
		}
	}
	return highest_path;
}
double MHSamplingWeightFunction::calculateObsScale(
		const patMultiModalPath& sp_path) {

	if (patNBParameters::the()->mh_link_scale_relative == 1) {
		double length_coef = -log(2.0)
				/ ((m_link_cost_scale - 1.0) * sp_path.getLength());
		m_link_cost_scale = length_coef / m_link_coefficients[ENUM_LENGTH];
//		m_link_coefficients[ENUM_LENGTH]=length_coef;
		cout << "MHSamplingWeightFunction: new link cost scale"
				<< m_link_cost_scale << endl;
	}
	if (m_path_probas != NULL && patNBParameters::the()->mh_obs_scale>0) {
		cout << "calibrate obs scale" << endl;
		cout << "path proba size" << m_path_probas->size() << endl;
		double sp_proba = 0.0;
		map<patMultiModalPath, double>::const_iterator find_path =
				m_path_probas->find(sp_path);
		if (find_path != m_path_probas->end()) {
			sp_proba = find_path->second;

		}

		double highest_proba = 0.0;
		patMultiModalPath highest_path;

		double average_proba = 0.0;
		double average_cost = 0.0;
		for (map<patMultiModalPath, double>::const_iterator path_iter =
				m_path_probas->begin(); path_iter != m_path_probas->end();
				++path_iter) {
			if (path_iter->first.getUpNode() == sp_path.getUpNode()
					&& path_iter->first.getDownNode()
							== sp_path.getDownNode()) {
				average_proba += path_iter->second;
				average_cost += getCost(path_iter->first);
//			cout<<"path proba: "<<path_iter->second<<endl;
				if (path_iter->second > highest_proba) {
					highest_proba = path_iter->second;
					highest_path = path_iter->first;
				}
			}
		}
		average_proba /= (double) m_path_probas->size();
		average_cost /= (double) m_path_probas->size();
		double sp_utility = getCost(sp_path);
		double highest_utility = getCost(highest_path);
		if (sp_utility == highest_utility) {
//			m_obs_scale = 0.0;
		} else {
			if (m_function_type == 1) {
				m_obs_scale = patNBParameters::the()->mh_obs_scale
						* (exp(sp_utility - highest_utility) - 1.0)
						/ highest_proba; //option 1
			} else if (m_function_type == 2) {
				m_obs_scale = patNBParameters::the()->mh_obs_scale
						* (sp_utility - highest_utility)
						/ (highest_proba - sp_proba); //option 2
//			m_obs_scale = patNBParameters::the()->mh_obs_scale
//					* (sp_utility - average_cost) / (average_proba - sp_proba); //option 2 average cost
			} else {
				throw RuntimeException("Wrong  mh function type");
			}
		}
		cout << sp_utility << "," << highest_utility << "," << highest_proba
				<< "," << sp_proba << endl;
		cout << "obs scale:" << m_obs_scale << endl;
		if (!isfinite(m_obs_scale)) {
			throw RuntimeException("inifinit obs scale");
		}
		if (m_obs_scale < 0.0) {
			cout << "negative obs scale, set to zero istead" << endl;
			m_obs_scale = 0.0;
		}
		cout << "log weight for shortest:" << logWeigthOriginal(sp_path)
				<< ", most likely:" << logWeigthOriginal(highest_path) << endl;
	}
	return m_obs_scale;
}
void MHSamplingWeightFunction::calibrate(const patMultiModalPath& path) {
	calculateObsScale(path);
}


#pragma once

#include <string>

#include <Eigen/Core>
#include <opencv2/core/core.hpp>

#include <tf/transform_broadcaster.h>

#include "rcvio/tracker.hpp"
#include "rcvio/updater.hpp"
#include "rcvio/input_buffer.hpp"
#include "rcvio/pre_integrator.hpp"

namespace rcvio
{
    class System
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        System(const std::string &settings_file);

        inline void pushImuData(ImuData *data) { input_buffer_->pushImuData(data); }
        inline void pushImageData(ImageData *data) { input_buffer_->pushImageData(data); }

        void initialize(const Eigen::Vector3d &w, const Eigen::Vector3d &a,
                        const int imu_data, const bool enable_alignment);

        void monoVIO();

    private:
        double imu_rate_;
        double cam_time_offset_;

        int sliding_window_size_;
        int min_clone_states_;

        bool enable_alignment_;
        bool record_outputs_;

        bool is_moving_;
        bool is_ready_;

        double threshold_angle_;
        double threshold_displacement_;

        double gravity_;

        double sigma_gyro_noise_;
        double sigma_gyro_bias_;
        double sigma_accel_noise_;
        double sigma_accel_bias_;

        Eigen::VectorXd xkk;
        Eigen::MatrixXd Pkk;

        Tracker::Ptr tracker_;
        Updater::Ptr updater_;
        InputBuffer::Ptr input_buffer_;
        PreIntegrator::Ptr pre_integrator_;

        ros::NodeHandle system_node_;
        ros::Publisher path_publish_;
        ros::Publisher odometry_publish_;
        tf::TransformBroadcaster tf_publish_;
    };
}


#include <iostream>
#include <boost/log/trivial.hpp>

#include "tut/ch1.hpp"

namespace tut {
  namespace ch1 {

void exe_1_1() {
  std::cout << "Welcome to c++" << std::endl;
  std::cout << "Welcome to computer science " << std::endl;
  std::cout << "Programming is fun " << std::endl;
}

void exe_1_2(){
  std::cout << "Welcome to c++" << std::endl;
  std::cout << "Welcome to c++" << std::endl;
  std::cout << "Welcome to c++" << std::endl;
  std::cout << "Welcome to c++" << std::endl;
  std::cout << "Welcome to c++" << std::endl;
}

void exe_1_3(){
  std::cout << "      CCCC       " << std::endl;
  std::cout << "     C           " << std::endl;
  std::cout << "     C      + +  " << std::endl;
  std::cout << "     C           " << std::endl;
  std::cout << "       CCCC      " << std::endl;
}

void exe_1_4() {
  std::cout << "a     a^2     a^3  " << std::endl;
  std::cout << "1     1       1    " << std::endl;
  std::cout << "2     4       8    " << std::endl;
  std::cout << "3     9       27   " << std::endl;
  std::cout << "4     16      64   " << std::endl;
}

void exe_1_5() {
  std::cout << (9.5 * 4.5) - (2.5 * 3) / (45.5 - 3.5) << std::endl;
}

void exe_1_6() {
  std::cout << 1 + 2 + 3 + 4 + 5 + 6 + 7 + 8 + 9 << std::endl;
}

int exe_1_7() { // BODMAS
  double x {4.0 * ( 1.0 - ( (1.0/3.0) + (1.0/5.0) ) - ( (1.0/7.0) + (1.0/9.0) ) - (1.0/11.0) )};
  double y {4.0 * ( 1.0 - ( (1.0/3.0) + (1.0/5.0) ) - ( (1.0/7.0) + (1.0/9.0) ) - ( (1.0/11.0) + (1.0/13.0) ))};
  BOOST_LOG_TRIVIAL(debug) << "Aprox val of pie: x=" << x << " : y ="<< y<< ": "<< x * y;
 return x;
}

  }

}


/* BestFit.cpp
 * -*- coding: utf-8 -*-
 *
 */

#include <boost/test/unit_test.hpp>

// system includes =============================================================

#include <string>
#include <vector>
#include <memory>
#include <csv.h>

// local includes ==============================================================

#include "../src/DCD.hpp"
#include "../src/ProteinFile.hpp"
#include "../src/SecondaryStructureFile.hpp"
#include "../src/Frame.hpp"
#include "../src/Protein.hpp"
#include "../src/ProteinSegment.hpp"
#include "../src/FrameSegment.hpp"

#include "utils/type.hpp"
#include "utils/log.hpp"
#include "utils/definitions.hpp"

BOOST_AUTO_TEST_SUITE(best_fit)

BOOST_AUTO_TEST_CASE(bstft_complete_protein) {
    TEST_MESSAGE("bstft_complete_protein");

    DCD trajectory("test/resources/mb_ca.dcd");
    std::shared_ptr<PDB> pdb(new PDB("test/resources/integration/mb_ca.pdb"));
    Protein protein(pdb);
    std::shared_ptr<ProteinSegment> protein_segment(new ProteinSegment(protein.get_residues(), 1, 151, COMPLETE_PROTEIN));
    FrameSegment frame_segment(protein_segment);

    for(int i = 1; i <= 2000; i++) {
        Frame frame = trajectory.get_next_frame();
        frame_segment.set_frame(frame);
        Eigen::VectorXd displacement_vector = frame_segment.fit_to_reference();

        std::stringstream ss;
        ss << "test/resources/bstft/mb_ca_bstft_frame";
        ss << std::setfill('0') << std::setw(4) << i;
        ss << "_fit_hex.csv";
        io::CSVReader<3> reach_fit_data(ss.str());
        reach_fit_data.read_header(io::ignore_extra_column,"x1_hex","x2_hex","x3_hex");
        std::string x1,x2,x3;

        for (size_t k=0; reach_fit_data.read_row(x1,x2,x3); ++k) {
            BOOST_REQUIRE_CLOSE_FRACTION(cast_hex_to_double(x1), displacement_vector(3*k), PRECISION);
            BOOST_REQUIRE_CLOSE_FRACTION(cast_hex_to_double(x2), displacement_vector(3*k+1), PRECISION);
            BOOST_REQUIRE_CLOSE_FRACTION(cast_hex_to_double(x3), displacement_vector(3*k+2), PRECISION);
        }
    }
}

BOOST_AUTO_TEST_SUITE_END()

// vim: tabstop=8 expandtab shiftwidth=4 softtabstop=4 fenc=utf-8