// Topocentric Coordinates (U=east, V=north, W=up)
struct UVW
{
    double dU,dV,dW;
};

// Kinematic Vector in m/s
struct Kin_Vect
{
    double dVx, dVy, dVz;
};

// Geographic Coordinates in rad
struct LLA_Rad
{
    double dLat, dLon, dAlt;
};

struct XYZ
{
    double dX = 0,dY = 0,dZ = 0;
};

// Kinematic Parameters (dCrs, dClb in rad, dSpd in m/s)
struct Movement
{
    double dCrs, dSpd, dClb;
};



/** Generate_Movement
  *
  * Generates the kinematic data based on the
  * geocentric kinematic vector and the loaction
  */
Movement Generate_Movement(Kin_Vect Kin_Dat, LLA_Rad LLA_rCoord, XYZ XYZ_Coord)
{
    Movement Result;

    // Transform geocentric vector to topocentric vector


    UVW UVW_Coord;

    UVW_Coord.dU = -(Kin_Dat.dVx) * sin(LLA_rCoord.dLon) +
        (Kin_Dat.dVy) * cos(LLA_rCoord.dLon);

    UVW_Coord.dV = -(Kin_Dat.dVx) * sin(LLA_rCoord.dLat) * cos(LLA_rCoord.dLon) -
        (Kin_Dat.dVy) * sin(LLA_rCoord.dLat) * sin(LLA_rCoord.dLon) +
        (Kin_Dat.dVz) * cos(LLA_rCoord.dLat);

    UVW_Coord.dW = (Kin_Dat.dVx) * cos(LLA_rCoord.dLat) * cos(LLA_rCoord.dLon) +
        (Kin_Dat.dVy) * cos(LLA_rCoord.dLat) * sin(LLA_rCoord.dLon) +
        (Kin_Dat.dVz) * sin(LLA_rCoord.dLat);


    // Transform  topocentric vector to Mov_Dat

    if (abs(UVW_Coord.dU) < dEPS and abs(UVW_Coord.dV) < dEPS and abs(UVW_Coord.dW) < dEPS)
    {
        Result.dSpd = 0;

        Result.dClb = 0;

        Result.dCrs = 0;

    } else
    {
        Result.dSpd = pow( pow(UVW_Coord.dU,2) + pow(UVW_Coord.dV,2) + pow(UVW_Coord.dW,2), 0.5);

        Result.dCrs = ShiftRadCrs(atan2( UVW_Coord.dU, UVW_Coord.dV ));

        Result.dClb = ShiftRadClb(atan2( UVW_Coord.dW, pow( pow(UVW_Coord.dU,2) + pow(UVW_Coord.dV,2), 0.5) ));
    }


    return Result;
};


/** Generate_Kin_Vect
  *
  * Generates the geocentric kinematic vector based on the
  * kinematic data and the location
  */
Kin_Vect Generate_Kin_Vect(Movement Mov_Dat, LLA_Rad LLA_rCoord, XYZ XYZ_Coord)
{
    Kin_Vect Result;

    //Transform Mov_Dat to topocentric vector
    UVW UVW_Coord;
    // Topocentric Coordinates (U=east, V=north, W=up)

    UVW_Coord.dU = Mov_Dat.dSpd * cos(Mov_Dat.dClb) * sin(Mov_Dat.dCrs);
    UVW_Coord.dV = Mov_Dat.dSpd * cos(Mov_Dat.dClb) * cos(Mov_Dat.dCrs);
    UVW_Coord.dW = Mov_Dat.dSpd * sin(Mov_Dat.dClb);

    //Transform topocentric vector to geocentric vector

    Result.dVx = -UVW_Coord.dU * sin(LLA_rCoord.dLon) -
                 UVW_Coord.dV * sin(LLA_rCoord.dLat) * cos(LLA_rCoord.dLon) +
                 UVW_Coord.dW * cos(LLA_rCoord.dLat) * cos(LLA_rCoord.dLon);

    Result.dVy = UVW_Coord.dU * cos(LLA_rCoord.dLon) -
                 UVW_Coord.dV * sin(LLA_rCoord.dLat) * sin(LLA_rCoord.dLon) +
                 UVW_Coord.dW * cos(LLA_rCoord.dLat) * sin(LLA_rCoord.dLon);

    Result.dVz = UVW_Coord.dV * cos(LLA_rCoord.dLat) +
                 UVW_Coord.dW * sin(LLA_rCoord.dLat);


    return Result;
};


/// Return new position after Timestep
inline XYZ GetNewPosKin(XYZ StartPos, Kin_Vect* Motion, Acc_Vect Acceleration, unsigned int Timestep, unsigned char DR_Algorithm = 5)
{
    XYZ Result;

    double dTimestep = static_cast<double>(Timestep);

    if (DR_Algorithm == 4 or DR_Algorithm == 5)
    {
        Result.dX = StartPos.dX + Motion->dVx * dTimestep / 1000.0 + 0.5 * Acceleration.dAx * pow(dTimestep / 1000.0, 2);
        Result.dY = StartPos.dY + Motion->dVy * dTimestep / 1000.0 + 0.5 * Acceleration.dAy * pow(dTimestep / 1000.0, 2);
        Result.dZ = StartPos.dZ + Motion->dVz * dTimestep / 1000.0 + 0.5 * Acceleration.dAz * pow(dTimestep / 1000.0, 2);

        Motion->dVx += Acceleration.dAx * dTimestep / 1000.0;
        Motion->dVy += Acceleration.dAy * dTimestep / 1000.0;
        Motion->dVz += Acceleration.dAz * dTimestep / 1000.0;
    }
    else
    {
        Result.dX = StartPos.dX + Motion->dVx * dTimestep / 1000.0;
        Result.dY = StartPos.dY + Motion->dVy * dTimestep / 1000.0;
        Result.dZ = StartPos.dZ + Motion->dVz * dTimestep / 1000.0;
    };

    return Result;
}

// Geographic Coordinates in rad
struct LLA_Rad
{
    double dLat, dLon, dAlt;
};


/// Return new position after Timestep
inline LLA_Rad GetNewPos(LLA_Rad StartPos, Movement Motion, unsigned int Timestep)
{
    LLA_Rad Result;

    double Distance = Motion.dSpd * cos(Motion.dClb) * Timestep / 1000.0;

    double DeltaAlt = Motion.dSpd * sin(Motion.dClb) * Timestep / 1000.0;

    Result.dLat = asin( sin(StartPos.dLat) * cos( Distance / dR) + cos(StartPos.dLat) * sin( Distance / dR) * cos(Motion.dCrs));

    Result.dLon = StartPos.dLon + atan2(sin(Motion.dCrs) * sin( Distance / dR) * cos(StartPos.dLat),
                                        cos( Distance / dR) - sin(StartPos.dLat) * sin(Result.dLat));

    Result.dAlt = StartPos.dAlt + DeltaAlt;

    Result = ShiftLLA_Rad(Result);

    return Result;
}