const double M_2PI = M_PI * 2;
const double DEG2RAD = M_PI / 180.;
const double GP2RAD = DEG2RAD / 360000.;
const double RAD2GP = 6000. / (M_PI / 10800);
const double NM2km = 1.852;
const double R = 6372795;



/// Нормализация угла курса (от 0 до M_2PI)
double NormBrg(double brg)
{
    if(brg <= 0.0)  return M_2PI + fmod(brg, M_2PI);
    if(brg > M_2PI) return fmod(brg, M_2PI);
    return brg;
}

/// Нормализация угла курса (от -M_PI до M_PI)
double NormBrgPI(double brg) { return NormBrg(brg + M_PI) - M_PI; }

/// Класс для вычислений расстояний, перевода из одних единиц измерения в другие
class QCalculation
{
public:
    enum MEASURE { M_RAD = 0, M_CRDGP, M_CRDDEG, M_DSTKM, M_DSTMTR, M_DSTNM, M_CRSMAGN };
    enum TURN { T_NONE = 0, T_ANY, T_RIGHT, T_LEFT };

private:
    MEASURE    _crdUseun, _crsUseun, _distUseun, _hghtUseun;

private:
    double Rng_(double lat1, double lon1, double lat2, double lon2);
    double Brg_(double lat1, double lon1, double lat2, double lon2);
    void Ll_(double &lat2, double &lon2, double lat1, double lon1, double brg, double rng);
    void Ll_2_(double &lat2, double &lon2, double lat1, double lon1, double brg, double rng);
    void La_(double &lat2, double &lon2, double lat1, double lon1, double latC, double lonC, double rng,
            TURN LeftRight);

public:
    QCalculation(MEASURE Crd = M_CRDGP, MEASURE Crs = M_CRSMAGN, MEASURE Dst = M_DSTKM,  MEASURE Hgh = M_DSTMTR):
                _crdUseun(Crd), _crsUseun(Crs), _distUseun(Dst), _hghtUseun(Hgh) {}
    double UseUnToRad(double x, MEASURE un);
    double RadToUseUn(double x, MEASURE un);
    double Rng(double lat1_, double lon1_, double lat2_, double lon2_, bool need_transference = false);
    double Brg(double lat1_, double lon1_, double lat2_, double lon2_, bool need_transference = false);
    void Ll(double &lat2_, double &lon2_, double lat1_, double lon1_, double brg1_, double rng_,
            bool need_transference = false);
    void Ll_2(double &lat2_, double &lon2_, double lat1_, double lon1_, double brg2_, double rng_,
            bool need_transference = false);
    void La(double &lat2_, double &lon2_, double lat1_, double lon1_, double latC_, double lonC_,
            double rng_, TURN LeftRight, bool need_transference = false);
};

/// Перевод из одних единиц измерения в радианы
double QCalculation::UseUnToRad(double x, MEASURE un)
{
    switch(un)
    {
    case M_RAD:
    break;
    case M_CRDGP: case M_CRDDEG:
        x *= (un == M_CRDGP ? GP2RAD : DEG2RAD);
        while(x > M_PI) x -= M_2PI;
        while(x < -M_PI) x += M_2PI; 
    break;
    case M_DSTMTR:
        x /= 1000;
    break;
    case M_DSTKM:
        x /= NM2km;
    break;
    case M_DSTNM:
        x *= DEG2RAD / 60.0;
    break;
    case M_CRSMAGN:
        x *= DEG2RAD;
        x = NormBrg(x);
    break;
    }

    return x;
}

/// Перевод из радиан в другие единицы измерения
double QCalculation::RadToUseUn(double x, MEASURE un)
{
    switch(un)
    {
    case M_RAD:
    break;
    case M_CRDGP: case M_CRDDEG:
        while(x > M_PI) x -= M_2PI;
        while(x < -M_PI) x += M_2PI; 
        x /= (un == M_CRDGP ? GP2RAD : DEG2RAD);
    break;
    case M_DSTMTR:
        x *= 1000;
    break;
    case M_DSTKM:
        x *= NM2km;
    break;
    case M_DSTNM:
        x /= DEG2RAD / 60.0;
    break;
    case M_CRSMAGN:
        x = NormBrg(x);
        x /= DEG2RAD;
    break;
    }

    return x;
}

/// Параметры всех функций должны быть поданы в радианах

/// Расчет расстояния между 2 точками (на шаре)
double QCalculation::Rng_(double lat1, double lon1, double lat2, double lon2)
{
    double acos_rng = sin(lat1) * sin(lat2) + cos(lat1) * cos(lat2) * cos(lon2 - lon1);
    if(acos_rng >= 1. || _isnan(acos_rng))
        return 0.;
    if(acos_rng <= -1.)
        return M_PI;
    return acos(acos_rng);
}

/// Расчет курса от точки 1 к точке 2
double QCalculation::Brg_(double lat1, double lon1, double lat2, double lon2)
{
    if(fabs(NormBrgPI(lat1 - lat2)) + fabs(NormBrgPI(lon1 - lon2)) < 0.000000001)
        return 0.;
    double retbrg = atan2(sin(lon2 - lon1) * cos(lat2),
                            cos(lat1) * sin(lat2) - sin(lat1) * cos(lon2 - lon1) * cos(lat2));
    if(retbrg < 0.) retbrg = M_2PI + retbrg;
    if(_isnan(retbrg)) return 0.;
    return retbrg;
}

/// Расчет точки по полярным координатам (курс из точки 1 в точку 2)
void QCalculation::Ll_(double &lat2, double &lon2, double lat1, double lon1, double brg, double rng)
{
    lat2 = asin(sin(lat1) * cos(rng) + cos(lat1) * sin(rng) * cos(brg));
    lon2 = lon1 + atan2(sin(brg) * sin(rng), cos(lat1) * cos(rng) - sin(lat1) * sin(rng) * cos(brg));
}

/// Расчет точки по полярным координатам (курс в точке 2 из точки 1)
void QCalculation::Ll_2_(double &lat2, double &lon2, double lat1, double lon1, double brg, double rng)
{
    double sin_rng = sin(rng);
    double sin_rng_sin_brg = sin_rng * sin(brg);
    double Dlon = asin(sin_rng_sin_brg / cos(lat1));
    lat2 = asin((sin(lat1 + rng) + sin_rng * cos(lat1) * (cos(brg) * cos(Dlon) - 1.0)) /
                (1.0 - sin_rng_sin_brg * sin_rng_sin_brg));
    lon2 = lon1 + Dlon;
}

/// Расчет точки на дуге
void QCalculation::La_(double &lat2, double &lon2, double lat1, double lon1, double latC, double lonC,
                      double rng, TURN LeftRight)
{
    double brg = Brg_(latC, lonC, lat1, lon1), radius = Rng_(latC, lonC, lat1, lon1);
    if(radius < 0.000000001)
    {
        lat2 = lat1;
        lon2 = lon1;
    }
    else
    {
        if(LeftRight == T_LEFT)
            brg -= rng / radius;
        else if(LeftRight == T_RIGHT)
            brg += rng / radius;
        Ll_(lat2, lon2, latC, lonC, brg, radius);
    }
}

/// Параметры всех функций в других единицах измерения

/// Расчет расстояния между 2 точками (на шаре)
double QCalculation::Rng(double lat1_, double lon1_, double lat2_, double lon2_, bool need_transference)
{
    double    lat1 = UseUnToRad(lat1_, _crdUseun),
            lat2 = UseUnToRad(lat2_, _crdUseun),
            lon1 = UseUnToRad(lon1_, _crdUseun),
            lon2 = UseUnToRad(lon2_, _crdUseun);
    if(need_transference)
        return RadToUseUn(Rng_(lat1, lon1, lat2, lon2), _crdUseun);
    else
        return Rng_(lat1, lon1, lat2, lon2);
}

/// Расчет курса от точки 1 к точке 2
double QCalculation::Brg(double lat1_, double lon1_, double lat2_, double lon2_, bool need_transference)
{
    if(fabs(lat1_ - lat2_) + fabs(lon1_ - lon2_) < 0.00000001)
        return 0.;
    double    lat1 = UseUnToRad(lat1_, _crdUseun),
            lat2 = UseUnToRad(lat2_, _crdUseun),
            lon1 = UseUnToRad(lon1_, _crdUseun),
            lon2 = UseUnToRad(lon2_, _crdUseun);
    if(need_transference)
        return RadToUseUn(Brg_(lat1, lon1, lat2, lon2), _crsUseun);
    else
        return Brg_(lat1, lon1, lat2, lon2);
}

/// Расчет точки по полярным координатам (курс из точки 1 в точку 2)
void QCalculation::Ll(double &lat2_, double &lon2_, double lat1_, double lon1_, double brg_, double rng_,
                      bool need_transference)
{
    double    lat1 = UseUnToRad(lat1_, _crdUseun),
            lon1 = UseUnToRad(lon1_, _crdUseun),
            brg = UseUnToRad(brg_, _crsUseun),
            rng = UseUnToRad(rng_, _distUseun);
    double lat2, lon2;
    Ll_(lat2, lon2, lat1, lon1, brg, rng);
    if(need_transference)
        lat2_ = RadToUseUn(lat2, _crdUseun),
        lon2_ = RadToUseUn(lon2, _crdUseun);
    else
        lat2_ = lat2,
        lon2_ = lon2;
}

/// Расчет точки по полярным координатам (курс в точке 2 из точки 1)
void QCalculation::Ll_2(double &lat2_, double &lon2_, double lat1_, double lon1_, double brg_, double rng_,
                        bool need_transference)
{
    double    lat1 = UseUnToRad(lat1_, _crdUseun),
            lon1 = UseUnToRad(lon1_, _crdUseun),
            brg = UseUnToRad(brg_, _crsUseun),
            rng = UseUnToRad(rng_, _distUseun);
    double lat2, lon2;
    Ll_2_(lat2, lon2, lat1, lon1, brg, rng);
    if(need_transference)
        lat2_ = RadToUseUn(lat2, _crdUseun),
        lon2_ = RadToUseUn(lon2, _crdUseun);
    else
        lat2_ = lat2,
        lon2_ = lon2;
}

/// Расчет точки на дуге
void QCalculation::La(double &lat2_, double &lon2_, double lat1_, double lon1_, double latC_, double lonC_,
                        double rng_, TURN LeftRight, bool need_transference)
{
    double    rng = UseUnToRad(rng_, _distUseun),
            lat1 = UseUnToRad(lat1_, _crdUseun),
            lon1 = UseUnToRad(lon1_, _crdUseun),
            lat3 = UseUnToRad(latC_, _crdUseun),
            lon3 = UseUnToRad(lonC_, _crdUseun);
    double lat2, lon2;
    La_(lat2, lon2, lat1, lon1, lat3, lon3, rng, LeftRight);
    if(need_transference)
        lat2_ = RadToUseUn(lat2, _crdUseun),
        lon2_ = RadToUseUn(lon2, _crdUseun);
    else
        lat2_ = lat2,
        lon2_ = lon2;
}