#include <cmath>

static const float PI = 3.14159265358979323846f;

float computeHeadingAngleInDegrees(const float x, const float y)
{
    if (x == 0 && y > 0)
        return 90.f;
    else if (x == 0 && y < 0)
        return 270.f;
    else if (x > 0 && y == 0)
        return 0.f;
    else if (x < 0 && y == 0)
        return 180.f;
    else if (x > 0 && y > 0)
        return std::asin(y) * 180.f / PI;
    else if (x < 0 && y > 0)
        return (PI - std::asin(y)) * 180.f / PI;
    else if (x < 0 && y < 0)
        return (PI + std::asin(-y)) * 180.f / PI;
    else if (x > 0 && y < 0)
        return (2 * PI - std::asin(-y)) * 180.f / PI;
    else
        // This should not happen
        return -1.f;
}

void computeHeadingVector(const float angleInDegrees, float& x, float& y)
{
    x = std::cos(angleInDegrees * PI / 180.f);
    y = std::sin(angleInDegrees * PI / 180.f);
}

// Formulas used by Elbit
float COneSimAgentManager::HelpMethodComputeHeadingAngle(const float x, const float y, const float z)
{
    static const float PI = 3.14159265358979323846f;
    float dHeading = 0.5*PI - atan2(y, x);
    return dHeading;
}

void COneSimAgentManager::HelpMethodComputeHeadingVector(const float angle, float& x, float& y, float& z)
{
    x = std::sin(angle);
    y = std::cos(angle);
    z = 0;
}