/*
 * Vector.h
 *
 *  Created on: 7 мая 2017 г.
 *      Author: titov
 */

#ifndef SOURCES_CONTROL_VECTOR_H_
#define SOURCES_CONTROL_VECTOR_H_

#include "../math/math_inc.hh"

namespace control {

typedef float Value;

struct IValueTransform {
    virtual Value operator()(Value value) const = 0;
};

struct NaturalCoordinate {
	float u;
	float v;
	float w;

	NaturalCoordinate() noexcept : u(NAN), v(NAN), w(NAN) {}
	NaturalCoordinate( float _u, float _v, float _w) noexcept : u(_u), v(_v), w(_w) {}
	NaturalCoordinate( const NaturalCoordinate & rh ) noexcept : u(rh.u), v(rh.v), w(rh.w) {}
	NaturalCoordinate & operator=( const NaturalCoordinate & rh ) { u = rh.u, v = rh.v, w = rh.w; return *this; }
    NaturalCoordinate( const volatile NaturalCoordinate & rh ) noexcept : u(rh.u), v(rh.v), w(rh.w) {}
    NaturalCoordinate & operator=( const volatile NaturalCoordinate & rh ) { u = rh.u, v = rh.v, w = rh.w; return *this; }

    bool operator==( const NaturalCoordinate & rh ) const { return u == rh.u and v == rh.v and w == rh.w; }
    bool operator!=( const NaturalCoordinate & rh ) const { return not rh.operator ==(*this); }

    operator bool() const {

        using namespace std;
        return not( isnan(u) or isnan(v) or isnan(w) );

    }

};

struct StandingVector {
	float alfa;
	float beta;

    StandingVector() noexcept : alfa(NAN), beta(NAN) {}
    StandingVector(float _alfa, float _beta) noexcept : alfa(_alfa), beta(_beta) {}
    StandingVector( const volatile StandingVector & rh ) noexcept : alfa(rh.alfa), beta(rh.beta) {}
    StandingVector( const StandingVector & rh ) noexcept : alfa(rh.alfa), beta(rh.beta) {}
    StandingVector & operator=( const volatile StandingVector & rh ) noexcept { alfa = rh.alfa, beta = rh.beta; return *this; }
    StandingVector & operator=( const StandingVector & rh ) noexcept { alfa = rh.alfa, beta = rh.beta; return *this; }

	float sqrlen() const { return alfa * alfa + beta * beta; }
	float len() const { return std::sqrt( sqrlen() ); }
	StandingVector & operator*=(float mul) { alfa *= mul; beta *= mul; return *this; }
	StandingVector operator*(float mul) {
	    return StandingVector( alfa * mul, beta * mul );
	}

	bool operator==( const StandingVector & rh ) const { return alfa == rh.alfa and beta == rh.beta; }
    bool operator!=( const StandingVector & rh ) const { return not rh.operator ==(*this); }


};

struct IStandingVectorFactorTransform {
    virtual StandingVector operator()( StandingVector value, Value factor ) const = 0;
};

struct RotatingVector {
	float q;
	float d;

    RotatingVector() noexcept : q(NAN), d(NAN) {}
    RotatingVector(float _q, float _d) : q(_q), d(_d) {}
    RotatingVector( const volatile RotatingVector & rh ) : q(rh.q), d(rh.d) {}
    RotatingVector & operator=( const volatile RotatingVector & rh ) { q = rh.q, d = rh.d; return *this;}
    RotatingVector( const RotatingVector & rh ) : q(rh.q), d(rh.d) {}
    RotatingVector & operator=( const RotatingVector & rh ) { q = rh.q, d = rh.d; return *this;}

	float sqrlen() const { return q * q + d * d; }
	float len() const { return std::sqrt( sqrlen() ); }
	RotatingVector & operator*=(float mul) { q *= mul; d *= mul; return *this; }
	RotatingVector operator*(float mul) { return RotatingVector( q * mul, d * mul ); }

    RotatingVector operator-( const RotatingVector & rv ) {
        return RotatingVector( q - rv.q, d - rv.d );
    }

    RotatingVector operator+( const RotatingVector & rv ) {
        return RotatingVector( q + rv.q, d + rv.d );
    }

    RotatingVector & operator+=( const RotatingVector & rv ) {
        q += rv.q, d += rv.d;
        return *this;
    }

    bool operator==( const RotatingVector & rh ) const { return q == rh.q and d == rh.d; }
    bool operator!=( const RotatingVector & rh ) const { return not rh.operator ==(*this); }
};

//!Функция нормализации значения заданного тремя векторами сдвинутиными на 120 гр.
/*!Функция нормализации значения заданного тремя векторами сдвинутиными на 120 гр.
 * Вычисляет новые значения исходя из предположения что сумма всех значений равна нулю.
 *
 */
NaturalCoordinate normalize( const NaturalCoordinate & input );

//!Функция вычисляет значение нулевой последовательности.
float calc_offset(const NaturalCoordinate & input);

//!Функция быстрого вычисления длинны вектора.
float flength(const StandingVector & vector);
//!Функция быстрого вычисления длинны вектора.
float flength(const RotatingVector & vector);

StandingVector tf_clark( const NaturalCoordinate & );
RotatingVector tf_park( const StandingVector &, float );

NaturalCoordinate itf_clark( const StandingVector & );
StandingVector itf_park( const RotatingVector &, float );

//!Функция ограничивает амплитуду вектора.
/*!Функция ограничивает амплитуду вектора по быстрому алгоритму.
 * Направление вектора не изменяется.
 * Возможное отклонение не превышает 5%.
 * \param[in,out] vector Вектор подлежащий ограничению
 * \param[in] limit Максимальная амплитуда вектора.
 * \return Признак того что вектор был ограничен.
 */
bool flimitation( RotatingVector & vector, float limit );
//!\copydoc flimitation( RotatingVector & vector, float limit )
bool flimitation( StandingVector & vector, float limit );

//!Функция ограничивает амплитуду вектора.
/*!Функция ограничивает амплитуду вектора.
 * Направление вектора не изменяется.
 * \param[in,out] vector Вектор подлежащий ограничению
 * \param[in] limit Максимальная амплитуда вектора.
 * \return Признак того что вектор был ограничен.
 */
bool limitation( RotatingVector & vector, float limit );
//!\copydoc flimitation( RotatingVector & vector, float limit )
bool limitation( StandingVector & vector, float limit );

template<typename Type>
struct PhaseSpacePoint {

    typedef Type Value;

    Type value;
    Type d_value;

    PhaseSpacePoint() noexcept : value(), d_value() {}
    PhaseSpacePoint( Type val, Type d_val ) noexcept : value(val), d_value(d_val) {}
    PhaseSpacePoint( const PhaseSpacePoint & rh ) noexcept : value(rh.value), d_value(rh.d_value) {}
    PhaseSpacePoint operator*( const Type & mul ) { return PhaseSpacePoint( value * mul, d_value * mul ); }
    PhaseSpacePoint & operator=( const PhaseSpacePoint & rh ) { value = rh.value, d_value = rh.d_value; return *this;}

    bool operator==( const PhaseSpacePoint & rh ) const { return value == rh.value and d_value == rh.d_value; }
    bool operator!=( const PhaseSpacePoint & rh ) const { return not rh.operator ==(*this); }
};

typedef PhaseSpacePoint<float> PhaseSpaceValue;

bool limitation( PhaseSpaceValue & value, float limit );

float next( const PhaseSpaceValue & val, float time );
template<typename Type, typename Time = float>
Type next( const PhaseSpacePoint<Type> & val, Time time );

bool limitation( PhaseSpaceValue & value, float upper_limit, float lower_limit );

}


inline float control::flength(const StandingVector & vector) {
	const float amax = 0.960433870f;
	const float bmin = 0.397824735f;

	const float qa = std::fabs(vector.alfa);
	const float qb = std::fabs(vector.beta);

	if(qa > qb)
		return qa * amax + qb * bmin;
	else
		return qa * bmin + qb * amax;
}

inline float control::flength(const RotatingVector & vector) {
	const float amax = 0.960433870f;
	const float bmin = 0.397824735f;

	const float qa = std::fabs(vector.q);
	const float qb = std::fabs(vector.d);

	if(qa > qb)
		return qa * amax + qb * bmin;
	else
		return qa * bmin + qb * amax;
}

inline control::NaturalCoordinate control::normalize(const NaturalCoordinate & input) {
    return NaturalCoordinate(
            ( input.u * 2 - input.v - input.w ) * 0.333333333f,
            ( input.v * 2 - input.w - input.u ) * 0.333333333f,
            ( input.w * 2 - input.u - input.v ) * 0.333333333f
    );
}

inline float control::calc_offset(const NaturalCoordinate & input) {
    return ( input.u + input.v + input.w ) * 0.333333333f;
}

inline control::StandingVector control::tf_clark( const NaturalCoordinate & value ) {

    return StandingVector(
        value.u,
        ( value.u + 2 * value.v ) * 0.577350269f
    );

}

inline control::RotatingVector control::tf_park( const StandingVector & value, float iota ) {

    float sin_val;
    float cos_val;

    math::function::sincos(iota, &sin_val, &cos_val);

    return RotatingVector(
        - value.alfa * sin_val + value.beta * cos_val,
          value.alfa * cos_val + value.beta * sin_val
    );

}

inline control::NaturalCoordinate control::itf_clark( const StandingVector & value ) {

    return NaturalCoordinate(
        value.alfa,
        (math::constants::sqrt3 * value.beta - value.alfa) * 0.5f,
        - value.alfa - (math::constants::sqrt3 * value.beta - value.alfa) * 0.5f
    );

}

inline control::StandingVector control::itf_park( const RotatingVector & value, float iota ) {

    float sin_val;
    float cos_val;

    math::function::sincos(iota, &sin_val, &cos_val);

    return StandingVector(
        -value.q * sin_val + value.d * cos_val,
         value.q * cos_val + value.d * sin_val
    );

}

inline float control::next( const PhaseSpaceValue & val, float time ) {
    return val.value + val.d_value * time;
}

template<typename Type, typename Time>
inline Type control::next( const PhaseSpacePoint<Type> & val, Time time ) {
    return val.value + val.d_value * time;
}

inline bool control::flimitation( RotatingVector & vector, float limit ) {

    return limitation( vector, limit );

}

inline bool control::flimitation( StandingVector & vector, float limit ) {

    return limitation( reinterpret_cast<RotatingVector &>(vector), limit );

}

inline bool control::limitation( RotatingVector & vector, float limit ) {
    bool is_limited;

    float a_sqrt = vector.sqrlen();
    float inv_sqrt = math::function::q_rsqrt( a_sqrt );

    if( is_limited = ( a_sqrt > limit*limit ) )
        vector = vector * ( limit * inv_sqrt );

    return is_limited;
}


inline bool control::limitation( StandingVector & vector, float limit ) {

    return limitation( reinterpret_cast<RotatingVector &>(vector), limit );

}

inline bool control::limitation( PhaseSpaceValue & value, float limit ) {

    bool is_limited = false;

    using namespace std;

    if( value.value > limit ) {
        value = PhaseSpaceValue( limit, 0.0f );
        is_limited = true;
    }

    if( value.value < - limit ) {
        value = PhaseSpaceValue( - limit, 0.0f );
        is_limited = true;
    }

    return is_limited;

}

inline bool control::limitation( PhaseSpaceValue & value, float upper_limit, float lower_limit ) {

    bool is_limited = false;

    using namespace std;

    if( value.value > upper_limit ) {
        value = PhaseSpaceValue( upper_limit, 0.0f );
        is_limited = true;
    }

    if( value.value < lower_limit ) {
        value = PhaseSpaceValue( lower_limit, 0.0f );
        is_limited = true;
    }

    return is_limited;

}

#endif /* SOURCES_CONTROL_VECTOR_H_ */