{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# ExoMarsRSP SPICE Kernel Dataset Testcase "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This Jupyter Notebook is used to illustrate some examples on how to use the testcase provided in the ExoMarsRSP SKD v0.1.0. This SKD features the following scenario:\n",
    "\n",
    "This is the first release of the ExoMarsRSP SPICE Kernel Dataset it is in draft state and it is aimed to provide the ROCC the capability to start testing SPICE in their systems. It includes a basic set of reference frames defined for the RM structures and PanCam and includes sample IKs for NavCam and PanCam, it also includes an example of an structures SPKs and several sample SPKs and CKs.\n",
    "\n",
    "The scenario that this release covers simulates a first travel of the rover and a first observation by moving the payload bench on top of the mast by a given pan and tilt set of angles. The travel of the rover can be depicted as follows (XY plane):\n",
    "\n",
    "            .----------------.\n",
    "          .                   .\n",
    "         .                     .                   +Ydisp\n",
    "         |                     |                     ^\n",
    "         |                     |                     |  Rover Disposal\n",
    "         |                      .                    |\n",
    "         |                       '.__________________o----->\n",
    "         |                                                 +Xdisp\n",
    "         |\n",
    "         |\n",
    "         |\n",
    "         X End Point\n",
    "\n",
    "\n",
    "The timeline of the scenario is as follows:\n",
    "\n",
    "         2020 NOV 01 00:00:00 TDB  -  Lander Disposal\n",
    "         2020 NOV 01 00:00:01 TDB  -  Drive 5 meters\n",
    "         2020 NOV 01 00:01:42 TDB  -  Turn Right\n",
    "         2020 NOV 01 00:02:43 TDB  -  Drive 5 meters\n",
    "         2020 NOV 01 00:04:06 TDB  -  Turn Left\n",
    "         2020 NOV 01 00:04:34 TDB  -  Drive 3.7 meters\n",
    "         2020 NOV 01 00:05:46 TDB  -  Stop at ( -1.3e-02 -2.e-03 0 ) [km]\n",
    "\n",
    "         2020 NOV 01 01:04:22 UTC  -  Start PTR Pan to 20 degrees\n",
    "         2020 NOV 01 01:04:40 UTC  -  PTR Pan at 20 degrees\n",
    "         2020 NOV 01 01:04:41 UTC  -  Start PTR Tilt to 10 degrees\n",
    "         2020 NOV 01 01:04:42 UTC  -  PTR Tilt at 10 degrees\n",
    "\n",
    "         2020 NOV 01 10:04:50 UTC  -  Start PTR Tilt to 0 degrees\n",
    "         2020 NOV 01 10:10:04 UTC  -  PTR Tilt at 0 degrees\n",
    "         2020 NOV 01 10:10:07 UTC  -  Start PTR Pan to 0 degrees\n",
    "         2020 NOV 02 10:10:19 UTC  -  PTR Pan at 0 degrees\n",
    "\n",
    "         2020 NOV 02 00:00:00 TDB  -  End of test scenario\n",
    "         \n",
    "First we load the basic Python Packages we need including SpiceyPy."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "import matplotlib as mpl\n",
    "mpl.rcParams['figure.figsize'] = (20,10)\n",
    "\n",
    "import spiceypy as cspice\n",
    "import matplotlib.pyplot as plt\n",
    "import numpy as np"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We now load the EMRSP meta-kernel and define a series of Time Windows that we will use to plot different quantities. We use the CSPICE FURNSH and STR2ET APIs for this purpose."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "cspice.furnsh('/Users/mcosta/ExoMarsRSP/kernels/mk/emrsp_preops_local.tm')\n",
    "\n",
    "start_time = '2020-11-01 00:00:00 TDB'\n",
    "finish_time = '2020-11-01 23:00:00 TDB'\n",
    "\n",
    "start_et = cspice.str2et(start_time)\n",
    "finish_et = cspice.str2et(finish_time)\n",
    "\n",
    "interval_state = np.arange(start_et, finish_et, 5.0)\n",
    "\n",
    "start_time = '2020-11-01 00:00:00 TDB'\n",
    "finish_time = '2020-11-01 00:05:00 TDB'\n",
    "\n",
    "start_et = cspice.str2et(start_time)\n",
    "finish_et = cspice.str2et(finish_time)\n",
    "\n",
    "interval_rm = np.arange(start_et, finish_et, 5.0)\n",
    "\n",
    "start_time = '2020-11-01 00:03:00 TDB'\n",
    "finish_time = '2020-11-01 23:50:00 TDB'\n",
    "\n",
    "start_et = cspice.str2et(start_time)\n",
    "finish_et = cspice.str2et(finish_time)\n",
    "\n",
    "interval_htc = np.arange(start_et, finish_et, 5.0)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We now loop through those time intervals to retrieve some quantities:\n",
    "\n",
    "1-Position of the Rover w.r.t the Disposal Site (Landing Site). We use the SPICE API SPKEZR for that.\n",
    "\n",
    "2-Angular Offset of the Rover +X axis (driving direction) w.r.t to the -X axis of the Mars Local Geodetic frame of the Disposal Site. We use the SPICE APIs PXFORM, MXV and VSEP for that.\n",
    "\n",
    "3-We retrieve the SPICE variables for the PanCam HRC stored in SPICE, we plot them and then we calculate the Euler Angles of the rotation of the HRC boresight w.r.t. the Rover Mast. We use the SPICE APIs BODN2C, GETFOV, PXFORM and M2EUL for that purpose."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "RM_PANCAM_HRC shape:   RECTANGLE\n",
      "RM_PANCAM_HRC frame:   RM_PANCAM_HRC\n",
      "RM_PANCAM_HRC bsight:  [0. 0. 1.]\n",
      "RM_PANCAM_HRC vectors: 4\n",
      "[[ 0.04253463  0.04253463  0.99818917]\n",
      " [-0.04253463  0.04253463  0.99818917]\n",
      " [-0.04253463 -0.04253463  0.99818917]\n",
      " [ 0.04253463 -0.04253463  0.99818917]]\n"
     ]
    }
   ],
   "source": [
    "rm_state = []\n",
    "for et in interval:\n",
    "\n",
    "    rm_state.append(cspice.spkezr('EXOMARS ROVER',et, 'RM_MLG_DISPOSAL',\n",
    "                                  'NONE', 'RM_SITE_DISPOSAL')[0])\n",
    "\n",
    "rm_xoffset = []\n",
    "for et in interval_rm:\n",
    "\n",
    "\n",
    "    mat = cspice.pxform('RM_MLG_DISPOSAL', 'RM_ROVER', et)\n",
    "    mlg_x  = cspice.mxv(mat, [1,0,0])\n",
    "    ang = cspice.vsep(mlg_x, [1,0,0])*cspice.dpr()\n",
    "    rm_xoffset.append(ang)\n",
    "\n",
    "\n",
    "pancam_hrc_euler_1 = []\n",
    "pancam_hrc_euler_2 = []\n",
    "pancam_hrc_euler_3 = []\n",
    "\n",
    "sensor_name = 'RM_PANCAM_HRC'\n",
    "\n",
    "sensor_id = cspice.bodn2c(sensor_name)\n",
    "(shape, frame, bsight, vectors, bounds) = cspice.getfov(sensor_id, 100)\n",
    "\n",
    "print('{} shape:   {}'.format(sensor_name, shape))\n",
    "print('{} frame:   {}'.format(sensor_name, frame))\n",
    "print('{} bsight:  {}'.format(sensor_name, bsight))\n",
    "print('{} vectors: {}'.format(sensor_name, vectors))\n",
    "print(bounds)\n",
    "\n",
    "for et in interval_htc:\n",
    "\n",
    "    mat = cspice.pxform(frame, 'RM_PTR_ZERO', et)\n",
    "    eul = cspice.m2eul(mat,3,2,1)\n",
    "    pancam_hrc_euler_1.append(cspice.dpr()*eul[0])\n",
    "    pancam_hrc_euler_2.append(cspice.dpr()*eul[1])\n",
    "    pancam_hrc_euler_3.append(cspice.dpr()*eul[2])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As a test we check that the final velocity of the Rover is aligned with the driving direction."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "(array([0., 0., 0.]), 0.0)\n",
      "(array([6.123234e-17, 0.000000e+00, 1.000000e+00]), 1.0)\n"
     ]
    }
   ],
   "source": [
    "print(cspice.unorm(rm_state[-1][3:6]))\n",
    "\n",
    "mat = cspice.invert(mat)\n",
    "vec  = cspice.mxv(mat, [1,0,0])\n",
    "\n",
    "print(cspice.unorm(vec))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Finally we plot the quantities that we computed before."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "[encoded data, 32740 characters]\n",
      "text/plain": [
       "<Figure size 1440x720 with 1 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 46072 characters]\n",
      "text/plain": [
       "<Figure size 1440x720 with 1 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 22496 characters]\n",
      "text/plain": [
       "<Figure size 1440x720 with 1 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "x, y = [], []\n",
    "\n",
    "for element in rm_state:\n",
    "    x.append(element[0])\n",
    "    y.append(element[1])\n",
    "\n",
    "fig = plt.figure()\n",
    "\n",
    "plt.plot(x,y)\n",
    "plt.title('ExoMars RSP Rover Module Testcase Travel')\n",
    "plt.ylabel('Y - Distance [km]')\n",
    "plt.xlabel('X - Distance [km]')\n",
    "plt.show()\n",
    "\n",
    "fig = plt.figure()\n",
    "\n",
    "plt.plot(interval_rm,rm_xoffset)\n",
    "plt.title('ExoMars RSP Rover Module Testcase X-Offset')\n",
    "plt.ylabel('+X offset [deg]')\n",
    "plt.xlabel('Ephemeris Time [s]')\n",
    "plt.show()\n",
    "\n",
    "fig = plt.figure()\n",
    "ax1 = fig.add_subplot(111)\n",
    "\n",
    "ax1.plot(interval_htc,pancam_hrc_euler_1, label='Euler 1')\n",
    "ax1.plot(interval_htc,pancam_hrc_euler_2, label='Euler 2')\n",
    "plt.title('ExoMars RSP Rover Module PanCam HTC Euler Angles w.r.t Mast')\n",
    "plt.legend(loc='upper right');\n",
    "plt.ylabel('Angle [deg]')\n",
    "plt.xlabel('Ephemeris Time [s]')\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "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.6.4"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
