/************************************************************************
 *                                                                      *
 * Copyright (C) 2012 OVSM/IPGP                                         *
 *                                                                      *
 * This program is free software: you can redistribute it and/or modify *
 * it under the terms of the GNU General Public License as published by *
 * the Free Software Foundation, either version 3 of the License, or    *
 * (at your option) any later version.                                  *
 *                                                                      *
 * This program is distributed in the hope that it will be useful,      *
 * but WITHOUT ANY WARRANTY; without even the implied warranty of       *
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the        *
 * GNU General Public License for more details.                         *
 *                                                                      *
 * This program is part of 'Projet TSUAREG - INTERREG IV Caraïbes'.     *
 * It has been co-financed by the European Union and le Ministère de    *
 * l'Ecologie, du Développement Durable, des Transports et du Logement. *
 *                                                                      *
 ************************************************************************/

#define SEISCOMP_COMPONENT Md

#include "md.h"
#include "l4c1hz.h"
#include <seiscomp3/processing/waveformprocessor.h>
#include <seiscomp3/math/filter/stalta.h>
#include <seiscomp3/math/filter/iirfilter.h>
#include <seiscomp3/math/filter/butterworth.h>
#include <seiscomp3/logging/log.h>
#include <seiscomp3/core/strings.h>
#include <seiscomp3/seismology/magnitudes.h>
#include <seiscomp3/math/mean.h>
#include <seiscomp3/math/filter/seismometers.h>
#include <seiscomp3/math/restitution/fft.h>
#include <seiscomp3/math/geo.h>
#include <iostream>
#include <boost/bind.hpp>
#include <unistd.h>

#define _DEPTH_MAX 200.0
#define _SIGNAL_WINDOW_END 30.0
#define _LINEAR_CORRECTION 1.0
#define _OFFSET 0.0
#define _SNR_MIN 1.2
#define _DELTA_MAX 400.0
#define _MD_MAX 5.0
#define _FMA -0.87
#define _FMB 2.0
#define _FMD 0.0035
#define _FMF 0.0
#define _FMZ 0.0
#define _STACOR 0.0 //! not fully implemented !
/**
 * Seismometer selection
 * 1 for Wood-Anderson
 * 2 for Seismometer 5 sec
 * 3 for WWSSN LP ? filter
 * 4 for WWSSN SP? filter
 * 5 for Generic Seismometer ? filter
 * 6 for Butterworth Low Pass ? filter
 * 7 for Butterwoth High Pass ? filter
 * 8 for Butterworth Band Pass ? filter
 * 9 for L4C 1Hz seismometer
 **/
#define _SEISMO 9
#define _BUTTERWORTH ""

ADD_SC_PLUGIN("Md duration magnitude plugin", "IPGP <www.ipgp.fr>", 0, 1, 1)

#define AMPTAG "[Amp] [Md]"
#define MAGTAG "[Mag] [Md]"

using namespace Seiscomp;
using namespace Seiscomp::Math;
using namespace Seiscomp::Processing;

/*----[ AMPLITUDE PROCESSOR CLASS ]----*/

IMPLEMENT_SC_CLASS_DERIVED(AmplitudeProcessor_Md, AmplitudeProcessor, "AmplitudeProcessor_Md");
REGISTER_AMPLITUDEPROCESSOR(AmplitudeProcessor_Md, "Md");

struct ampConfig {

		double DEPTH_MAX;
		double SIGNAL_WINDOW_END;
		double SNR_MIN;
		double DELTA_MAX;
		double MD_MAX;
		double FMA;
		double FMB;
		double FMD;
		double FMF;
		double FMZ;
		double STACOR;
		int SEISMO;
		std::string BUTTERWORTH;
};
ampConfig aFile;

// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
AmplitudeProcessor_Md::AmplitudeProcessor_Md() :
		AmplitudeProcessor("Md") {

	setSignalStart(0.);
	setSignalEnd(150.);
	setMinSNR(aFile.SNR_MIN);
	setMaxDist(8);
	_computeAbsMax = true;
	_isInitialized = false;
}
// <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
AmplitudeProcessor_Md::AmplitudeProcessor_Md(const Core::Time& trigger) :
		AmplitudeProcessor(trigger, "Md") {

	setSignalStart(0.);
	setSignalEnd(150.);
	setMinSNR(aFile.SNR_MIN);
	setMaxDist(8);
	_computeAbsMax = true;
	_isInitialized = false;

	computeTimeWindow();
}
// <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
bool AmplitudeProcessor_Md::setup(const Settings& settings) {

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

	bool isButterworth = false;
	try {
		aFile.SEISMO = settings.getInt("md.seismo");
		std::string type;
		switch ( aFile.SEISMO ) {
			case 1:
				type = "WoodAnderson";
			break;
			case 2:
				type = "Seismo5sec";
			break;
			case 3:
				type = "WWSSN LP";
			break;
			case 4:
				type = "WWSSN SP";
			break;
			case 5:
				type = "Generic Seismometer";
			break;
			case 6:
				type = "Butterworth Low Pass";
				isButterworth = true;
			break;
			case 7:
				type = "Butterworth High Pass";
				isButterworth = true;
			break;
			case 8:
				type = "Butterworth Band Pass";
				isButterworth = true;
			break;
			case 9:
				type = "L4C 1Hz Seismometer";
			break;
			default:
				break;
		}
		SEISCOMP_DEBUG("%s sets SEISMO to  %s [%s.%s]", AMPTAG, type.c_str(),
		    settings.networkCode.c_str(), settings.stationCode.c_str());
	}
	catch ( ... ) {
		aFile.SEISMO = _SEISMO;
		SEISCOMP_ERROR("%s can not read SEISMO value from configuration file [%s.%s]", AMPTAG,
		    settings.networkCode.c_str(), settings.stationCode.c_str());
	}

	if ( isButterworth == true ) {
		try {
			aFile.BUTTERWORTH = settings.getString("md.butterworth");
			SEISCOMP_DEBUG("%s sets Butterworth filter to  %s [%s.%s]", AMPTAG,
			    aFile.BUTTERWORTH.c_str(), settings.networkCode.c_str(),
			    settings.stationCode.c_str());
		}
		catch ( ... ) {
			aFile.BUTTERWORTH = _BUTTERWORTH;
			SEISCOMP_ERROR("%s can not read Butterworth filter value from configuration file [%s.%s]", AMPTAG,
			    settings.networkCode.c_str(), settings.stationCode.c_str());
		}
	}

	try {
		aFile.DEPTH_MAX = settings.getDouble("md.depthmax");
		SEISCOMP_DEBUG("%s sets DEPTH MAX to  %.2f [%s.%s]", AMPTAG, aFile.DEPTH_MAX,
		    settings.networkCode.c_str(), settings.stationCode.c_str());
	}
	catch ( ... ) {
		aFile.DEPTH_MAX = _DEPTH_MAX;
		SEISCOMP_ERROR("%s can not read DEPTH MAX value from configuration file [%s.%s]", AMPTAG,
		    settings.networkCode.c_str(), settings.stationCode.c_str());
	}

	try {
		aFile.DELTA_MAX = settings.getDouble("md.deltamax");
		SEISCOMP_DEBUG("%s sets DELTA MAX to  %.2f [%s.%s]", AMPTAG, aFile.DELTA_MAX,
		    settings.networkCode.c_str(), settings.stationCode.c_str());
	}
	catch ( ... ) {
		aFile.DELTA_MAX = _DELTA_MAX;
		SEISCOMP_ERROR("%s can not read DELTA MAX value from configuration file [%s.%s]", AMPTAG,
		    settings.networkCode.c_str(), settings.stationCode.c_str());
	}

	try {
		aFile.SNR_MIN = settings.getDouble("md.snrmin");
		SEISCOMP_DEBUG("%s sets SNR MIN to  %.2f [%s.%s]", AMPTAG, aFile.SNR_MIN,
		    settings.networkCode.c_str(), settings.stationCode.c_str());
	}
	catch ( ... ) {
		aFile.SNR_MIN = _SNR_MIN;
		SEISCOMP_ERROR("%s can not read SNR MIN value from configuration file [%s.%s]", AMPTAG,
		    settings.networkCode.c_str(), settings.stationCode.c_str());
	}

	try {
		aFile.MD_MAX = settings.getDouble("md.mdmax");
		SEISCOMP_DEBUG("%s sets MD MAX to  %.2f [%s.%s]", AMPTAG, aFile.MD_MAX,
		    settings.networkCode.c_str(), settings.stationCode.c_str());
	}
	catch ( ... ) {
		aFile.MD_MAX = _MD_MAX;
		SEISCOMP_ERROR("%s can not read MD MAX value from configuration file [%s.%s]", AMPTAG,
		    settings.networkCode.c_str(), settings.stationCode.c_str());
	}

	try {
		aFile.FMA = settings.getDouble("md.fma");
		SEISCOMP_DEBUG("%s sets FMA to  %.4f [%s.%s]", AMPTAG, aFile.FMA,
		    settings.networkCode.c_str(), settings.stationCode.c_str());
	}
	catch ( ... ) {
		aFile.FMA = _FMA;
		SEISCOMP_ERROR("%s can not read FMA value from configuration file [%s.%s]", AMPTAG,
		    settings.networkCode.c_str(), settings.stationCode.c_str());
	}

	try {
		aFile.FMB = settings.getDouble("md.fmb");
		SEISCOMP_DEBUG("%s sets FMB to  %.4f [%s.%s]", AMPTAG, aFile.FMB,
		    settings.networkCode.c_str(), settings.stationCode.c_str());
	}
	catch ( ... ) {
		aFile.FMB = _FMB;
		SEISCOMP_ERROR("%s can not read FMB value from configuration file [%s.%s]", AMPTAG,
		    settings.networkCode.c_str(), settings.stationCode.c_str());
	}

	try {
		aFile.FMD = settings.getDouble("md.fmd");
		SEISCOMP_DEBUG("%s sets FMD to  %.4f [%s.%s]", AMPTAG, aFile.FMD,
		    settings.networkCode.c_str(), settings.stationCode.c_str());
	}
	catch ( ... ) {
		aFile.FMD = _FMD;
		SEISCOMP_ERROR("%s can not read FMD value from configuration file [%s.%s]", AMPTAG,
		    settings.networkCode.c_str(), settings.stationCode.c_str());
	}

	try {
		aFile.FMF = settings.getDouble("md.fmf");
		SEISCOMP_DEBUG("%s sets FMF to  %.4f [%s.%s]", AMPTAG, aFile.FMF,
		    settings.networkCode.c_str(), settings.stationCode.c_str());
	}
	catch ( ... ) {
		aFile.FMF = _FMF;
		SEISCOMP_ERROR("%s can not read FMF value from configuration file [%s.%s]", AMPTAG,
		    settings.networkCode.c_str(), settings.stationCode.c_str());
	}

	try {
		aFile.FMZ = settings.getDouble("md.fmz");
		SEISCOMP_DEBUG("%s sets FMZ to  %.4f [%s.%s]", AMPTAG, aFile.FMZ,
		    settings.networkCode.c_str(), settings.stationCode.c_str());
	}
	catch ( ... ) {
		aFile.FMZ = _FMZ;
		SEISCOMP_ERROR("%s can not read FMZ value from configuration file [%s.%s]",
		    AMPTAG, settings.networkCode.c_str(), settings.stationCode.c_str());
	}

	_isInitialized = true;

	return true;
}
// <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
void AmplitudeProcessor_Md::initFilter(double fsamp) {

	if ( !_enableResponses ) {

		SEISCOMP_DEBUG("Using custom responses");

		Math::Filtering::InPlaceFilter<double>* f;
		switch ( aFile.SEISMO ) {
			case 1:
				AmplitudeProcessor::setFilter(new Filtering::IIR::WoodAndersonFilter<
				        double>(Velocity));
			break;
			case 2:
				AmplitudeProcessor::setFilter(new Filtering::IIR::Seismometer5secFilter<
				        double>(Velocity));
			break;
			case 3:
				AmplitudeProcessor::setFilter(new Filtering::IIR::WWSSN_LP_Filter<
				        double>(Velocity));
			break;
			case 4:
				AmplitudeProcessor::setFilter(new Filtering::IIR::WWSSN_SP_Filter<
				        double>(Velocity));
			break;
			case 5:
				AmplitudeProcessor::setFilter(new Filtering::IIR::GenericSeismometer<
				        double>(Velocity));
			break;
			case 6:
				f = new Math::Filtering::IIR::ButterworthLowpass<double>(3, 1, 15);
				AmplitudeProcessor::setFilter(f);
			break;
			case 7:
				f = new Math::Filtering::IIR::ButterworthHighpass<double>(3, 1, 15);
				AmplitudeProcessor::setFilter(f);
			break;
			case 8:
				f = new Math::Filtering::IIR::ButterworthBandpass<double>(3, 1, 15, 1, true);
				AmplitudeProcessor::setFilter(f);
			break;
			case 9:
				AmplitudeProcessor::setFilter(new Filtering::IIR::L4C_1Hz_Filter<
				        double>(Velocity));
			break;
			default:
				SEISCOMP_ERROR("%s can not initialize the chosen filter, "
					"please review your configuration file", AMPTAG);
			break;
		}
	}
	else
		AmplitudeProcessor::setFilter(NULL);

	AmplitudeProcessor::initFilter(fsamp);
}
// <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
int AmplitudeProcessor_Md::capabilities() const {
	return MeasureType;
}
// <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
AmplitudeProcessor::IDList
AmplitudeProcessor_Md::capabilityParameters(Capability cap) const {

	if ( cap == MeasureType ) {
		IDList params;
		params.push_back("AbsMax");
		params.push_back("MinMax");
		return params;
	}

	return AmplitudeProcessor::capabilityParameters(cap);
}
// <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
bool AmplitudeProcessor_Md::setParameter(Capability cap,
                                         const std::string& value) {

	if ( cap == MeasureType ) {

		if ( value == "AbsMax" ) {
			_computeAbsMax = true;
			return true;
		}
		else if ( value == "MinMax" ) {
			_computeAbsMax = false;
			return true;
		}
		return false;
	}

	return AmplitudeProcessor::setParameter(cap, value);
}
// <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
bool AmplitudeProcessor_Md::deconvolveData(Response* resp,
                                           DoubleArray& data,
                                           int numberOfIntegrations) {
	if ( numberOfIntegrations < -1 )
		return false;

	SEISCOMP_DEBUG("Inside deconvolve function");

	double m, n;
	Math::Restitution::FFT::TransferFunctionPtr tf =
		resp->getTransferFunction(numberOfIntegrations < 0 ? 0 : numberOfIntegrations);

	if ( !tf )
		return false;

	Math::GroundMotion gm;

	if ( numberOfIntegrations < 0 )
		gm = Math::Displacement;
	else
		gm = Math::Velocity;

	Math::Restitution::FFT::TransferFunctionPtr cascade;
	Math::SeismometerResponse::WoodAnderson woodAndersonResp(gm);
	Math::SeismometerResponse::Seismometer5sec seis5sResp(gm);
	Math::SeismometerResponse::L4C_1Hz l4c1hzResp(gm);

	Math::Restitution::FFT::PolesAndZeros woodAnderson(woodAndersonResp);
	Math::Restitution::FFT::PolesAndZeros seis5sec(seis5sResp);
	Math::Restitution::FFT::PolesAndZeros l4c1hz(l4c1hzResp);

	SEISCOMP_DEBUG("SEISMO = %d", aFile.SEISMO);

	switch ( aFile.SEISMO ) {
		case 1:
			cascade = *tf / woodAnderson;
		break;
		case 2:
			cascade = *tf / seis5sec;
		break;
		case 9:
			SEISCOMP_INFO("%s Applying filter L4C 1Hz to data", AMPTAG);
			cascade = *tf / l4c1hz;
		break;
		default:
			cascade = tf;
			SEISCOMP_INFO("%s No seismometer specified, no signal reconvolution performed", AMPTAG);
			return false;
		break;
	}

	// Remove linear trend
	Math::Statistics::computeLinearTrend(data.size(), data.typedData(), m, n);
	Math::Statistics::detrend(data.size(), data.typedData(), m, n);

	return Math::Restitution::transformFFT(data.size(), data.typedData(),
	    _stream.fsamp, cascade.get(), _config.respTaper, _config.respMinFreq,
	    _config.respMaxFreq);
}
// <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
bool AmplitudeProcessor_Md::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) {

	double amax, Imax, ofs_sig, amp_sig;
	DoubleArrayPtr d;

	if ( *snr < aFile.SNR_MIN )
		SEISCOMP_DEBUG("%s computed SNR is under configured SNR MIN", AMPTAG);

	if ( _computeAbsMax ) {
		size_t imax = find_absmax(data.size(), data.typedData(), si1, si2, offset);
		amax = fabs(data[imax] - offset);
		dt->index = imax;
	}
	else {
		int lmin, lmax;
		find_minmax(lmin, lmax, data.size(), data.typedData(), si1, si2, offset);
		amax = data[lmax] - data[lmin];
		dt->index = (lmin + lmax) * 0.5;
		dt->begin = lmin - dt->index;
		dt->end = lmax - dt->index;
	}

	Imax = dt->index;

	SEISCOMP_DEBUG("%s Amplitude max: %.2f", AMPTAG, amax);

	//! searching for Coda second by second through the end of the window
	//! TODO: elevate accuracy by using a nanometers scale (maybe)
	unsigned int i = si1;
	bool hasEndSignal = false;
	double calculatedSnr = -1;

	for (i = (int) Imax; i < i2; i = i + 1 * (int) _stream.fsamp) {

		int window_end = i + 1 * (int) _stream.fsamp;
		d = static_cast<DoubleArray*>(data.slice(i, window_end));

		//! computes pre-arrival offset
		ofs_sig = d->median();

		//! computes rms after removing offset
		amp_sig = 2 * d->rms(ofs_sig);

		if ( amp_sig / *_noiseAmplitude <= aFile.SNR_MIN ) {
			SEISCOMP_DEBUG("%s End of signal found! (%.2f <= %.2f)", AMPTAG,
			    (amp_sig / *_noiseAmplitude), aFile.SNR_MIN);
			hasEndSignal = true;
			calculatedSnr = amp_sig / *_noiseAmplitude;
			break;
		}
	}

	if ( !hasEndSignal ) {
		SEISCOMP_ERROR("%s SNR stayed over configured SNR_MIN! (%.2f > %.2f), "
			"skipping magnitude calculation for this station", AMPTAG,
		    calculatedSnr, aFile.SNR_MIN);
		return false;
	}

	dt->index = i;

	//amplitude->value = 2 * amp_sig; //! actually it would have to be max. peak-to-peak
	amplitude->value = amp_sig;

	if ( _streamConfig[_usedComponent].gain != 0.0 )
		amplitude->value /= _streamConfig[_usedComponent].gain;
	else {
		setStatus(MissingGain, 0.0);
		return false;
	}

	// Convert m/s to nm/s
	amplitude->value *= 1.E09;

	*period = i - i1 + (_config.signalBegin * _stream.fsamp);

	SEISCOMP_DEBUG("%s calculated event amplitude = %.2f", AMPTAG, amplitude->value);
	SEISCOMP_DEBUG("%s calculated signal end at %.2f ms from P phase", AMPTAG, *period);

	return true;
}
// <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
double AmplitudeProcessor_Md::timeWindowLength(double distance_deg) const {

	if ( !_isInitialized ) {

		aFile.MD_MAX = _MD_MAX;
		aFile.FMA = _FMA;
		aFile.FMZ = _FMZ;
		aFile.DEPTH_MAX = _DEPTH_MAX;
		aFile.STACOR = _STACOR;
		aFile.FMD = _FMD;
		aFile.FMB = _FMB;
		aFile.FMF = _FMF;
		aFile.SNR_MIN = _SNR_MIN;
		aFile.DELTA_MAX = _DELTA_MAX;
		aFile.SIGNAL_WINDOW_END = _SIGNAL_WINDOW_END;
		aFile.SEISMO = _SEISMO;
		aFile.BUTTERWORTH = _BUTTERWORTH;
	}

	double distance_km = Math::Geo::deg2km(distance_deg);
	double windowLength = (aFile.MD_MAX - aFile.FMA - (aFile.FMZ * aFile.DEPTH_MAX)
	        - aFile.STACOR - (aFile.FMD * distance_km)) / (aFile.FMB + aFile.FMF);

	windowLength = pow(10, windowLength) + aFile.SIGNAL_WINDOW_END;
	SEISCOMP_DEBUG("%s Requesting stream of %.2fsec for current station", AMPTAG, windowLength);

	return windowLength;
}
// <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

/*----[ END OF AMPLITUDE PROCESSOR CLASS ]----*/

/*----[ MAGNITUDE PROCESSOR CLASS ]----*/

IMPLEMENT_SC_CLASS_DERIVED(MagnitudeProcessor_Md, MagnitudeProcessor, "MagnitudeProcessor_Md");
REGISTER_MAGNITUDEPROCESSOR(MagnitudeProcessor_Md, "Md");

struct magConfig {

		double DEPTH_MAX;
		double LINEAR_CORRECTION;
		double OFFSET;
		double DELTA_MAX;
		double MD_MAX;
		double FMA;
		double FMB;
		double FMD;
		double FMF;
		double FMZ;
		double STACOR;
};
magConfig mFile;

// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
MagnitudeProcessor_Md::MagnitudeProcessor_Md() :
		MagnitudeProcessor("Md") {

	_linearCorrection = mFile.LINEAR_CORRECTION;
	_constantCorrection = mFile.OFFSET;
}
// <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
bool MagnitudeProcessor_Md::setup(const Settings& settings) {

	try {
		mFile.DELTA_MAX = settings.getDouble("md.deltamax");
		SEISCOMP_DEBUG("%s sets DELTA MAX to  %.2f [%s.%s]", MAGTAG, mFile.DELTA_MAX,
		    settings.networkCode.c_str(), settings.stationCode.c_str());
	}
	catch ( ... ) {
		mFile.DELTA_MAX = _DELTA_MAX;
		SEISCOMP_ERROR("%s can not read DELTA MAX value from configuration file [%s.%s]",
		    MAGTAG, settings.networkCode.c_str(), settings.stationCode.c_str());
	}

	try {
		mFile.DEPTH_MAX = settings.getDouble("md.depthmax");
		SEISCOMP_DEBUG("%s sets DEPTH MAX to  %.2f [%s.%s]", MAGTAG, mFile.DEPTH_MAX,
		    settings.networkCode.c_str(), settings.stationCode.c_str());
	}
	catch ( ... ) {
		mFile.DEPTH_MAX = _DEPTH_MAX;
		SEISCOMP_ERROR("%s can not read DEPTH MAX value from configuration file [%s.%s]",
		    MAGTAG, settings.networkCode.c_str(), settings.stationCode.c_str());
	}

	try {
		mFile.MD_MAX = settings.getDouble("md.mdmax");
		SEISCOMP_DEBUG("%s sets MD MAX to  %.2f [%s.%s]", MAGTAG, mFile.MD_MAX,
		    settings.networkCode.c_str(), settings.stationCode.c_str());
	}
	catch ( ... ) {
		mFile.MD_MAX = _MD_MAX;
		SEISCOMP_ERROR("%s can not read MD MAX value from configuration file [%s.%s]",
		    MAGTAG, settings.networkCode.c_str(), settings.stationCode.c_str());
	}

	try {
		mFile.LINEAR_CORRECTION = settings.getDouble("md.linearcorrection");
		SEISCOMP_DEBUG("%s sets LINEAR CORRECTION to  %.2f [%s.%s]", MAGTAG,
		    mFile.LINEAR_CORRECTION, settings.networkCode.c_str(), settings.stationCode.c_str());
	}
	catch ( ... ) {
		mFile.LINEAR_CORRECTION = _LINEAR_CORRECTION;
		SEISCOMP_ERROR("%s can not read LINEAR CORRECTION value from configuration file [%s.%s]",
		    MAGTAG, settings.networkCode.c_str(), settings.stationCode.c_str());
	}

	try {
		mFile.OFFSET = settings.getDouble("md.offset");
		SEISCOMP_DEBUG("%s sets OFFSET to  %.2f [%s.%s]", MAGTAG, mFile.OFFSET,
		    settings.networkCode.c_str(), settings.stationCode.c_str());
	}
	catch ( ... ) {
		mFile.OFFSET = _OFFSET;
		SEISCOMP_ERROR("%s can not read OFFSET value from configuration file [%s.%s]",
		    MAGTAG, settings.networkCode.c_str(), settings.stationCode.c_str());
	}

	try {
		mFile.FMA = settings.getDouble("md.fma");
		SEISCOMP_DEBUG("%s sets FMA to  %.4f [%s.%s]", MAGTAG, mFile.FMA,
		    settings.networkCode.c_str(), settings.stationCode.c_str());
	}
	catch ( ... ) {
		mFile.FMA = _FMA;
		SEISCOMP_ERROR("%s can not read FMA value from configuration file [%s.%s]",
		    MAGTAG, settings.networkCode.c_str(), settings.stationCode.c_str());
	}

	try {
		mFile.FMB = settings.getDouble("md.fmb");
		SEISCOMP_DEBUG("%s sets FMB to  %.4f [%s.%s]", MAGTAG, mFile.FMB,
		    settings.networkCode.c_str(), settings.stationCode.c_str());
	}
	catch ( ... ) {
		mFile.FMB = _FMB;
		SEISCOMP_ERROR("%s can not read FMB value from configuration file [%s.%s]",
		    MAGTAG, settings.networkCode.c_str(), settings.stationCode.c_str());
	}

	try {
		mFile.FMD = settings.getDouble("md.fmd");
		SEISCOMP_DEBUG("%s sets FMD to  %.4f [%s.%s]", MAGTAG, mFile.FMD,
		    settings.networkCode.c_str(), settings.stationCode.c_str());
	}
	catch ( ... ) {
		mFile.FMD = _FMD;
		SEISCOMP_ERROR("%s can not read FMD value from configuration file [%s.%s]",
		    MAGTAG, settings.networkCode.c_str(), settings.stationCode.c_str());
	}

	try {
		mFile.FMF = settings.getDouble("md.fmf");
		SEISCOMP_DEBUG("%s sets FMF to  %.4f [%s.%s]", MAGTAG, mFile.FMF,
		    settings.networkCode.c_str(), settings.stationCode.c_str());
	}
	catch ( ... ) {
		mFile.FMF = _FMF;
		SEISCOMP_ERROR("%s can not read FMF value from configuration file [%s.%s]",
		    MAGTAG, settings.networkCode.c_str(), settings.stationCode.c_str());
	}

	try {
		mFile.FMZ = settings.getDouble("md.fmz");
		SEISCOMP_DEBUG("%s sets FMZ to  %.4f [%s.%s]", MAGTAG, mFile.FMZ,
		    settings.networkCode.c_str(), settings.stationCode.c_str());
	}
	catch ( ... ) {
		mFile.FMZ = _FMZ;
		SEISCOMP_ERROR("%s can not read FMZ value from configuration file [%s.%s]",
		    MAGTAG, settings.networkCode.c_str(), settings.stationCode.c_str());
	}

	try {
		mFile.STACOR = settings.getDouble("md.stacor");
		SEISCOMP_DEBUG("%s sets STACOR to  %.4f [%s.%s]", MAGTAG, mFile.STACOR,
		    settings.networkCode.c_str(), settings.stationCode.c_str());
	}
	catch ( ... ) {
		mFile.STACOR = _STACOR;
		SEISCOMP_ERROR("%s can not read STACOR value from configuration file [%s.%s]",
		    MAGTAG, settings.networkCode.c_str(), settings.stationCode.c_str());
	}

	return true;
}
// <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
MagnitudeProcessor::Status
MagnitudeProcessor_Md::computeMagnitude(double amplitude, double period,
                                        double delta, double depth,
                                        double& value) {

	double epdistkm;
	epdistkm = Math::Geo::deg2km(delta);

	SEISCOMP_DEBUG("%s --------------------------------", MAGTAG);
	SEISCOMP_DEBUG("%s |    PARAMETERS   |    VALUE   |", MAGTAG);
	SEISCOMP_DEBUG("%s --------------------------------", MAGTAG);
	SEISCOMP_DEBUG("%s | delta max       | %.2f ", MAGTAG, mFile.DELTA_MAX);
	SEISCOMP_DEBUG("%s | depth max       | %.2f ", MAGTAG, mFile.DEPTH_MAX);
	SEISCOMP_DEBUG("%s | md max          | %.2f ", MAGTAG, mFile.MD_MAX);
	SEISCOMP_DEBUG("%s | fma             | %.4f ", MAGTAG, mFile.FMA);
	SEISCOMP_DEBUG("%s | fmb             | %.4f ", MAGTAG, mFile.FMB);
	SEISCOMP_DEBUG("%s | fmd             | %.4f ", MAGTAG, mFile.FMD);
	SEISCOMP_DEBUG("%s | fmf             | %.4f ", MAGTAG, mFile.FMF);
	SEISCOMP_DEBUG("%s | fmz             | %.4f ", MAGTAG, mFile.FMZ);
	SEISCOMP_DEBUG("%s | stacor          | %.4f ", MAGTAG, mFile.STACOR);
	SEISCOMP_DEBUG("%s --------------------------------", MAGTAG);
	SEISCOMP_DEBUG("%s | (f-p)           | %.2f sec ", MAGTAG, period);
	SEISCOMP_DEBUG("%s | seismic depth   | %.2f km ", MAGTAG, depth);
	SEISCOMP_DEBUG("%s | epicenter dist  | %.2f km ", MAGTAG, epdistkm);
	SEISCOMP_DEBUG("%s --------------------------------", MAGTAG);

	if ( amplitude <= 0. ) {
		value = 0;
		SEISCOMP_ERROR("%s calculated amplitude is wrong, "
			"no magnitude will be calculated", MAGTAG);
		return Error;
	}

	if ( (mFile.DELTA_MAX) < epdistkm ) {
		SEISCOMP_ERROR("%s epicenter distance is out of configured range, "
			"no magnitude will be calculated", MAGTAG);
		return DistanceOutOfRange;
	}

	value = mFile.FMA + mFile.FMB * log10(period) + (mFile.FMF * period)
	        + (mFile.FMD * epdistkm) + (mFile.FMZ * depth) + mFile.STACOR;

	if ( value > mFile.MD_MAX )
		SEISCOMP_WARNING("%s Calculated magnitude is beyond max Md value [value= %.2f]",
		    MAGTAG, value);

	return OK;
}
// <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

/*----[ END OF MAGNITUDE PROCESSOR CLASS ]----*/


// General ROS functionality
#include <ros/ros.h>

// PI
#include "pi/PIController.h"
#include "pi/global.h"
#include <boost/property_tree/xml_parser.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/foreach.hpp>
#include <boost/regex.hpp>
#include <boost/thread/mutex.hpp>
#include "std_msgs/String.h"
#include <algorithm>
// END PI

#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

// UGV services
#include <hal_ugv/State.h>

// UGV controllers
#include <hal_ugv/Hover.h>
#include <hal_ugv/control/Takeoff.h>
#include <hal_ugv/control/VelocityHeight.h>

#include <hal_ugv/GetEstimate.h>
#include <hal_ugv/SetTruth.h>

#include <math.h>

#define EPS_POS			5e-1	// epsilon position
#define EPS_VEL			1e-3	// epsilon position

//#define DEBUG_INFO 0

typedef enum { IDLE, TAKEOFF, VELHEIGHT, HOVER, WAYPOINT, OTHER } FSM;

using namespace std;

vector<ros::ServiceClient> srvTakeOff;
vector<ros::ServiceClient> srvHover;
vector<ros::ServiceClient> srvVelocityHeight;

vector<FSM> state_FSM; // Finite State Machine state (TODO: get the controller from the msg)
vector<double> jstate;

// used to safely exit the program
bool stop;
bool turn, go,onekick;
bool oneshot;
unsigned long int seed = 0;
gsl_rng *rnd;				// Random seed.
bool arg_seed = false;

// PI
PIController pi;
// END PI

vector<double> heights;
vector<string> idunits;
int units;
vector<string> oidunits;
int ounits;

boost::mutex guard;

double period=20.0;

// Rotation from body to navigation frame
void b2n(double rot[3], double vec[3])
{
    double tmp[3];
    for (int i = 0; i < 3; i++)
        tmp[i] = vec[i];
    double sph = sin(rot[0]); double cph = cos(rot[0]);
    double sth = sin(rot[1]); double cth = cos(rot[1]);
    double sps = sin(rot[2]); double cps = cos(rot[2]);
    double dcm[3][3];
    dcm[0][0] = cth*cps;
    dcm[1][0] = cth*sps;
    dcm[2][0] = -sth;
    dcm[0][1] = sph*sth*cps - cph*sps;
    dcm[1][1] = sph*sth*sps + cph*cps;
    dcm[2][1] = sph*cth;
    dcm[0][2] = cph*sth*cps + sph*sps;
    dcm[1][2] = cph*sth*sps - sph*cps;
    dcm[2][2] = cph*cth;
    for (int i = 0; i < 3; i++)
    {
        vec[i] = 0;
        for (int j = 0; j < 3; j++)
            vec[i] += dcm[i][j] * tmp[j];
    }
}

// Rotation from navigation to body frame
void n2b(double rot[3], double vec[3])
{
    double tmp[3];
    for (int i = 0; i < 3; i++)
        tmp[i] = vec[i];
    double sph = sin(rot[0]); double cph = cos(rot[0]);
    double sth = sin(rot[1]); double cth = cos(rot[1]);
    double sps = sin(rot[2]); double cps = cos(rot[2]);
    double dcm[3][3];
    dcm[0][0] = cth*cps;
    dcm[0][1] = cth*sps;
    dcm[0][2] = -sth;
    dcm[1][0] = sph*sth*cps - cph*sps;
    dcm[1][1] = sph*sth*sps + cph*cps;
    dcm[1][2] = sph*cth;
    dcm[2][0] = cph*sth*cps + sph*sps;
    dcm[2][1] = cph*sth*sps - sph*cps;
    dcm[2][2] = cph*cth;
    for (int i = 0; i < 3; i++)
    {
        vec[i] = 0;
        for (int j = 0; j < 3; j++)
            vec[i] += dcm[i][j] * tmp[j];
    }
}

// Callback for ugv state
void StateCallback(int unit, const hal_ugv::State::ConstPtr& msg)
{
	if (msg->controller == (std::string) "Hover" || msg->controller == (std::string) "VelocityHeight")
	{
		state_FSM[unit]  = HOVER;
				// angles of the platform
		double rot[3] = {msg->roll, msg->pitch, msg->yaw};

		// put BODY velocity in vel, and rotate to NAVIGATIONAL velocity
		double vel[3] = {msg->u, msg->v, msg->w};
		b2n(rot, vel);

		// update internal state
		jstate[unit*4+0] = msg->x;
		jstate[unit*4+1] = msg->y;
		jstate[unit*4+2] = rot[2];
		jstate[unit*4+3] = msg->t;
#ifdef DEBUG_INFO
		ROS_INFO("UAV %d got state (x, y, dx, dy) = (%f, %f, %f, %f)", unit, msg->x, msg->y, vel[0], vel[1]);
#endif
	}
}

void stopCallback(const std_msgs::String::ConstPtr& msg)
{
#ifdef DEBUG_INFO
	ROS_INFO("Received: PI_STOP.\nSwitching all units to Hover.");
#endif
	stop = true;
}

/*

void timerCallback26(const ros::TimerEvent&)
{
	ROS_INFO("go");
	go = true;
	turn = false;
	onekick = false;

}

void timerCallback20(const ros::TimerEvent&)
{
	ROS_INFO("turn");
	go = false;
	turn = true;

}
*/

// Main entry point of application
int main(int argc, char **argv)
{
	int i, j;
	if (argc<2) {
	  ROS_FATAL("requires xml_file as argument");
	  return 1;
	}
	else if (argc==2) {
		ROS_INFO("using seed from file");
	}
	else {
		seed = atoi(argv[2]);
		arg_seed = true;
		ROS_INFO("Using seed = %s", argv[2]);
	}

	try {
		ifstream is(argv[1]);
		// read parameters from xml file
		using boost::property_tree::ptree;
		ptree pt;
		if (!is.is_open()) {
		  ROS_FATAL("xml config not found");
		  return 1;
		}
		read_xml(is, pt);

		// get vector of desired heights
		BOOST_FOREACH(ptree::value_type &v,	pt.get_child("takeoffz"))
			heights.push_back(v.second.get<double>(""));

		// get vector of ids
		BOOST_FOREACH(ptree::value_type &v,	pt.get_child("idunits"))
			idunits.push_back(v.second.get<string>(""));
		BOOST_FOREACH(ptree::value_type &v,	pt.get_child("oidunits"))
			oidunits.push_back(v.second.get<string>(""));
		if (!arg_seed) {
			// and seed to start in random position
			seed =pt.get<int>("seed");
		}

		// initialize Joint State variable
		units = pt.get<int>("units");
		ounits = pt.get<int>("ounits");
		jstate.resize( (units+ounits)*pt.get<int>("dimUAVx") );

		// Initialize PI controller
		pi.Init(pt);

		//double T = pt.get<double>("T");

	} catch (int e) {
		cout << "Exception" << e << endl;
	}

	// used to safely exit the program
	stop = false;

	// Initialise the ROS client
	string nodestr = string("main_mugv");
	ros::init(argc, argv, nodestr.c_str());

	// Create a node handle, which this C code will use to bind to the ROS server
	ros::NodeHandle n;

	// Try and get a list of topics currently being braodcast
	ros::master::V_TopicInfo topics;
	int units_found = 0;
	vector<string> names;
	vector<string> nameso;
	vector<ros::Subscriber> top_state;

	// Iterate over the topics, checking for entities on /hal/<type>/<model>/<id>/Estimate
	if (ros::master::getTopics(topics)) {
		for (ros::master::V_TopicInfo::iterator i = topics.begin(); i != topics.end(); i++) {
			boost::cmatch mr;
			if (boost::regex_match(i->name.c_str(), mr, boost::regex("^/hal/ugv/irobotcreate/(.+)/Estimate$"))) {
				ROS_INFO("%s -> %s", i->name.c_str(), i->datatype.c_str());
				names.push_back(i->name);
				units_found++;
			}
		}
	}
	if (ros::master::getTopics(topics)) {
		for (ros::master::V_TopicInfo::iterator i = topics.begin(); i != topics.end(); i++) {
			boost::cmatch mro;
			if (boost::regex_match(i->name.c_str(), mro, boost::regex("^/hal/ugv/obstacle/(.+)/Estimate$"))) {
				ROS_INFO("%s -> %s", i->name.c_str(), i->datatype.c_str());
				nameso.push_back(i->name);
				//units_found++;
			}
		}
	}

	// check if number of units found corresponds to the expected units (defined in the xml)
	if ( units_found != units ) {
	  ROS_FATAL("Units found %d does not match units defined in the config %d: ", units_found, units);
	  return 1;
	}

	// sort the names (currently required, since the joint state is indexed by unit)
	sort(names.begin(), names.end());
	sort(nameso.begin(), nameso.end());

	// Subscribe to a stop message
	ros::Subscriber sub_stop = n.subscribe("PI_STOP", 1000, stopCallback);

	// subscribe and add controllers for each unit
	for (i=0; i<units; i++) {

		// init Finite State Machine
		state_FSM.push_back(IDLE);

		ROS_INFO("%s",names[i].c_str());

		// Subscribe to the states of the ugvs (requires explicit)
		top_state.push_back( n.subscribe<hal_ugv::State>(
			names[i].c_str(),10,boost::bind(StateCallback, i, _1))
		);
		ROS_INFO("%s subscribed!", names[i].c_str());

		// Add TakeOff controller
		string takeoff_cmd = string("/hal/ugv/irobotcreate/") + idunits[i] + string("/controller/Takeoff");
		srvTakeOff.push_back(n.serviceClient<hal_ugv::Takeoff>(takeoff_cmd.c_str()) );

		// Add Hover controller
		string hover_cmd = string("/hal/ugv/irobotcreate/") + idunits[i] + string("/controller/Hover");
		srvHover.push_back(n.serviceClient<hal_ugv::Hover>(hover_cmd.c_str()) );

		// Add VelocityHeight controller
		string velheight_cmd = string("/hal/ugv/irobotcreate/") + idunits[i] + string("/controller/VelocityHeight");
		srvVelocityHeight.push_back( n.serviceClient<hal_ugv::VelocityHeight>(velheight_cmd.c_str()) );
	}
	for (i=0; i<ounits; i++) {

		// init Finite State Machine

		ROS_INFO("%s",nameso[i].c_str());

		// Subscribe to the states of the ugvs (requires explicit)
		top_state.push_back( n.subscribe<hal_ugv::State>(
			nameso[i].c_str(),10,boost::bind(StateCallback, i+units, _1))
		);
		ROS_INFO("%s subscribed!", nameso[i].c_str());

	}

	// Call Takeoff on each unit
	for (i=0; i<units; i++) {
		hal_ugv::Takeoff msgTakeoff;
		msgTakeoff.request.altitude = heights[i];
		if(!srvTakeOff[i].call(msgTakeoff)){
			ROS_FATAL("NO TAKE OFF!!");
		} else {
			ROS_INFO("Taking OFF!!");
		}
		state_FSM[i] = TAKEOFF;
	}
	//ros::Timer timer1[units] = n.createTimer(ros::Duration(15.0+5.0/3), timerCallback26);
        //ros::Timer timer2[units];

	// Keep going indefinitely
	ros::Rate r(15);
	double TempVel[3], TempEuler[3];
	go = true;
	turn = false;
	onekick = false;
	oneshot = true;

	float randomtime;

	gsl_rng_default_seed = seed;
	rnd = gsl_rng_alloc (gsl_rng_default);

	double contact[units*(units+ounits)*2];

	double starttime[units], currenttime[units];
	bool ifcontact[units];
	bool initial[units];
	bool go[units];
	bool turn[units];
	bool begincturn[units];
	for (i=0; i<units; i++){
	ifcontact[i] = false; initial[i] = true; go[i] = true; turn[i] = false; begincturn[i] = false;}

    while (ros::ok())
    {
    	// Collect all state estimates

		ros::spinOnce();

/*
		if (go == true && onekick == false)
		{

			onekick = true;
			timer2 = n.createTimer(ros::Duration(20.0), timerCallback20, oneshot);

		}*/

		// check stop condittion
		if (stop)
		{
			bool success = true;
			// Call Hover on each unit
			for (i=0; i<units; i++)
			{
				hal_ugv::Hover msgHover;
				if(!srvHover[i].call(msgHover))
				{
					success = false;
					ROS_FATAL("NO Hover for UAV %d!!", i);
				}
			}
			if(success)
			{
				ROS_INFO("Succesfully switched all units to Hover.");
				break;
				return 0;
			}
		}

		// Check that units are ready to receive VelocityHeight commands
		bool ready = true;
		for (i=0; i<units; i++) {
			ready = ready && (state_FSM[i] == HOVER);
		}

		if (ready)
		{
			// Path Integral controller (based on VelocityHeight)
			vec X = jstate;
			//vec action = pi.computeControl(X);
			//vec action = pi.computeControlFeedback(X);
			//vec Xp = pi.predictState(X, action);

			// Distribute commands

			for (i=0; i<units; i++) {
				if (initial[i] == true)
				{
					starttime[i] = jstate[i*4+3];
					initial[i] = false;
					ROS_INFO("initial %lf,%lf",starttime[i],jstate[i*4+3]);
				}
				currenttime[i] = jstate[i*4+3];
				if (ifcontact[i] == false || begincturn[i] == true)
				{
					if (go[i] == true && (currenttime[i] - starttime[i] > 20.0))
					{
						go[i] = false; turn[i] = true; starttime[i] = currenttime[i];
						//ROS_INFO("1");

					}
					else if (turn[i] == true && currenttime[i] - starttime[i] > 5.0/3)
					{
						go[i] = true; turn[i] = false; starttime[i] = currenttime[i];
						begincturn[i] = false;
						//ROS_INFO("2");
					}
				}
				else if (ifcontact[i] == true && begincturn[i] == false)
				{
					go[i] = false; turn[i] = true; starttime[i] = currenttime[i]; begincturn[i] = true;
				}

				if (go[i] == true)
				{	randomtime = (gsl_rng_uniform(rnd) - 0.5);
					TempVel[0] = 0.33;
					TempVel[1] = 0;
					TempVel[2] = 0;
					TempEuler[0] = 0;
					TempEuler[1] = 0;
					TempEuler[2] = jstate[i*4+2]+randomtime/2;
					//ROS_INFO("3");
				}
				else
				{

					TempVel[0] = 0;
					TempVel[1] = 0;
					TempVel[2] = 0;
					TempEuler[0] = 0;
					TempEuler[1] = 0;
					TempEuler[2] = jstate[i*4+2]+3.14/2;
					//ROS_INFO("4");
				}

				if (abs(jstate[i*4]) >10 || abs(jstate[i*4+1]) >10)
				{
					TempVel[0] = 0;
					TempVel[1] = 0;
					TempVel[2] = 0;
					TempEuler[0] = 0;
					TempEuler[1] = 0;
					TempEuler[2] = 0;

				}

				b2n(TempEuler, TempVel);

				// convert pi control to VelocityHeight control
				hal_ugv::VelocityHeight msgVelocityHeight;
				msgVelocityHeight.request.dx  = TempVel[0];
				msgVelocityHeight.request.dy  = TempVel[1];
				msgVelocityHeight.request.z   = TempVel[2];
				msgVelocityHeight.request.yaw  = TempEuler[2];

#ifdef DEBUG_INFO
				ROS_INFO("UAV %d sent command (dx, dy, z, yaw) = (%f, %f, %f, %f)",
					i, msgVelocityHeight.request.dx, msgVelocityHeight.request.dy,
					msgVelocityHeight.request.z, msgVelocityHeight.request.yaw);
#endif

				if(!srvVelocityHeight[i].call(msgVelocityHeight)){
					ROS_FATAL("NO VELHEIGHT!!");
				}
			}

			for (i=0; i<units; i++)
			{
				for (j=0; j<(ounits+units); j++)
				{
					contact[i*(ounits+units)+j*2] = sqrt(pow((jstate[j*4+1]-jstate[i*4+1]),2)+pow((jstate[j*4]-jstate[i*4]),2));
					contact[i*(ounits+units)+j*2+1] = atan2((jstate[j*4+1]-jstate[i*4+1]),(jstate[j*4]-jstate[i*4]))-jstate[i*4+2];

					if (contact[i*(ounits+units)+j*2] < 0.33 && contact[i*(ounits+units)+j*2] > 0.1&& (abs(contact[i*(ounits+units)+j*2+1])<3.14/2.5))
					{
						ifcontact[i]=true;
						ROS_INFO("irobot %d contact",i);
						break;
					}
					else
					{
						ifcontact[i]=false;

					}

				}
			}

		}

		// Sleep
		r.sleep();
    }

	ROS_INFO("Terminating PI control (muav_main).");
	ros::spin();

	// Success!
	return 0;
}


#include "cctbx/maptbx/skeletons.h"
#include "iotbx/xplor/map_reader.h"

#include <iostream>
#include <boost/range/algorithm.hpp>
#include <boost/range/numeric.hpp>

using namespace cctbx::maptbx;
using namespace cctbx::sgtbx;
using namespace cctbx::uctbx;
using namespace iotbx::xplor;
using namespace std;
const char nl='\n';

static string symbol(const list<string> &title_lines)
{
  for(const auto &s : title_lines)
  {
    size_t pos = s.find("SPACE GROUP HALL");
    if( pos!=string::npos )
    {
      pos = s.find(':', pos+14);
      if( pos!=string::npos )
        return s.substr(++pos);
    }
  }
  return "";
}

static string symbol(int argc, char *argv[])
{
  for(int i=1; i<argc; ++i)
  {
    if( string(argv[i]).find("-space_group")!=string::npos
      && 1+i<argc)
    {
      return argv[1+i];
    }
  }
  return "";
}

asymmetric_map make_map(int argc, char *argv[], unit_cell &cell)
{
  if( argc<2 )
    throw cctbx::error("Provide map file on command line");
  string file_name(argv[1]);
  map_reader rdr(file_name);
  cell = unit_cell(rdr.unit_cell_parameters);
  string spgrs = symbol(rdr.title_lines);
  if( spgrs.empty() )
    spgrs = symbol(argc, argv);
  if( spgrs.empty() )
    throw cctbx::error("Space group symbol is missing, use: -space_group "
      "command line option");
  space_group_symbols smb("Hall: "+spgrs);
  space_group group(smb);
  scitbx::af::c_interval_grid<3> grid(rdr.grid_first, rdr.grid_last);
  long sz = boost::accumulate(rdr.grid_size, 1L, multiplies<long>());
  if( grid.size_1d()*2UL <= sz && group.order_z()>1U )
    return asymmetric_map(group.type(), move(rdr.data), rdr.grid_size);
  CCTBX_ASSERT( grid.size_1d() == sz );
  cout << "Creating asymmetric map from whole unit cell map\n";
  return asymmetric_map(group.type(), rdr.data.const_ref());
}

void run(int argc, char *argv[])
{
  unit_cell cell;
  asymmetric_map amap = make_map(argc, argv, cell);
  CCTBX_ASSERT( cell.volume()>1E-3 );
  cout
    << "Crystal: " << cell.parameters() << "  (" << cell.volume() << ")  "
    << amap.space_group().type().lookup_symbol()
    << "\nMap grid: [" << amap.box_begin() << ", " << amap.box_end() << ") on "
    << amap.unit_cell_grid_size()
    << ";  size: " << amap.data().accessor().size_1d()
    << nl;
  double sigma = 3.;
  skeleton skelet = swanson(amap, sigma);
  auto components=skeleton_components(skelet.joins, skelet.maximums.size()+1U);
  cout << "Blob count: " << components.first << nl;
  auto sizes = mask_components(skelet.marks, components.second);
  {
    asymmetric_map tmap = amap.explicit_copy();
    const char fn[] = "higest_blob-xplor.map";
    cout << "Most intense blob size? " << sizes[1] << "  DEBUG: " << sizes[0]
      << "\nSaving blob in: " << fn << nl;
    mask_density_map(tmap.data_ref(), skelet.marks, 1);
    tmap.save(fn, cell);
  }
  {
    CCTBX_ASSERT( !sizes.empty() );
    auto i = boost::max_element(sizes);
    long m = i-sizes.begin();
    const char fn[] = "largest_blob-xplor.map";
    cout << "Largest blob: " << m << " size: " << *i
      << "\nSaving in: " << fn << nl;
    CCTBX_ASSERT( m!=0 );
    // boost::partial_sort(sizes, sizes.begin()+3);
    mask_density_map(amap.data_ref(), skelet.marks, m);
    amap.save(fn, cell);
  }
}

int main(int argc, char *argv[])
{
  try {
    run(argc, argv);
    return 0;
  }
  catch(std::exception &err)
  {
    std::cerr << "ERROR! " << err.what() << std::endl;
  }
  return -1;
}


//==================================================================================================
/**
  Copyright 2017 NumScale SAS

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

#ifndef BOOST_SIMD_FUNCTION_SCALAR_ATANPI_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SCALAR_ATANPI_HPP_INCLUDED

#include <boost/simd/function/definition/atanpi.hpp>
#include <boost/simd/arch/common/scalar/function/atanpi.hpp>

#endif


/*
   Copyright 2018 Simon Vogl <svogl@voxel.at>
                  Angel Merino-Sastre <amerino@voxel.at>

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.
*/
#pragma once

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

#include <string>
#include <toffy/filter.hpp>

namespace toffy {
namespace filters {
/**
 * @brief Distance/Amplitude correction via interpolation of an
 * amplitude-dependent correction term
 * @ingroup Filters
 *
 */
class DistAmpl: public toffy::Filter{
    // linear interpolation breakpoints and coefficients
    float *breaks,
	*coeffs;

    int blen,
	clen;

    std::string _in_ampl,
	_in_depth;

    std::vector< std::vector <double > > arr_coef,
	arr_log;
    std::vector< int > arr_breaks;

public:
    DistAmpl(std::string name="distAmpl");

    virtual bool filter(const toffy::Frame& in, toffy::Frame& out);

    virtual int loadConfig(const boost::property_tree::ptree& pt);
    virtual boost::property_tree::ptree getConfig() const;
    virtual void updateConfig(const boost::property_tree::ptree &pt);

private:
    virtual bool f1(const toffy::Frame& in, toffy::Frame& /*out*/);

    float linearInterp(int measuredAmpl)
    {
	int i=1;
	while ( measuredAmpl < breaks[i] && i<blen ) {
	    i++;
	}
	// mA is now in breaks[i-1] .. breaks[i] interval

	// norm to [0..1[ interval:
	float mLocal = ( measuredAmpl - breaks[i-1] ) / ( breaks[i] - breaks[i-1]);

	// compute correction term as linear interpolation of the two factors:
	float res = mLocal * coeffs[i-1] + (1-mLocal) * coeffs[i];
	return res;
    }

    /**
     * @brief pchip lookup function
     * @param x
     * @return
     */
    double pchip(double measuredAmpl) {
	int blen = arr_breaks.size();

	double y,xlocal;
	int i=1;
	// find start:
	while ( measuredAmpl > arr_breaks[i] && i<blen) { // todo: log n lookup
	    i++;
	}
	if (i==blen) return measuredAmpl; // overflow - should not happen.

	double ampl0=arr_breaks[i-1];
	//double ampl1=arr_breaks[i];

	// matlab 'help mkpp':
	//COEFS(i,1)*(X-BREAKS(i))^(K-1) + COEFS(i,2)*(X-BREAKS(i))^(K-2) + ...
	//  COEFS(i,K-1)*(X-BREAKS(i)) + COEFS(i,K)

	xlocal = measuredAmpl-ampl0;
	std::vector<double> coefs = arr_coef[i-1];

	double x2 = xlocal*xlocal;
	double x3 = x2 * xlocal;
	y = coefs[0]*x3 + coefs[1]*x2 + coefs[2]*xlocal + coefs[3];

	return y;
    }

};

}}


//Copyright (c) 2008-2016 Emil Dotchevski and Reverge Studios, Inc.

//Distributed under the Boost Software License, Version 1.0. (See accompanying
//file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/qvm/mat_operations.hpp>
#include <boost/qvm/mat_traits_array.hpp>
using namespace boost::qvm::sfinae;

void
f()
    {
    float a[4][4];
    float det = determinant(a);
    mref(a) *= 42;
    }


/*********************************************************************************
 *  OKVIS - Open Keyframe-based Visual-Inertial SLAM
 *  Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions are met:
 *
 *   * Redistributions of source code must retain the above copyright notice,
 *     this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above copyright notice,
 *     this list of conditions and the following disclaimer in the documentation
 *     and/or other materials provided with the distribution.
 *   * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of
 *     its contributors may be used to endorse or promote products derived from
 *     this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 *  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 *  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 *  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 *  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 *  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 *  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 *  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 *  Created on: Aug 30, 2013
 *      Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
 *********************************************************************************/

/**
 * @file HomogeneousPointParameterBlock.hpp
 * @brief Header file for the HomogeneousPointParameterBlock class.
 * @author Stefan Leutenegger
 */

#ifndef INCLUDE_OKVIS_CERES_HOMOGENEOUSPOINTPARAMETERBLOCK_HPP_
#define INCLUDE_OKVIS_CERES_HOMOGENEOUSPOINTPARAMETERBLOCK_HPP_

#include <okvis/ceres/ParameterBlockSized.hpp>
#include <okvis/ceres/HomogeneousPointLocalParameterization.hpp>
#include <Eigen/Core>

/// \brief okvis Main namespace of this package.
namespace okvis {
/// \brief ceres Namespace for ceres-related functionality implemented in okvis.
namespace ceres {

/// \brief Wraps the parameter block for a speed / IMU biases estimate
class HomogeneousPointParameterBlock : public ParameterBlockSized<4, 3,
    Eigen::Vector4d>
{
 public:

  /// \brief The estimate type (4D vector).
  typedef Eigen::Vector4d estimate_t;

  /// \brief The base class type.
  typedef ParameterBlockSized<4, 3, estimate_t> base_t;

  /// \brief Default constructor (assumes not fixed).
  HomogeneousPointParameterBlock();

  /// \brief Constructor with estimate and time.
  /// @param[in] point The homogeneous point estimate.
  /// @param[in] id The (unique) ID of this block.
  /// @param[in] initialized Whether or not the 3d position is considered initialised.
  HomogeneousPointParameterBlock(const Eigen::Vector4d& point, uint64_t id,
                                 bool initialized = true);

  /// \brief Constructor with estimate and time.
  /// @param[in] point The homogeneous point estimate.
  /// @param[in] id The (unique) ID of this block.
  /// @param[in] initialized Whether or not the 3d position is considered initialised.
  HomogeneousPointParameterBlock(const Eigen::Vector3d& point, uint64_t id,
                                 bool initialized = true);

  /// \brief Trivial destructor.
  virtual ~HomogeneousPointParameterBlock();

  /// @name Setters
  /// @{

  /// @brief Set estimate of this parameter block.
  /// @param[in] point The estimate to set this to.
  virtual void setEstimate(const Eigen::Vector4d& point);

  /// \brief Set initialisaiton status.
  /// @param[in] initialized Whether or not the 3d position is considered initialised.
  void setInitialized(bool initialized)
  {
    initialized_ = initialized;
  }

  /// @}

  /// @name Getters
  /// @{

  /// @brief Get estimate
  /// \return The estimate.
  virtual Eigen::Vector4d estimate() const;

  /// \brief Get initialisaiton status.
  /// \return Whether or not the 3d position is considered initialised.
  bool initialized() const
  {
    return initialized_;
  }

  /// @}

  // minimal internal parameterization
  // x0_plus_Delta=Delta_Chi[+]x0
  /// \brief Generalization of the addition operation,
  ///        x_plus_delta = Plus(x, delta)
  ///        with the condition that Plus(x, 0) = x.
  /// @param[in] x0 Variable.
  /// @param[in] Delta_Chi Perturbation.
  /// @param[out] x0_plus_Delta Perturbed x.
  virtual void plus(const double* x0, const double* Delta_Chi,
                    double* x0_plus_Delta) const
  {
    HomogeneousPointLocalParameterization::plus(x0, Delta_Chi, x0_plus_Delta);
  }

  /// \brief The jacobian of Plus(x, delta) w.r.t delta at delta = 0.
  /// @param[in] x0 Variable.
  /// @param[out] jacobian The Jacobian.
  virtual void plusJacobian(const double* x0, double* jacobian) const
  {
    HomogeneousPointLocalParameterization::plusJacobian(x0, jacobian);
  }

  // Delta_Chi=x0_plus_Delta[-]x0
  /// \brief Computes the minimal difference between a variable x and a perturbed variable x_plus_delta
  /// @param[in] x0 Variable.
  /// @param[in] x0_plus_Delta Perturbed variable.
  /// @param[out] Delta_Chi Minimal difference.
  /// \return True on success.
  virtual void minus(const double* x0, const double* x0_plus_Delta,
                     double* Delta_Chi) const
  {
    HomogeneousPointLocalParameterization::minus(x0, x0_plus_Delta, Delta_Chi);
  }

  /// \brief Computes the Jacobian from minimal space to naively overparameterised space as used by ceres.
  /// @param[in] x0 Variable.
  /// @param[out] jacobian the Jacobian (dimension minDim x dim).
  /// \return True on success.
  virtual void liftJacobian(const double* x0, double* jacobian) const
  {
    HomogeneousPointLocalParameterization::liftJacobian(x0, jacobian);
  }

  /// @brief Return parameter block type as string
  virtual std::string typeInfo() const
  {
    return "HomogeneousPointParameterBlock";
  }

 private:
  bool initialized_;  ///< Whether or not the 3d position is considered initialised.

};

}  // namespace ceres
}  // namespace okvis

#endif /* INCLUDE_OKVIS_CERES_HOMOGENEOUSPOINTPARAMETERBLOCK_HPP_ */