GTA 5: Correct Teleport to waypoint

TeleportToWaypoint()
{
   float Zheight;
   if (UI::IS_WAYPOINT_ACTIVE())
   {
      if (PED::IS_PED_IN_ANY_VEHICLE(PLAYER::PLAYER_PED_ID()))
      {
         int EntityID= PED::GET_VEHICLE_PED_IS_IN(PLAYER::PLAYER_PED_ID(),0);
      }
      else
      {
          int EntityID= PLAYER::PLAYER_PED_ID();
      }
   CAM::DO_SCREEN_FADE_OUT(200);
   Vector3 CoordBlip = UI::GET_BLIP_INFO_ID_COORD(UI::GET_FIRST_BLIP_INFO_ID(UI::_GET_BLIP_INFO_ID_ITERATOR()));
   GAMEPLAY::GET_GROUND_Z_FOR_3D_COORD(CoordBlip.X,CoordBlip.Y,5000,&Zheight);
   Vector3 TeleCoord = new Vector3 ( CoordBlip.X,CoordBlip.Y,Zheight )
   ENTITY::SET_ENTITY_COORDS(EntityID,TeleCoord,1,0,0,1);
   CAM::DO_SCREEN_FADE_IN(200);
   }
   else
   {
      // Message that no waypoint is set
   }
}