/****************************************************************************
 *
 *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
 *   Author: 	@author Thomas Gubler <thomasgubler@gmail.com>
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. 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.
 * 3. Neither the name PX4 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.
 *
 ****************************************************************************/


/**
 * @file mTecs_blocks.h
 *
 * Custom blocks for the mTecs
 *
 * @author Thomas Gubler <thomasgubler@gmail.com>
 */

#pragma once

#include <controllib/blocks.hpp>
#include <systemlib/err.h>

namespace fwPosctrl
{

using namespace control;

/* An block which can be used to limit the output */
class BlockOutputLimiter: public SuperBlock
{
public:
// methods
	BlockOutputLimiter(SuperBlock *parent, const char *name, bool isAngularLimit = false) :
		SuperBlock(parent, name),
		_isAngularLimit(isAngularLimit),
		_min(this, "MIN"),
		_max(this, "MAX")
	{};
	virtual ~BlockOutputLimiter() {};
	/*
	 * Imposes the limits given by _min and _max on value
	 *
	 * @param value is changed to be on the interval _min to _max
	 * @param difference if the value is changed this corresponds to the change of value * (-1)
     * otherwise unchanged
	 * @return: true if the limit is applied, false otherwise
	 */
	bool limit(float& value, float& difference) {
		float minimum = isAngularLimit() ? getMin() * M_DEG_TO_RAD_F : getMin();
		float maximum = isAngularLimit() ? getMax() * M_DEG_TO_RAD_F : getMax();
		if (value < minimum) {
			difference = value - minimum;
			value = minimum;
			return true;
		} else if (value > maximum) {
			difference = value - maximum;
			value = maximum;
			return true;
		}
		return false;
	}
//accessor:
	bool isAngularLimit() {return _isAngularLimit ;}
	float getMin() { return _min.get(); }
	float getMax() { return _max.get(); }
protected:
//attributes
	bool _isAngularLimit;
	control::BlockParamFloat _min;
	control::BlockParamFloat _max;
};

typedef

/* A combination of feed forward, P and I gain using the output limiter*/
class BlockFFPILimited: public SuperBlock
{
public:
// methods
	BlockFFPILimited(SuperBlock *parent, const char *name, bool isAngularLimit = false) :
		SuperBlock(parent, name),
		_outputLimiter(this, "", isAngularLimit),
		_integral(this, "I"),
		_kFF(this, "FF"),
		_kP(this, "P"),
		_kI(this, "I"),
		_offset(this, "OFF")
	{};
	virtual ~BlockFFPILimited() {};
	float update(float inputValue, float inputError) { return calcLimitedOutput(inputValue, inputError, _outputLimiter); }
// accessors
	BlockIntegral &getIntegral() { return _integral; }
	float getKFF() { return _kFF.get(); }
	float getKP() { return _kP.get(); }
	float getKI() { return _kI.get(); }
	float getOffset() { return _offset.get(); }
	BlockOutputLimiter &getOutputLimiter() { return _outputLimiter; };
protected:
	BlockOutputLimiter _outputLimiter;

	float calcUnlimitedOutput(float inputValue, float inputError) {return getOffset() + getKFF() * inputValue + getKP() * inputError + getKI() * getIntegral().update(inputError);}
	float calcLimitedOutput(float inputValue, float inputError, BlockOutputLimiter &outputLimiter) {
		float difference = 0.0f;
		float integralYPrevious = _integral.getY();
		float output = calcUnlimitedOutput(inputValue, inputError);
		if(outputLimiter.limit(output, difference) &&
			(((difference < 0) && (getKI() * getIntegral().getY() < 0)) ||
			((difference > 0) && (getKI() * getIntegral().getY() > 0)))) {
				getIntegral().setY(integralYPrevious);
		}
		return output;
	}
private:
	BlockIntegral _integral;
	BlockParamFloat _kFF;
	BlockParamFloat _kP;
	BlockParamFloat _kI;
	BlockParamFloat _offset;
};

/* A combination of feed forward, P and I gain using the output limiter with the option to provide a special output limiter (for example for takeoff)*/
class BlockFFPILimitedCustom: public BlockFFPILimited
{
public:
// methods
	BlockFFPILimitedCustom(SuperBlock *parent, const char *name, bool isAngularLimit = false) :
		BlockFFPILimited(parent, name, isAngularLimit)
		{};
	virtual ~BlockFFPILimitedCustom() {};
	float update(float inputValue, float inputError, BlockOutputLimiter *outputLimiter = NULL) {
		return calcLimitedOutput(inputValue, inputError, outputLimiter == NULL ? _outputLimiter : *outputLimiter);
	}
};

/* A combination of P gain and output limiter */
class BlockPLimited: public SuperBlock
{
public:
// methods
	BlockPLimited(SuperBlock *parent, const char *name, bool isAngularLimit = false) :
		SuperBlock(parent, name),
		_kP(this, "P"),
		_outputLimiter(this, "", isAngularLimit)
	{};
	virtual ~BlockPLimited() {};
	float update(float input) {
		float difference = 0.0f;
		float output = getKP() * input;
		getOutputLimiter().limit(output, difference);
		return output;
	}
// accessors
	BlockOutputLimiter &getOutputLimiter() { return _outputLimiter; };
	float getKP() { return _kP.get(); }
private:
	control::BlockParamFloat _kP;
	BlockOutputLimiter _outputLimiter;
};

/* A combination of P, D gains and output limiter */
class BlockPDLimited: public SuperBlock
{
public:
// methods
	BlockPDLimited(SuperBlock *parent, const char *name, bool isAngularLimit = false) :
		SuperBlock(parent, name),
		_kP(this, "P"),
		_kD(this, "D"),
		_derivative(this, "D"),
		_outputLimiter(this, "", isAngularLimit)
	{};
	virtual ~BlockPDLimited() {};
	float update(float input) {
		float difference = 0.0f;
		float output = getKP() * input + (getDerivative().getDt() > 0.0f ? getKD() * getDerivative().update(input) : 0.0f);
		getOutputLimiter().limit(output, difference);

		return output;
	}
// accessors
	float getKP() { return _kP.get(); }
	float getKD() { return _kD.get(); }
	BlockDerivative &getDerivative() { return _derivative; }
	BlockOutputLimiter &getOutputLimiter() { return _outputLimiter; };
private:
	control::BlockParamFloat _kP;
	control::BlockParamFloat _kD;
	BlockDerivative _derivative;
	BlockOutputLimiter _outputLimiter;
};

}