#
#  ISC License
#
#  Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder
#
#  Permission to use, copy, modify, and/or distribute this software for any
#  purpose with or without fee is hereby granted, provided that the above
#  copyright notice and this permission notice appear in all copies.
#
#  THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
#  WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
#  MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
#  ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
#  WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
#  ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
#  OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
#

r"""
Overview
--------

This scenario demonstrates how to set up a spacecraft orbiting a celestial body. The gravity can be a first order
approximation or run with high-order spherical harmonic terms.  The following diagram illustrates how the
Basilisk components are interconnected.

.. image:: /_images/static/test_scenarioBasicOrbit.svg
   :align: center

The script is found in the folder ``basilisk/examples`` and executed by using::

    python3 scenarioBasicOrbit.py

.. important:: This scenario is currently the most minimal example available. If it doesn't make sense consider
                   returning to the :ref:`examples` before moving on.


Making a Copy of the Example Basilisk Scenario Script
-----------------------------------------------------

However, to play with any scenario scripts as tutorials, you should make a copy of
them into a custom folder outside of the Basilisk directory.

To copy them, first find the location of the Basilisk installation. After installing, you can find the
installed location of Basilisk by opening a python interpreter and running the commands:

.. code-block:: python

   import Basilisk
   basiliskPath = Basilisk.__path__[0]
   print basiliskPath

Copy the folder ``{basiliskPath}/examples`` into a new folder in a different directory.
Now, when you want to use a tutorial, navigate inside that folder, and edit and execute the copied integrated tests.


Enable Terminal Bar to Show Simulation Progress
-----------------------------------------------

To enable progress bar, one need to set ``showProgressBar`` data member of class SimulationBaseClass to true.

.. code-block:: python

     scSim = SimulationBaseClass.SimBaseClass()
     scSim.SetProgressBar(True)

Method ``SetProgressBar`` should be used to set variable ``showProgressBar`` as True with the above statement. After
enabling the progress bar, all the simulation run by ``scSim.ExecuteSimulation()`` will show the progress bar in the
terminal.


Vizard Visualization Option
---------------------------

If you wish to transmit the simulation data to the United based `Vizard <vizard>`_ Visualization application,
then uncomment
the following line from the python scenario script. This will cause the BSK simulation data
to be stored in a binary file inside the _VizFiles sub-folder with the scenario folder. This file can be read in by
`Vizard <vizard>`_ and played back after running the BSK simulation. ::

    vizSupport.enableUnityVisualization(scSim, simTaskName, simProcessName, gravBodies=gravFactory, saveFile=fileName)

The vizInterface module must be built into BSK. This is done if the correct `CMake options <cmakeOptions>`_
are selected. The default
CMake will include this vizInterface module in the BSK build. See the BSK HTML documentation on more
information of CMake options. By using the gravFactory support class to create and add planetary bodies
the vizInterface module will automatically be able to find the correct celestial body ephemeris names.
If these names are changed, then the vizSupport.py support library has to be customized. Currently Vizard
supports playback of stored simulation data files, as well as streaming the data directly to Vizard.
By default the Viz is running in realtime mode with a 1x
speed up factor of the data playback. On the bottom right of the Vizard GUI this can be increased or decreased.
Further, some display elements such as thruster or reaction wheel panels are only visible if such devices are
being simulated in BSK.

While Vizard has many visualization features that can be customized from within the application, many
Vizard settings can also be scripted from the Basilisk python script.
A complete discussion on these options and features can be found the the `Vizard Settings <vizardSettings>`_ page.


Illustration of Simulation Results
----------------------------------

The following images illustrate the expected simulation run returns for a range of script configurations.

::

    show_plots = True, orbitCase='LEO', useSphericalHarmonics=False, planetCase='Earth'

This scenario places the spacecraft about the Earth in a LEO orbit and without considering gravitational
spherical harmonics.

.. image:: /_images/Scenarios/scenarioBasicOrbit1LEO0Earth.svg
   :align: center

.. image:: /_images/Scenarios/scenarioBasicOrbit2LEO0Earth.svg
   :align: center

.. image:: /_images/Scenarios/scenarioBasicOrbit3LEO0Earth.svg
   :align: center

::

    show_plots = True, orbitCase='GTO', useSphericalHarmonics=False, planetCase='Earth'

This case illustrates an elliptical Geosynchronous Transfer Orbit (GTO) with zero orbit inclination.

.. image:: /_images/Scenarios/scenarioBasicOrbit1GTO0Earth.svg
   :align: center

.. image:: /_images/Scenarios/scenarioBasicOrbit2GTO0Earth.svg
   :align: center

.. image:: /_images/Scenarios/scenarioBasicOrbit3GTO0Earth.svg
   :align: center

::

    show_plots = True, orbitCase='GEO', useSphericalHarmonics=False, planetCase='Earth'

This case illustrates a circular Geosynchronous Orbit (GEO) with zero orbit inclination.

.. image:: /_images/Scenarios/scenarioBasicOrbit1GEO0Earth.svg
   :align: center

.. image:: /_images/Scenarios/scenarioBasicOrbit2GEO0Earth.svg
   :align: center

.. image:: /_images/Scenarios/scenarioBasicOrbit3GEO0Earth.svg
   :align: center

::

    show_plots = True, orbitCase='LEO', useSphericalHarmonics=True, planetCase='Earth'

This case illustrates a circular LEO with a non-zero orbit inclination.
In this case the Earth's spherical harmonics are turned on.

.. image:: /_images/Scenarios/scenarioBasicOrbit1LEO1Earth.svg
   :align: center

.. image:: /_images/Scenarios/scenarioBasicOrbit2LEO1Earth.svg
   :align: center

::

    show_plots = True, orbitCase='LEO', useSphericalHarmonics=True, planetCase='Mars'

This case illustrates a circular Low Mars Orbit or LMO with a non-zero orbit inclination.
In this simulation setup the planet's spherical harmonics are turned on.

.. image:: /_images/Scenarios/scenarioBasicOrbit1LEO0Mars.svg
   :align: center

.. image:: /_images/Scenarios/scenarioBasicOrbit2LEO0Mars.svg
   :align: center

.. image:: /_images/Scenarios/scenarioBasicOrbit3LEO0Mars.svg
   :align: center

"""



#
# Basilisk Scenario Script and Integrated Test
#
# Purpose:  Integrated test of the spacecraft() and gravity modules.  Illustrates
#           a 3-DOV spacecraft on a range of orbit types.
# Author:   Hanspeter Schaub
# Creation Date:  Nov. 26, 2016
#

import os
import numpy as np
from astropy.time import Time
import ctypes
import spiceypy
import time
#import csv

import matplotlib.pyplot as plt
from copy import copy

# To play with any scenario scripts as tutorials, you should make a copy of them into a custom folder
# outside of the Basilisk directory.
#
# To copy them, first find the location of the Basilisk installation.
# After installing, you can find the installed location of Basilisk by opening a python interpreter and
# running the commands:
from Basilisk import __path__
bskPath = __path__[0]
fileName = os.path.basename(os.path.splitext(__file__)[0])

# Copy the folder `{basiliskPath}/examples` into a new folder in a different directory.
# Now, when you want to use a tutorial, navigate inside that folder, and edit and execute the *copied* integrated tests.


# import simulation related support
from Basilisk.simulation import spacecraft
from Basilisk.simulation import svIntegrators
# general support file with common unit test functions
# import general simulation support files
from Basilisk.utilities import SimulationBaseClass
from Basilisk.utilities import macros
from Basilisk.utilities import orbitalMotion
from Basilisk.utilities import simIncludeGravBody
from Basilisk.utilities import unitTestSupport
from Basilisk.utilities import vizSupport

# always import the Basilisk messaging support
from Basilisk.architecture import messaging

from Basilisk.topLevelModules import pyswice

def orbprop(gravN, simStep, simDur):
    """
    Args:
        gravN (int): number of gravity field spherical harmonics used
        simDur (double): simulation duration [days]
        simStep (double): propagation step [s]
    """

    runstart = time.time()
    
    ##########################################################
    ############ Create simulation variable names ############
    ##########################################################
    
    spiceypy.spiceypy.furnsh('/opt/Basilisk/supportData/EphemerisData/earth_000101_220328_220103.bpc')
    spiceypy.spiceypy.furnsh('/opt/Basilisk/supportData/EphemerisData/naif0012.tls')
    spiceypy.spiceypy.furnsh('/opt/Basilisk/supportData/EphemerisData/pck00010.tpc')
    
    simTaskName = "simTask"
    simProcessName = "simProcess"
    
    #timeInitString = '2020 DEC 04 00:00:58 (UTC)'
    
    orbitephem_name = '/opt/Basilisk/Simulations/Basilisk_RO_TSX_90days_10s_20201204_005800_GGM03S.csv'
    
    # astrotimeInitString = '2020-12-04T00:00:58'
    # 
    # astrotimeInit = Time(astrotimeInitString, format='isot', scale='utc')
    # astrotimeInit_GPSsecs = astrotimeInit.gps
    
    # TerraSAR-X
    astrotimeInitString = '2020/12/04 00:58:00'
    astrotimeInit_GPSsecs = spiceypy.spiceypy.str2et(astrotimeInitString) + round(630763148.816)
    
    rN = np.zeros(3)
    vN = np.zeros(3)
    
    rN[0] = -138514.048884
    rN[1] = -888553.853077
    rN[2] =  6818674.278964
    
    vN[0] = -7306.6601
    vN[1] =  2126.208457
    vN[2] =  128.787296
    
    simulationTimeStep = macros.sec2nano(simStep)
    simulationTime = macros.sec2nano(simDur*86400.0)
    
    ##########################################################
    ##########################################################
    ##########################################################

    #  Create a sim module as an empty container
    scSim = SimulationBaseClass.SimBaseClass()

    # (Optional) If you want to see a simulation progress bar in the terminal window, the
    # use the following SetProgressBar(True) statement
    scSim.SetProgressBar(True)

    #  create the simulation process
    dynProcess = scSim.CreateNewProcess(simProcessName)

    # create the dynamics task and specify the integration update time
    # simulationTimeStep = macros.sec2nano(10.)
    dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep))
    # Note that a priority of 100 is chosen to ensure this task runs before the logging task below
    # which should come last.  Higher priority tasks are evaluated first.

    # setup the simulation tasks/objects
    # initialize spacecraft object and set properties
    # The dynamics simulation is setup using a Spacecraft() module.
    scObject = spacecraft.Spacecraft()
    scObject.ModelTag = "TSX"
    
    # add spacecraft object to the simulation process
    scSim.AddModelToTask(simTaskName, scObject)

    # setup Gravity Body
    # The first step to adding gravity objects is to create the gravity body factor class.  Note that
    # this call will create an empty gravitational body list each time this script is called.  Thus, there
    # is not need to clear any prior list of gravitational bodies.
    gravFactory = simIncludeGravBody.gravBodyFactory()

    # To attach an Earth gravity model to this spacecraft, the following macros  invoked.
    # The gravFactor() class stores the Earth gravitational object within the class, but it also returns a
    # handler to this gravitational object as a convenience.  The celestial object position and velocity
    # vectors are all defaulted to zero values.  If non-zero values are required, this can be manually
    # overridden.  If multiple bodies are simulated, then their positions can be
    # dynamically updated.  See scenarioOrbitMultiBody.py to learn how this is
    # done via a SPICE object.
    planet = gravFactory.createEarth()
    planet.isCentralBody = True          # ensure this is the central gravitational body
    # If extra customization is required, see the createEarth() macro to change additional values.
    # For example, the spherical harmonics are turned off by default.  To engage them, the following code
    # is used
    planet.useSphericalHarmParams = True
    # simIncludeGravBody.loadGravFromFile(bskPath + '/supportData/LocalGravData/GGM03S.txt', planet.spherHarm, 70)
    simIncludeGravBody.loadGravFromFile('/opt/Basilisk/supportData/LocalGravData/GGM03S.txt', planet.spherHarm, gravN)
    # simIncludeGravBody.loadGravFromFile(bskPath + '/supportData/LocalGravData/GGM03S-J2-only.txt',
    #                                     planet.spherHarm, 2)
    # The value 2 indicates that the first two harmonics, excluding the 0th order harmonic,
    # are included.  This harmonics data file only includes a zeroth order and J2 term.   
    mu = planet.mu

    # Finally, the gravitational body must be connected to the spacecraft object.  This is done with
    scObject.gravField.gravBodies = spacecraft.GravBodyVector(list(gravFactory.gravBodies.values()))
    # Here the complete list of gravitational bodies is automatically assigned to the spacecraft, regardless if
    # it is only one body like Earth or Mars, or a list of multiple bodies.

    # Note that the default planets position and velocity vectors in the gravitational body are set to zero.  If
    # alternate position or velocity vectors are required, this can be done by creating the planet ephemeris message
    # that is connected to the gravity effector input message `planetBodyInMsg`.
    # If time varying planet ephemeris messages are to be included use the Spice module.  For non-zero messages
    # the planet's default ephemeris would be replaced with the desired custom values.

    # rN, vN = orbitalMotion.elem2rv(mu, oe)
    oe = orbitalMotion.ClassicElements()
    oe = orbitalMotion.rv2elem(mu, rN, vN)      # this stores consistent initial orbit elements
    # with circular or equatorial orbit, some angles are arbitrary

    # To set the spacecraft initial conditions, the following initial position and velocity variables are set:
    scObject.hub.r_CN_NInit = rN  # m   - r_BN_N
    scObject.hub.v_CN_NInit = vN  # m/s - v_BN_N

    # These vectors specify the inertial position and velocity vectors relative to the planet of the
    # spacecraft center of mass location.  Note that there are 2 points that can be tracked.  The user always
    # specifies the spacecraft center of mass location with the above code.  If the simulation output should be
    # about another body fixed point B, this can be done as well.  This is useful in particular with more challenging
    # dynamics where the center of mass moves relative to the body.  The following vector would specify the location of
    # the spacecraft hub center of mass (Bc) relative to this body fixed point, as in
    #
    #    scObject.hub.r_BcB_B = [[0.0], [0.0], [1.0]]
    #

    # If this vector is not specified, as in this tutorial scenario, then it defaults to zero.  If only a rigid hub
    # is modeled, the Bc (hub center of mass) is the same as C (spacecraft center of mass).  If the spacecraft contains
    # state effectors such as hinged panels, fuel slosh, imbalanced reaction wheels, etc., then the points
    # Bc and C would not be the same.  Thus, in this simple simulation the body fixed point B and
    # spacecraft center of mass are identical.

    # Setup data logging before the simulation is initialized
    # The msg recorder can be told to sample at an with a minimum hold period in nano-seconds.
    # If no argument is provided, i.e. msg.recorder(), then a default 0ns minimum time period is used
    # which causes the msg to be recorded on every task update rate.
    # The recorder can be put onto a separate task with its own update rate.  However, this can be
    # trickier to do as the recording timing must be carefully balanced with the module msg writing
    # to avoid recording an older message.
    # samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints)
    # # create a logging task object of the spacecraft output message at the desired down sampling ratio
    # dataRec = scObject.scStateOutMsg.recorder(samplingTime)
    dataRec = scObject.scStateOutMsg.recorder()
    scSim.AddModelToTask(simTaskName, dataRec)

    # Vizard Visualization Option
    # ---------------------------
    # If you wish to transmit the simulation data to the United based Vizard Visualization application,
    # then uncomment the following
    # line from the python scenario script.  This will cause the BSK simulation data to
    # be stored in a binary file inside the _VizFiles sub-folder with the scenario folder.  This file can be read in by
    # Vizard and played back after running the BSK simulation.
    # To enable this, uncomment this line:

    viz = vizSupport.enableUnityVisualization(scSim, simTaskName, scObject,
                                              # saveFile=__file__,
                                              # liveStream=True,
                                              )

    # The vizInterface module must be built into BSK.  This is done if the correct CMake options are selected.
    # The default CMake will include this vizInterface module in the BSK build.  See the BSK HTML documentation on
    # more information of CMake options.

    # By using the gravFactory support class to create and add planetary bodies the vizInterface module will
    # automatically be able to find the correct celestial body ephemeris names.  If these names are changed, then the
    # vizSupport.py support library has to be customized.
    # Currently Vizard supports playback of stored simulation data files as well as live streaming.
    # Further, some display elements such as thruster or reaction wheel panels are only visible if
    # such devices are being simulated in BSK.

    # While Vizard has many visualization features that can be customized from within the application, many Vizard
    # settings can also be scripted from the Basilisk python script.  A complete discussion on these options and
    # features can be found the the Vizard documentation pages.

    # Before the simulation is ready to run, it must be initialized.  The following code uses a
    # convenient macro routine
    # which initializes each BSK module (run self init, cross init and reset) and clears the BSK logging stack.

    #   initialize Simulation:  This function runs the self_init()
    #   cross_init() and reset() routines on each module.
    scSim.InitializeSimulation()

    #   configure a simulation stop time time and execute the simulation run
    scSim.ConfigureStopTime(simulationTime)
    scSim.ExecuteSimulation()
    # Note that this module simulates both the translational and rotational motion of the spacecraft.
    # In this scenario only the translational (i.e. orbital) motion is tracked. This means the rotational motion
    # remains at a default inertial frame orientation in this scenario. There is no appreciable speed hit to
    # simulate both the orbital and rotational motion for a single rigid body.
    # Further, the default spacecraft parameters, such as the unit mass and the principle inertia values are
    # just fine for this orbit simulation as they don't impact the orbital dynamics in this case.
    # This is true for all gravity force only orbital simulations.

    # Retrieve the logged data
    # the data is stored inside dataLog variable.  The time axis is stored separately from the data vector and
    # can be access through dataLog.times().  The message data is access directly through the message
    # variable names.
    simTime = dataRec.times()
    simTime = macros.NANO2SEC*simTime
    simTime_GPSsecs = simTime + astrotimeInit_GPSsecs;
    simTime_col = simTime.reshape(-1, 1)
    simTime_GPSsecs_col = simTime_GPSsecs.reshape(-1, 1)
    
    posData = dataRec.r_BN_N
    velData = dataRec.v_BN_N
    
    #print(posvelData)
    
    posvelData_ECEF = np.column_stack((posData, velData))
    
    ind = 0
    for t,pos,vel in zip(simTime_GPSsecs, posData, velData):
      
      GPSsecs = t
      posvelData = np.concatenate([pos,vel])
      posvelecef = ECItoECEF(GPSsecs, posvelData)
      
      posvelData_ECEF[ind,:] = posvelecef
      
      #print(posvelecef)
      
      ind = ind + 1
    
    statemat = np.column_stack((simTime_col, simTime_GPSsecs_col, posData, velData, posvelData_ECEF))
    
    np.set_printoptions(precision=16)
    
    #print(type(posData))
    #csvwriter.writerow(statemat)
    #csvfile.close()
    
    #statemat.tofile('/opt/Basilisk/Simulations/test.csv',sep=',')#,format='%10.5f')
    #np.savetxt('/opt/Basilisk/Simulations/test.csv', statemat, fmt='%.5f', delimiter=",")
    np.savetxt(orbitephem_name, statemat, fmt='%i, %i, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f')

    # When the simulation completes 2 plots are shown for each case.  One plot always shows
    # the inertial position vector components, while the second plot either shows a planar
    # orbit view relative to the peri-focal frame (no spherical harmonics), or the
    # semi-major axis time history plot (with spherical harmonics turned on).
    # figureList, finalDiff = plotOrbits(dataRec.times(), posData, velData, oe, mu, P,
    #                         orbitCase, useSphericalHarmonics, planetCase, planet)
    # 
    # if show_plots:
    #     plt.show()
    # 
    # # close the plots being saved off to avoid over-writing old and new figures
    # plt.close("all")
    # 
    # return finalDiff, figureList
    
    runend = time.time()
    
    print(runend - runstart)
    
    return



def GPStoET(GPSsecs):
                                
    ETsecs = GPSsecs - 630763148.816;
    return ETsecs

def ECItoECEF(GPSsecs, posvelData):
                                
    ETsecs = GPSsecs - 630763148.816;
    
    T_ECI2ECEF = spiceypy.spiceypy.sxform("J2000", "ITRF93", ETsecs)
    
    posvelData_ECEF = T_ECI2ECEF.dot(posvelData)
    
    return posvelData_ECEF



if __name__ == "__main__":
    orbprop(70, 10.0, 90.0)
