/*
 * math_inc.h
 *
 *  Created on: 20 дек. 2017 г.
 *      Author: titov
 */

#ifndef SOURCE_COMMON_MATH_INC_H_
#define SOURCE_COMMON_MATH_INC_H_

#include <cmath>
#include <utility>

namespace math {
namespace constants {

    constexpr float pi2       = 3.141592653589793238462643383279502884197169399375105820974944f * 2.0f;
    constexpr float pi        = 3.141592653589793238462643383279502884197169399375105820974944f * 1.0f;
    constexpr float pi_2      = 3.141592653589793238462643383279502884197169399375105820974944f / 2.0f;
    constexpr float pi_3      = 3.141592653589793238462643383279502884197169399375105820974944f / 3.0f;
    constexpr float pi_4      = 3.141592653589793238462643383279502884197169399375105820974944f / 4.0f;
    constexpr float pi_6      = 3.141592653589793238462643383279502884197169399375105820974944f / 6.0f;

    constexpr float pi0_3     = 3.141592653589793238462643383279502884197169399375105820974944f * 0.0f / 3.0f;
    constexpr float pi1_3     = pi_3;
    constexpr float pi2_3     = 3.141592653589793238462643383279502884197169399375105820974944f * 2.0f / 3.0f;
    constexpr float pi3_3     = pi;
    constexpr float pi4_3     = 3.141592653589793238462643383279502884197169399375105820974944f * 4.0f / 3.0f;
    constexpr float pi5_3     = 3.141592653589793238462643383279502884197169399375105820974944f * 5.0f / 3.0f;
    constexpr float pi6_3     = pi2;

    constexpr float _1_pi     = 0.3183098861837906715f;
    constexpr float _2_pi     = 0.6366197723675813430f;

    constexpr float sqrt3     = 1.73205080757f;
    constexpr float sqrt3_2   = 1.73205080757f / 2.0f;

    constexpr float sqrt2     = 1.4142135623730950488f;
    constexpr float _1_sqrt2  = 0.7071067811865475244f;

    constexpr float _2_sqrtpi = 1.1283791670955125739f;

    constexpr float e         = 2.7182818284590452354f;
    constexpr float log2e     = 1.4426950408889634074f;
    constexpr float log10e    = 0.4342944819032518276f;
    constexpr float ln2       = 0.6931471805599453094f;
    constexpr float ln10      = 2.3025850929940456840f;

}

namespace function {

    //!Функция вычисляет время для заданного перемещения с заданными ограничениями скорости и ускорения для равноускоренного профиля движения.
    inline float get_time( float s, float v, float a ) {
        using namespace std;

        float v_max = sqrt( s / a ) * a;

        if( v_max > v )
            return s / v + v / a;
        else
            return sqrt( s / a ) * 2;
    }

    //!Функция
    inline void sincos(float angle, float * sin_val, float * cos_val) {

        using namespace std;

        *sin_val = sin(angle);
        *cos_val = cos(angle);

    }

    inline float isqrt(float val ) {

        using namespace std;

        return 1.0f / sqrt(val);

    }

    inline float q_rsqrt( float number ) {

        using namespace std;

        long i;
        float x2, y;
        const float threehalfs = 1.5f;

        x2 = number * 0.5f;
        y  = number;
        i  = * ( long * ) &y;                       // evil floating point bit level hacking
        i  = 0x5f3759dfl - ( i >> 1 );
        y  = * ( float * ) &i;
        y  = y * ( threehalfs - ( x2 * y * y ) );   // 1st iteration
        y  = y * ( threehalfs - ( x2 * y * y ) );   // 2nd iteration, this can be removed

        return y;
    }

    inline float saturation( float value, float max, float min ) {

        using namespace std;

        return fmaxf( fminf( value, max ), min );

    }

    inline float turn( float value ) {
        return  value >   math::constants::pi ? value - math::constants::pi2 :
                value <= -math::constants::pi ? value + math::constants::pi2 :
                                                value;
    }

    inline std::pair<float, float> calculate_kb( float x1, float x2, float y1, float y2 ) {

        float coefficient = ( y2 - y1 ) / ( x2 - x1 );
        float offset = y1 - x1 * coefficient;

        return { coefficient, offset };
    }
}

}

#endif /* SOURCE_COMMON_MATH_INC_H_ */