struct KinVec{
  double x,y,z;
};
struct MovementVec{
  double dSpd,dClb,dCrs;
};

struct LLA_Rad{
  double dLon,dLat;
};
struct UVW{
  double dU,dV,dW;
};

struct Movement
{
    double dCrs, dSpd, dClb;
};

Eigen::Vector3d Generate_Kin_Vect(MovementVec Mov_Dat, LLA_Rad LLA_rCoord)
{

    //Transform Mov_Dat to topocentric vector
    // UVW UVW_Coord;
    Eigen::Vector3d ENU;
    ENU[0] = Mov_Dat.dSpd * cos(Mov_Dat.dClb) * sin(Mov_Dat.dCrs);
    ENU[1] = Mov_Dat.dSpd * cos(Mov_Dat.dClb) * cos(Mov_Dat.dCrs);
    ENU[2] = Mov_Dat.dSpd * sin(Mov_Dat.dClb);

    //Transform topocentric vector to geocentric vector
    // Eigen::Vector3d UVW(UVW_Coord.dU,UVW_Coord.dV, UVW_Coord.dW);
    LOG_S(INFO)<<"UVW: "<< ENU[0]<< " "<< ENU[1]<< " " << ENU[2] ;

    Eigen::Matrix3d LocalRotationMatrix;

    LocalRotationMatrix << 
    -sin(LLA_rCoord.dLon), - cos(LLA_rCoord.dLon)*sin(LLA_rCoord.dLat) , cos(LLA_rCoord.dLon)*cos(LLA_rCoord.dLat) ,
     cos(LLA_rCoord.dLon), - sin(LLA_rCoord.dLon)* sin(LLA_rCoord.dLat), sin(LLA_rCoord.dLon) *cos(LLA_rCoord.dLat),
     0                     ,    cos(LLA_rCoord.dLat)                        , sin(LLA_rCoord.dLat);


    return LocalRotationMatrix*ENU;
};




struct Kin_Vect
{
    double dVx, dVy, dVz;
};

struct XYZ
{
    double dX,dY,dZ;
};
const double dEPS = 0.000001;

double ShiftRadCrs(double Rad)
{
  while(Rad < 0)
  {
    Rad += 2 * M_PI;
  }

  while (Rad > 2 * M_PI) 
  {
  Rad -= 2 * M_PI;
  }
  return Rad;

};

double ShiftRadClb(double Rad)
{
   while(Rad < -M_PI)
  {
    Rad += 2 * M_PI;
  }

  while (Rad >  M_PI) 
  {
    Rad -= 2 * M_PI;
  }
  return Rad;
};


Movement Generate_Movement(Eigen::Vector3d Kin_Dat, LLA_Rad LLA_rCoord, XYZ XYZ_Coord)
{
    Movement Result;

    // Transform geocentric vector to topocentric vector

    
    Eigen::Vector3d ENU;
    // 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);

     Eigen::Matrix3d LocalRotationMatrix;
     LocalRotationMatrix<<
     - sin(LLA_rCoord.dLon)                         ,   cos(LLA_rCoord.dLon)                          , 0,
     - sin(LLA_rCoord.dLat) * cos(LLA_rCoord.dLon), - sin(LLA_rCoord.dLat) * sin(LLA_rCoord.dLon), cos(LLA_rCoord.dLat),
       cos(LLA_rCoord.dLat) * cos(LLA_rCoord.dLon),   cos(LLA_rCoord.dLat) * sin(LLA_rCoord.dLon), sin(LLA_rCoord.dLat);

    

    ENU = LocalRotationMatrix * Kin_Dat;


    LOG_S(INFO)<<"ENU: "<< ENU[0]<< " "<<ENU[1]<< " " << ENU[2];

    // Transform  topocentric vector to Mov_Dat

    if (abs(ENU[0]) < dEPS && abs(ENU[1]) < dEPS && abs(ENU[2]) < dEPS)
    {
        Result.dSpd = 0;

        Result.dClb = 0;

        Result.dCrs = 0;

    } else
    {
        Result.dSpd = pow( pow(ENU[0],2) + pow(ENU[1],2) + pow(ENU[2],2), 0.5);

        Result.dCrs = ShiftRadCrs(atan2( ENU[0], ENU[1]));
         
        Result.dClb = ShiftRadClb(atan2( ENU[2], pow( pow(ENU[0],2) + pow(ENU[1],2), 0.5) ));

    }


    return Result;
};


 SimCore::Position  updatePosition(SimCore::Position ownPosition_, MovementVec mov,double dt)
    {

       
        
            

            // Create ECEF coordinate system based on reference point
            double lat = SimCore::UtilFunctions::DegToRad(ownPosition_.getGeodesicPos()(SimCore::LATITUDE));  // in radians
            double lon = SimCore::UtilFunctions::DegToRad(ownPosition_.getGeodesicPos()(SimCore::LONGITUDE));  // in radians
            double alt = ownPosition_.getGeodesicPos()(SimCore::HEIGHT);  // in meters

            LLA_Rad LLA_rCoord;
            LLA_rCoord.dLat = lat;
            LLA_rCoord.dLon = lon;

            // Matrix3d RotationMatrix = getRotationMatrix(lat, lon);        
            
            // Vector3d ecefVelocityVector = RotationMatrix * kinematicVec_;

            Eigen::Vector3d ecefVelocityVector = Generate_Kin_Vect(mov, LLA_rCoord);
            Eigen::Vector3d accelerationVec_(0,0,0);

            double X,Y,Z;    
            X = ownPosition_.getGeocentricPos()(SimCore::X) +  ecefVelocityVector.x() * (dt / 1000.0) + 0.5 * accelerationVec_.x() * pow((dt / 1000.0), 2);
            Y = ownPosition_.getGeocentricPos()(SimCore::Y) +  ecefVelocityVector.y() * (dt / 1000.0) + 0.5 * accelerationVec_.y() * pow((dt / 1000.0), 2);
            Z = ownPosition_.getGeocentricPos()(SimCore::Z) +  ecefVelocityVector.z() * (dt / 1000.0) + 0.5 * accelerationVec_.z() * pow((dt / 1000.0), 2);


            // kinematicVec_.x() += accelerationVec_.x() * dt / 1000.0;
            // kinematicVec_.y() += accelerationVec_.y() * dt / 1000.0;
            // kinematicVec_.z() += accelerationVec_.z() * dt / 1000.0;
          SimCore::Position newPos(X, Y, Z);
          return newPos;
       
    }