{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Generating a Nominal Trajectory\n",
    "\n",
    "This notebook demonstrates how to generate a nominal trajectory in the North-East-Down (NED) local coordinate system and convert it to WGS84 geographic coordinates (longitude, latitude, and height). The trajectory is based on a defined starting point and direction, with the ability to save the resulting positions in a CSV file for further analysis or visualization.\n",
    "\n",
    "## Objective\n",
    "\n",
    "- Define the starting point in geographic coordinates (latitude, longitude, height).\n",
    "- Specify a route direction (azimuth) and slope (pitch) to generate a trajectory.\n",
    "- Compute a series of local NED coordinates based on the specified direction and slope.\n",
    "- Convert the local NED coordinates to WGS84 geographic coordinates (latitude, longitude, height).\n",
    "- Save the resulting trajectory as a CSV file with appropriate headers."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Step 1: Import Required Libraries\n",
    "\n",
    "We begin by importing necessary libraries: "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 39,
   "metadata": {},
   "outputs": [],
   "source": [
    "from sargeom.coordinates import Cartographic, CartesianLocalNED\n",
    "from scipy.spatial.transform import Rotation\n",
    "import numpy as np"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Step 2: Define the NED Frame Origin\n",
    "\n",
    "The origin of the NED coordinate system is defined in terms of latitude, longitude, and height. This point serves as the reference for all local NED coordinates. In this example, we use the following coordinates:\n",
    "\n",
    "- Latitude: 3.2431077847784763490°\n",
    "- Longitude: 42.475181371623002°\n",
    "- Height: 3818 meters"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 40,
   "metadata": {},
   "outputs": [],
   "source": [
    "ned_frame_origin = Cartographic(\n",
    "    longitude=3.2431077847784763490, # degrees\n",
    "    latitude=42.475181371623002, # degrees\n",
    "    height=3818.0 # meters\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Step 3: Specify the Route and Slope\n",
    "\n",
    "The direction of travel (route) and the slope (pitch) of the trajectory are defined in degrees. In this example:\n",
    "\n",
    "- Route (azimuth): 270.615° (heading direction)\n",
    "- Slope (pitch): -0.087° (downward slope)\n",
    "- Step size: 0.28 meters between each sample\n",
    "- Number of trajectory samples: 100\n",
    "- First sample distance from the origin: 10 meters (this will be the distance of the first trajectory point from the NED origin)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 41,
   "metadata": {},
   "outputs": [],
   "source": [
    "heading_angle = 270.615 # degrees\n",
    "pitch_angle = -0.087 # degrees\n",
    "sample_step = 0.28 # meters\n",
    "number_of_samples = 30547\n",
    "initial_distance_from_origin = 3170 # meters"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Step 4: Compute the Direction of Travel\n",
    "\n",
    "We compute the direction of the trajectory in the NED (North-East-Down) coordinate system using a rotation matrix. The `scipy.spatial.transform.Rotation` class allows us to define rotations based on Euler angles. Here we use the \"ZYX\" intrinsic rotation convention to apply the azimuth (route) and pitch (slope)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 42,
   "metadata": {},
   "outputs": [],
   "source": [
    "travel_direction = Rotation.from_euler(\n",
    "    \"ZYX\",  # Intrinsic rotations\n",
    "    [heading_angle, pitch_angle, 0], # Route and slope angles (yaw, pitch, roll)\n",
    "    degrees=True,\n",
    ").apply([1, 0, 0]) # Apply the rotation to the x-axis direction"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Step 5: Define the Trajectory\n",
    "\n",
    "The trajectory is created by generating a sequence of NED coordinates. We define the displacement along the trajectory by multiplying the direction vector by a step size and creating an array of sample points along the trajectory."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 43,
   "metadata": {},
   "outputs": [],
   "source": [
    "distance_samples = initial_distance_from_origin + np.arange(0, number_of_samples) * sample_step # Create array of distances\n",
    "\n",
    "# Generate the trajectory in NED coordinates\n",
    "trajectory_ned = CartesianLocalNED.from_array(np.outer(distance_samples, travel_direction), origin=ned_frame_origin)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Step 6: Convert NED Coordinates to WGS84\n",
    "\n",
    "Once the trajectory is defined in NED coordinates, we convert it to WGS84 geographic coordinates (latitude, longitude, and height) using the .to_ecef().to_cartographic() method chain. This will allow us to obtain the desired geographic positions in degrees and meters."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 44,
   "metadata": {},
   "outputs": [],
   "source": [
    "trajectory_wgs84 = trajectory_ned.to_ecef().to_cartographic()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Step 7: Save the Trajectory to a CSV File\n",
    "\n",
    "Finally, we save the generated trajectory to a CSV file. The CSV file includes headers with descriptions of the coordinate fields, making it easy to understand the data. The file will store the coordinates with appropriate precision."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 45,
   "metadata": {},
   "outputs": [],
   "source": [
    "trajectory_wgs84.save_csv(\"nominal_trajectory.csv\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Conclusion\n",
    "\n",
    "In this notebook, we have demonstrated how to generate a nominal trajectory based on a given route and slope, starting from a specified geographic origin. We have also shown how to convert the local NED coordinates to the WGS84 geographic system and save the result in a CSV file. This process can be extended to more complex trajectory modeling or to visualize the trajectory in 3D space.\n",
    "\n",
    "## Further Reading\n",
    "\n",
    "- [SciPy Spatial Transformations](https://docs.scipy.org/doc/scipy/reference/spatial.transform.html#module-scipy.spatial.transform) for understanding rotation matrices and Euler angles."
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "sargeom-env",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.12.7"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}