##^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^Lance Simms, Lawrence Livermore National Laboratory (2011)
##^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^Extended code from Matthew Doty (2011)
#!/usr/bin/env python
import ephem
import os
import numpy as np
import numpy.linalg as la
import cgkit.cgtypes.quat as quat
import cgkit.cgtypes.vec3 as vec3
import cgkit.cgtypes.vec4 as vec4
import cgkit.cgtypes.mat3 as mat3
import cgkit.cgtypes.mat4 as mat4
import csv
import pdb
from sys import argv
from os.path import dirname, abspath,sep
from string import split
from math import sin, cos, tan, atan2, exp, pi
from pylab import sign

# Source Directory
src_dir = dirname(abspath(__file__)) + sep

# Spacecraft orbit from TLE file
tle_file = src_dir + sep + "cxrb.tle"

###############################################################################################################
#  
# NAME: 
#   c (constants)
# PURPOSE:
#   A simple class to define constants 
#
###############################################################################################################
class c:

  #Instantiation
  def __init__(self):

    #First add the instantaneous velocity of the spacecraft to the earth's rotational velocity
    self.G           = 6.673e-11 #Gravitational Constant (m^3 kg^-1 s^-2)
    self.EARTH_MASS  = 5.974e24  #Mass of earth (kg)
    self.rho_0       = 5.31e-13  #Density of air at 500 km (in kg/m^3)
    self.EARTH_R     = 6378100.  #Radius of Earth in Meters  
    self.OMEGA_EARTH = 7.2722e-5 #Angular velocity of earth in rads per second (assuming full rotation in 24 hrs)

###############################################################################################################
#  
# NAME: 
#   degrees
# PURPOSE:
#   Converts Radians to Degrees
# INPUTS: 
#    rads: float
#
###############################################################################################################

def degrees(rads):
  "Converts radians to degrees"
  return rads / ephem.pi * 180


###############################################################################################################
# 
# NAME: 
#   jd2gdy
# PURPOSE:
#   Converts a Julian date to a Gregorian Decimal Year
# INPUTS: 
#   julian_date: float
#     The Julian date
# RETURNS:
#   gregorian_date: float
#     The Gregorian Decimal year
# 
###############################################################################################################

def jd2gdy(julian_date):
  "Converts a Julian date to a Gregorian Decimal Year"
  return (julian_date - ephem.julian_date('2000/1/1')) / 365.25 + 2000


###############################################################################################################
# 
# NAME: 
#   getmagfield
# PURPOSE:
#   Returns a numpy array containing the geomagnetic field vector at a particular latititude, longitude, 
#   altitude, and date.  
#   - Latitude and longitude must be in signed degrees
#   - Altitude must be in kilometers from average sea level
#   - Date must be in Gregorian Decimal Years
#
# INPUTS: 
#   lat: float
#     The latitude in signed degress
#   lon: float
#     The longitude in signed degrees
#   alt: float
#     The altitude in kilometers from sea level
#   date: float
#     The date in Gregorian Decimal Year
#
# RETURNS:
#   MagFieldVector : 1x3 float array
#     The returned vector of the magnetic field at the latitude, longitude, and altitude.  The 
#     components of the vector are the following:
#     
#     B_x = The component of the magnetic field in the Northerly direction
#     B_y = The component of the magnetic field in the Easterly direction
#     B_z = The component of the magentic field in the Nadir direction (pointed toward center of earth)
#
#     NOTE: -------
#      - wmm returns the B field in Nanoteslas
#      - getmagfield() returns the B field in teslas
#
# NOTES: 
#   This function must be run from the CXBNNanoSatSimulation directory, and this directory must contain
#   the WMM subdirectory.  In order for this to work, you must have the following files present:
#     CXBNNanoSatSimulation/WMM/wmm
#     CXBNNanoSatSimulation/EGM9615.BIN
#     CXBNNanoSatSimulation/WMM.COF
#
###############################################################################################################
def getmagfield(lat,lon,alt,date):
    """Return a numpy array containing the geomagnetic field vector at a particular latitude, longitude, altitude and date.
  - Latitude and longitude must be in signed degrees, 
  - Altitude must be in kilometers from average sea level
  - Date must be in format YYYY/MM/DD"""

    #Form the command that will be applied via the command line
    cmd = "./WMM/wmm " + str(lat) + " " + str(lon) + " " + str(alt) + " " +  date

    #Place the output in a file
    wmm = os.popen(cmd)
    out = split(wmm.read(),' ')
    wmm.close()

    #Now put the output into an array and convert from Nano-Teslas to Teslas
    magfield = np.array(map(float,out))
    magfield = magfield*1e-9

    #Return the adjusted magnetic field
    return magfield

###############################################################################################################
# 
# NAME:
#   wmm2eci
# PURPOSE:
#   To convert a WMM geomagnetic field vector to Earth Centered Inertial coordinates
# INPUTS:
#   ra: float
#     The right ascension
#   dec: float
#     The declination
#   wwmvec: three vector
#      The magnetic field vector
# RETURNS:
#   The field vector in Earth Centered Inertial Coordinates
#
###############################################################################################################

def wmm2eci(ra, dec, wmmvec):
    "Converts a WMM geomagnetic field vector to Earth Centered Inertial coordinates; assumes the epoch is J2000"
    # See Documentation for meaning of this
    return np.dot(
        [[-cos(dec), 0, -sin(dec)],
         [sin(dec)*cos(ra), -sin(ra), -cos(dec)*cos(ra)],
         [sin(dec)*sin(ra), cos(ra), -cos(dec)*sin(ra)]], wmmvec)

###############################################################################################################
# 
# NAME:
#   getSatECI
# PURPOSE:
#   To get the coorinate and velocity vector of the satellite from the pyephem object  
# INPUTS:
#   sat: cxbn object
#     The pyephem instance of the satellite object
#   c: constant object
#     An instance of the class that has all of the physical constant values
#
# RETURNS:
#   r_eci : vec3 
#     The position vector in ECI coordinates
#   v_eci : vec3
#     The velocity vector in ECI coordinates
#
###############################################################################################################

def getSatECI(sat, c):

  #First get the RA and DEC from satellite object
  RA_rad  = float(sat.ra)
  DEC_rad = float(sat.dec)
  Radius  = float(sat.elevation+c.EARTH_R)

  #Now convert to ECI
  x_eci   = Radius*cos(DEC_rad)*cos(RA_rad)
  y_eci   = Radius*cos(DEC_rad)*sin(RA_rad)
  z_eci   = Radius*sin(DEC_rad)

  #Form a three vector and return it 
  r_eci   = vec3(x_eci, y_eci, z_eci)
  return r_eci

###############################################################################################################
# 
# NAME:
#   getinvmoitensor
# PURPOSE:
#   Extracts the moment of inertia tensor from a tab separated value file
# INPUTS:
#   filename: string
#     The full path of the file
# RETURNS:
#   The inverse of the moment of inertia tensor
#
###############################################################################################################

def getinvmoitensor(filename):
   "Extracts a moment of inertia tensor from a tab seperated value file, and returns the inverse"
   fp = open(filename)
   tsv = csv.reader(fp, delimiter='\t')
   fp.close()
   return la.inv(map(lambda x: map(float,x), tsv))

###############################################################################################################
# 
# NAME:
#   getsat 
# PURPOSE:
#   Gets the pyephem object that represents an earth orbiting satellite at a particular date, given the 
#   TLE file describing the orbit 
# INPUTS:
#   tlefile: string
#     The full path of the file
#   date: float
#     The pyephem representation of the date (number of seconds since 2000/1/1 at Noon)
# RETURNS:
#   sat: pyephem object
#
###############################################################################################################

def getsat(tlefile,date):
    "Get the a pyephem object representing a earth satellite at a particular date, given a TLE file describing the orbit"
    f = open(tlefile,'r')
    tle = f.readlines()
    f.close()
    sat = ephem.readtle(tle[0],tle[1],tle[2])
    sat.compute(date)
    return sat

###############################################################################################################
# 
# NAME:
#   getSolarPanelIllumination 
# PURPOSE:
#   Gets the solar panel illumination.  This is a four-quadrant output (+x face, -x face, +y face, -y face)
# INPUTS:
#   r_sun_b: vec3
#     The sun pointing vector 
#   Sat: cxbn object
#     This is an instance of the class that defines the satellite 
# RETURNS:
#   plus_x_I : float
#     The intensity recorded by the solar panel that extends in the +x direction
#   minus_x_I : float
#     The intensity recorded by the solar panel that extends in the -x direction
#   plus_y_I : float
#     The intensity recorded by the solar panel that extends in the +y direction
#   minus_y_I : float
#     The intensity recorded by the solar panel that extends in the -y direction
#   
###############################################################################################################
def getSolarPanelIllumination(r_sun_b, Sat):

  #First of all figure out the angle of the sun with respect to the +x,-x solar panels
  x_angle = atan2(r_sun_b.x, r_sun_b.z)
  y_angle = atan2(r_sun_b.y, r_sun_b.z)
  I_0 = 10

  #Now calculate the fluxes based on whether the blocking is occurring on the +x or -x face
  if ((x_angle <= 0) and (x_angle >= -pi/2)) : 
    #This solar panel is unobstructed
    plus_x_I  = I_0*Sat.body_x_plus_face_A*(Sat.sop_x_plus_face1_dA*r_sun_b)
    #This solar panel can only be blocked up to its full length (solar panel length = body length) 
    if (x_angle > -pi/4):
      minus_x_I = I_0*Sat.body_x_minus_face_A*(1-tan(abs(x_angle)))*(Sat.sop_x_minus_face1_dA*r_sun_b) 
    else:
      minus_x_I = 0
  elif ((x_angle > 0) and (x_angle < pi/2)) :  
    #This solar panel is unobstructed
    minus_x_I = I_0*Sat.body_x_plus_face_A*(Sat.sop_x_minus_face1_dA*r_sun_b)
    #This solar panel can only be blocked up to its full length (solar panel length = body length) 
    if (x_angle < pi/4):
      plus_x_I  = I_0*Sat.body_x_minus_face_A*(1-tan(abs(x_angle)))*(Sat.sop_x_plus_face1_dA*r_sun_b)
    else:
      plus_x_I  = 0
  else : 
    plus_x_I  = 0
    minus_x_I = 0 

  #Now calculate the fluxes based on whether the blocking is occurring on the +x or -x face
  if ((y_angle <= 0) and (y_angle >= -pi/2)) :
    #This solar panel is unobstructed
    plus_y_I  = I_0*Sat.body_y_plus_face_A*(Sat.sop_y_plus_face1_dA*r_sun_b)
    #This solar panel can only be blocked up to its full length (solar panel length = body length) 
    if (y_angle > -pi/4):
      minus_y_I = I_0*Sat.body_y_minus_face_A*(1-tan(abs(y_angle)))*(Sat.sop_y_minus_face1_dA*r_sun_b)
    else: 
      minus_y_I = 0
  elif ((y_angle >= 0) and (y_angle <= pi/2)) :
    #This solar panel is unobstructed
    minus_y_I = I_0*Sat.body_y_minus_face_A*(Sat.sop_y_minus_face1_dA*r_sun_b)
    #This solar panel can only be blocked up to its full length (solar panel length = body length) 
    if (y_angle < pi/4):
      plus_y_I  = I_0*Sat.body_y_plus_face_A*(1-tan(abs(y_angle)))*(Sat.sop_y_plus_face1_dA*r_sun_b)
    else:
      plus_y_I  = 0
  else :
    plus_y_I  = 0
    minus_y_I = 0

  #Return the four quadrant output 
  return plus_x_I, minus_x_I, plus_y_I, minus_y_I

###############################################################################################################
# 
# NAME:
#   getGravityGradientTorge
# PURPOSE:
#   To calculate the gravity gradient torque on the satellite. 
# INPUTS:
#   SC_pos_eci: vec3
#     The three vector representing the spacecraft position in ECI coordinates
#   e_2_b: matrix 3x3
#     The 3x3 matrix that represents the orientation of the satellite body coordinate 
#     system relative to the ECI coordinate system
#   Sat: cxbn object
#     This is an instance of the class that defines the satellite
#   c: constant object
#     This is an instance of the class that has the constants
#
# RETURNS:
#   gg_torque: vec3
#     The three vector that represents the gravity gradient torque on the satellite 
#
###############################################################################################################

def getGravityGradientTorque(sc_pos_eci, e_2_b, Sat, c):

  #The vector to the center of the earth is the opposite of the eci_vector
  R_c_eci  = -sc_pos_eci

  #And it must be defined in body coordinates to match up with the Inertia Tensor
  R_c_body = e_2_b * R_c_eci

  #Now use the formula in Analytical Mechanics of Aerospace Systems (Schaub and Junkins, p.157) 
  prefactor = (3*c.G*c.EARTH_MASS/(R_c_body.length())**5)
  gg_torque = prefactor*(R_c_body.cross(Sat.InertiaTensor*R_c_body))

  #Zero out any numerical erro
  if abs(gg_torque.x) < 1e-10 : gg_torque.x = 0
  if abs(gg_torque.y) < 1e-10 : gg_torque.y = 0
  if abs(gg_torque.z) < 1e-10 : gg_torque.z = 0

  #Return the torque vector
  return gg_torque

###############################################################################################################
# 
# NAME:
#   getAerodynamicTorge
# PURPOSE:
#   To calculate the aerodynamic torque on the satellite. 
# INPUTS:
#   SC_vel_eci: vec3
#     The three vector representing the spacecraft velocity in ECI coordinates
#   SC_pos_eci: vec3
#     The three vector representing the spacecraft position in ECI coordinates
#   e_2_b: matrix 3x3
#     The 3x3 matrix that represents the orientation of the satellite body coordinate 
#     system relative to the ECI coordinate system
#   Sat: cxbn object
#     This is an instance of the class that defines the satellite
#   c: constant object
#     This is an instance of the class that has the constants
#
# RETURNS:
#   aero_torq: vec3
#     The three vector that represents the aerodynamic torque on the satellite 
#
###############################################################################################################

def getAerodynamicTorque(sc_vel_air_eci, sc_pos_eci, e_2_b, Sat, c):

  #First add the instantaneous velocity of the spacecraft to the earth's rotational velocity
  omega_earth = vec3(0.0, 0.0, c.OMEGA_EARTH) 
  #sc_vel_air_eci = sc_vel_air_eci + sc_pos_eci.cross(omega_earth)
 
  #Next convert this from inertial coordinates to body coordinates 
  sc_vel_air_body = e_2_b*sc_vel_air_eci 

  #Compute the air density; this is based on the calculations used for HETE
  hg  = sc_pos_eci.length() #Returns the magnitude of the position vector; the height above sea level
  hg  = hg - c.EARTH_R
  hg  = 6356.766*hg/(6356766.0+hg)
  rho = c.rho_0*exp(-(hg-500)/65.6356)

  #Start the process of getting the aeordynamic torque
  aero_torque = vec3(0.,0.,0.)

  #Loop through and accumulate the torques
  for i in np.arange(Sat.NumSurfs):

    #Evaluate the dot product (n_hat dot v_b)
    TransverseComponent = Sat.dA_Arr[i] * sc_vel_air_body
    #If greater than zero, there is a force
    if (TransverseComponent > 0):
      aero_torque = aero_torque + Sat.A_Arr[i]*Sat.dP_Arr[i].cross(TransverseComponent*sc_vel_air_body)

  #Return to the total torque 
  aero_torque = -rho*aero_torque   
  return aero_torque

###############################################################################################################
#
#NAME:
#  SimpleControlLoop.py 
#
#PURPOSE:
#  This function determines the output torque for a very simple control 
#  loop with only a P term in the classical PID loop.
#
#INPUTS:
#  B: vec3 float
#    The magnetic field providing the torque
#  w_m: vec3 float
#    The measured angular velocity vector
#  w_d: float
#    The desired angular velocity vector
#  I_m: mat3
#    The 3x3 moment of intertia tensor
#  k: float
#    The proportional factor for the control loop
#  m_max: float
#    The maximum magnetic moment that can be generated in each axis
#
#RETURNS:
#  t : vec3
#    The control torque that will be applied
#
###############################################################################################################
def SimpleControlLoop(B, w_m, w_d, I_m, k, m_max):

  #Calculate the torque necessary
  t_x = I_m[0,0]*k*(w_d.x-w_m.x) + (I_m[2,2]-I_m[1,1])*w_m.y*w_m.z
  t_y = I_m[1,1]*k*(w_d.y-w_m.y) + (I_m[0,0]-I_m[2,2])*w_m.x*w_m.z
  t_z = I_m[2,2]*k*(w_d.z-w_m.z) + (I_m[1,1]-I_m[0,0])*w_m.x*w_m.y

  #Now find the maximum torque that can be exerted given the orientation
  t_x_max = abs(m_max*B.y-m_max*B.z)
  t_y_max = abs(m_max*B.z-m_max*B.x)
  t_z_max = abs(m_max*B.x-m_max*B.y)

  #Now apply upper limits to the torque
  if abs(t_x) > t_x_max : t_x = sign(t_x)*t_x_max
  if abs(t_y) > t_y_max : t_y = sign(t_y)*t_y_max
  if abs(t_z) > t_z_max : t_z = sign(t_z)*t_z_max

  #Form the three vector and return it
  mag_control_torque = vec3(t_x, t_y, t_z)
  return mag_control_torque

###############################################################################################################
#
#NAME:
#  ReturnMagFieldAlignTorque
#
#PURPOSE:
#  This function returns the torque necessary to align the satellite with the magnetic field 
#  of earth in its orbit.  This equates to making B(dot) = 0
#
#INPUTS:
#  B_t_m2 : vec3 float
#    The magnetic field as measured by the magnetometers two time steps previous (in body frame)
#  B_t_m1 : vec3 float
#    The magnetic field as measured by the magnetometers one time step previous (in body frame)
#  B_t_m0 : vec3 float
#    The magnetic field as measured by the magnetometers at the current time step (in body frame)
#  K_B : float
#    The proportional factor for the damping torque
#  K_T : float
#    The proportional factor for the alignment torque
#  m_max: float
#    The maximum magnetic moment that can be generated in each axis
#  dt: float
#    The time step over which the magnetic fields were measured (necessary for derivative)
#
#KEYWORDS:
#  z_body: vec3
#    The axis which we want to align with the satellite (we are taking this as the z-axis)
#
#RETURNS:
#  t : vec3
#    The control torque that will be applied
#
###############################################################################################################
def ReturnMagFieldAlignTorque(B_t_m2, B_t_m1, B_t_m0, K_B, K_T, m_max, dt, z_body=0):

  #Set z_body
  z_body = vec3(0,0,1)
  
  #Normalize the vectors to get rid of funky units
  B_t_m0_u = B_t_m0
  B_t_m0 = B_t_m0.normalize()
  B_t_m1 = B_t_m1.normalize()
  B_t_m2 = B_t_m2.normalize()

  #Calculate the cross product and the derivative
  B_x_B_0_b  = B_t_m1.cross(B_t_m2)
  B_x_B_1_b  = B_t_m0.cross(B_t_m1)
  d_b_x_B_dt = (1/dt)*(1/B_t_m0.length()**2)*(B_x_B_1_b - B_x_B_0_b)

  #Now solve for the damping torque
  d_b_x_B_dt_scaled    = -K_B*d_b_x_B_dt
  d_b_x_B_dt_scaled[2] = 0
  m_damp  = B_t_m0.cross(d_b_x_B_dt_scaled)

  #Now solve for the alignment magnetic moment
  z_body_cross_B_scaled = K_T * (z_body.cross(B_t_m0))
  z_body_cross_B_scaled[2] = 0
  m_align = B_t_m0.cross(z_body_cross_B_scaled)

  #Now calculate the torque
  m = m_damp + m_align
  torque = m.cross(B_t_m0_u)
  #print m_damp.x, m_damp.y, m_damp.z, m_align.x, m_align.y, m_align.z
  #pdb.set_trace()
  #Form the three vector and return it
  return torque

###############################################################################################################
#
#NAME:
#  ReturnCoarseSunTorque.py 
#
#PURPOSE:
#  This function will determine the torque necessary to orient the spacecraft so it points toward the 
#  sun.
#
#INPUTS:
#  SolarFluxArr: Float Arr (1x4)
#    The flux on each solar panel.  The order is [+x, -x, +y, -y]
#  B: vec3 float
#    The magnetic field providing the torque
#  w_m: vec3 float
#    The measured angular velocity vector
#  I_m: mat3
#    The 3x3 moment of intertia tensor
#  K_CS_Loop: float
#    The constant in the control loop for determining the torque.
#  m_max: float
#    The maximum magnetic moment that can be generated in each axis
#
#RETURNS:
#  t : vec3
#    The control torque that will be applied
#
###############################################################################################################
def ReturnCoarseSunTorque(SolarFluxArr, B, w_m, I_m, k_CS_Loop, m_max):

  #Create an array for the desired angular velocity
  w_d = vec3(0,0,0)

  #Find the maximum torque that can be exerted given the orientation
  t_x_max = abs(m_max*B.y-m_max*B.z)
  t_y_max = abs(m_max*B.z-m_max*B.x)

  #First of all, determine the sign of each torque
  SignXTorque = sign(SolarFluxArr[0]-SolarFluxArr[1])
  SignYTorque = sign(SolarFluxArr[2]-SolarFluxArr[3])

  #Next determine how much is necessary (how angled it is)
  MagXTorque  = abs(SolarFluxArr[0]-SolarFluxArr[1])
  MagYTorque  = abs(SolarFluxArr[2]-SolarFluxArr[3])

  #Equate this to an angular velocity in the right direction
  w_d.x = k_CS_Loop * MagXTorque * SignXTorque
  w_d.y = k_CS_Loop * MagYTorque * SignYTorque
 
  #If there is no illumination on the X solar panels, then they are not facing the sun.  So move them.
  if MagXTorque == 0: 
    t_x = t_x_max
  else:
    #w_d.x = -k_CS_Loop * MagXTorque * SignXTorque
    #t_x   = I_m[0,0]*(w_d.x-w_m.x) + (I_m[2,2]-I_m[1,1])*w_m.y*w_m.z
    t_x = k_CS_Loop * MagXTorque * SignXTorque + (I_m[2,2]-I_m[1,1])*w_m.y*w_m.z

  #If there is no illumination on the Y solar panels, then they are not facing the sun.  So move them.
  if MagYTorque == 0:
    t_y = t_y_max
  else:
    #w_d.y = -k_CS_Loop * MagYTorque * SignYTorque
    #t_y   = I_m[1,1]*(w_d.y-w_m.y) + (I_m[0,0]-I_m[2,2])*w_m.x*w_m.z
    t_y   = k_CS_Loop * MagYTorque * SignYTorque + (I_m[0,0]-I_m[2,2])*w_m.x*w_m.z

  #Now apply upper limits to the torque
  if abs(t_x) > t_x_max : t_x = sign(t_x)*t_x_max
  if abs(t_y) > t_y_max : t_y = sign(t_y)*t_y_max

  #Now return the torque
  t = vec3(t_x, t_y, 0)
  return t

###############################################################################################################
# 
# NAME:
#   solveEuler
# PURPOSE:
#   To solve Euler's equations of rotation for the spacecraft.  This is done with a linear, explicit 
#   method of time-stepping.  The necessary inputs are the angular velocity vector, the total torque, 
#   and the moment of inertia tesnor.  Note that this 
#   only solves if the moment of inertia tensor is diagonal.
# INPUTS:
#   omega: vec3
#     The three vector representing the spacecraft angular momentum
#   torque: vec3
#     The three vector that holds the torque on the spacecraft
#   Sat: cxbn object
#     This is an instance of the class that defines the satellite
#   dt: float
#     The time step to take (in seconds)
#   Mode: int
#     0 - Explicit Time Step
#     1 - 4th order Runge-Kutta Time Step
# 
# RETURNS:
#   omega_new: vec3
#     The new omega vector after the time-step dt
#
###############################################################################################################
def SolveEuler(omega, torque, I, dt, Mode):

  #EULER'S METHOD --------------------------------------------------------
  if Mode == 0:

    #Get the moment of inertia tensor
    del_omega_x = (1/I[0,0])*(-(I[2,2]-I[1,1])*omega.y*omega.z + torque.x)
    del_omega_y = (1/I[1,1])*(-(I[0,0]-I[2,2])*omega.z*omega.x + torque.y)
    del_omega_z = (1/I[2,2])*(-(I[1,1]-I[0,0])*omega.x*omega.y + torque.z)

    #Form the three vector
    del_omega = vec3(del_omega_x, del_omega_y, del_omega_z)
    
    #And add to the old vector 
    omega_new = omega+dt*del_omega
  
    return omega_new

  #FOURTH ORDER RUNGE-KUTTA METHOD --------------------------------------
  elif Mode == 1:

    #Assign the omega compoments to easier variables to deal with 
    w_x_0 = omega.x
    w_y_0 = omega.y
    w_z_0 = omega.z

    #Form the first delta (k1) 
    del_w_x_0 = (1/I[0,0])*(-(I[2,2]-I[1,1])*w_y_0*w_z_0 + torque.x) * dt
    del_w_y_0 = (1/I[1,1])*(-(I[0,0]-I[2,2])*w_z_0*w_x_0 + torque.y) * dt
    del_w_z_0 = (1/I[2,2])*(-(I[1,1]-I[0,0])*w_x_0*w_y_0 + torque.z) * dt

    #Now figure out where the midpoint lies based upon the initial slope
    w_x_1 = w_x_0 + 0.5 * del_w_x_0 
    w_y_1 = w_y_0 + 0.5 * del_w_y_0
    w_z_1 = w_z_0 + 0.5 * del_w_z_0

    #Form the second delta at the midpoint (k2)
    del_w_x_1 = (1/I[0,0])*(-(I[2,2]-I[1,1])*w_y_1*w_z_1 + torque.x) * dt
    del_w_y_1 = (1/I[1,1])*(-(I[0,0]-I[2,2])*w_z_1*w_x_1 + torque.y) * dt
    del_w_z_1 = (1/I[2,2])*(-(I[1,1]-I[0,0])*w_x_1*w_y_1 + torque.z) * dt

    #Now figure out where the midpoint lies based upon the midpoint slope
    w_x_2 = w_x_0 + 0.5 * del_w_x_1
    w_y_2 = w_y_0 + 0.5 * del_w_y_1
    w_z_2 = w_z_0 + 0.5 * del_w_z_1
  
    #Form the third delta at the midpoint (k3)
    del_w_x_2 = (1/I[0,0])*(-(I[2,2]-I[1,1])*w_y_2*w_z_2 + torque.x) * dt
    del_w_y_2 = (1/I[1,1])*(-(I[0,0]-I[2,2])*w_z_2*w_x_2 + torque.y) * dt
    del_w_z_2 = (1/I[2,2])*(-(I[1,1]-I[0,0])*w_x_2*w_y_2 + torque.z) * dt

    #Now figure out where the midpoint lies based upon the midpoint slope
    w_x_3 = w_x_0 + 0.5 * del_w_x_2
    w_y_3 = w_y_0 + 0.5 * del_w_y_2
    w_z_3 = w_z_0 + 0.5 * del_w_z_2

    #Form the fourth delta at the endpoint (k4)
    del_w_x_3 = (1/I[0,0])*(-(I[2,2]-I[1,1])*w_y_2*w_z_2 + torque.x) * dt
    del_w_y_3 = (1/I[1,1])*(-(I[0,0]-I[2,2])*w_z_2*w_x_2 + torque.y) * dt
    del_w_z_3 = (1/I[2,2])*(-(I[1,1]-I[0,0])*w_x_2*w_y_2 + torque.z) * dt

    #Calculate the final values 
    w_x_new = w_x_0 + (1./6)*(del_w_x_0 + 2*del_w_x_1 + 2*del_w_x_2 + del_w_x_3)
    w_y_new = w_y_0 + (1./6)*(del_w_y_0 + 2*del_w_y_1 + 2*del_w_y_2 + del_w_y_3)
    w_z_new = w_z_0 + (1./6)*(del_w_z_0 + 2*del_w_z_1 + 2*del_w_z_2 + del_w_z_3)

    #Form the new 3 vector
    omega_new = vec3(w_x_new, w_y_new, w_z_new)
    return omega_new 

###############################################################################################################
#
# NAME:
#   UpdateQuaternion
#
# PURPOSE:
#   To update the quaternion that represents the rotation of a rigid body.  In this case, that body is 
#   the spacecraft.  The method used here follows the NASA Patent "Closed-Form Integrator For the 
#   Quaternion (Euler Angle) Kinematics Equations".  This technique allows the quaternion to be integrated 
#   so that it stays on the surface of the 4-d unit sphere (note there is still error from evaluation of the 
#   cosine and sine terms). 
#
#   This function needs the omega[t] and omega[t+1] values calculated by the Euler Solver.
#
# INPUTS:
#   q_t0 : vec4
#     The four vector that represents the quaternion at time [t]
#   dt : float 
#     The time step over which we will integrate the quaternion
#   omega_t0: vec3
#     The angular velocity vector at time step [t]
#   omega_t1: vec3
#     The angular velocity vector at time step [t+1]
#
# RETURNS:
#   q_t1 : vec4
#     The updated quaternion four vector at time [t+1]
#   q_t1_quat : quat
#      The q_t1 vector in the cgtypes form (d, a, b, c)
#
###############################################################################################################

def UpdateQuaternion(q_t0, dt, omega_t0, omega_t1):

  #Get the Integrating Factor Matrix (Captial Omega)
  OMEGA = mat4()
  I_4   = mat4(np.eye(4))

  #And find the "time-averaged" angular velocity over the time interval**
  omega_avg = .5*(omega_t0+omega_t1)

  #If there is a non-zero omega, then the quaternion needs updating
  if ((omega_avg.x != 0) or (omega_avg.y != 0) or (omega_avg.z !=0)):

    #Now create the integrating factor matrix components
    P = dt*omega_avg.x ; Q = dt*omega_avg.y ; R = dt*omega_avg.z

    #And update the integrating factor matrix
    OMEGA[0,0] =  0    ; OMEGA[0,1] = +R/2 ; OMEGA[0,2] = -Q/2 ; OMEGA[0,3] = +P/2
    OMEGA[1,0] = -R/2  ; OMEGA[1,1] =  0   ; OMEGA[1,2] = +P/2 ; OMEGA[1,3] = +Q/2
    OMEGA[2,0] = +Q/2  ; OMEGA[2,1] = -P/2 ; OMEGA[2,2] =  0   ; OMEGA[2,3] = +R/2
    OMEGA[3,0] = -P/2  ; OMEGA[3,1] = -Q/2 ; OMEGA[3,2] = -R/2 ; OMEGA[3,3] =  0

    #Calculate the state transition matrix.  Make sure that 
    PSI = cos(dt*omega_avg.length()/2) * I_4 + \
          (2/(dt*omega_avg.length())) *sin(dt*omega_avg.length()/2) * OMEGA
    q_t1 = PSI*q_t0
  
  #If omega is zero, then the quaternion remains stationary
  else:
    q_t1 = q_t0

  #Assign the quaternions to a quaternion object and normalize
  q_t1_quat = quat(q_t1[3],q_t1[0],q_t1[1],q_t1[2])
  q_t1_quat = q_t1_quat.normalize()
  q_t1      = vec4(q_t1_quat.x, q_t1_quat.y, q_t1_quat.z, q_t1_quat.w)
 
  return q_t1, q_t1_quat

###############################################################################################################
#
# NAME:
#   ReturnDCM
#
# PURPOSE:
#   To calculate the Direction Cosine Matrix given two sets of basis vectors.  The first triad of vectors, u,  
#   belongs to the reference frame we are trying to rotate "from".  The second triad of vectors, v, belongs to 
#   the reference frame we are trying to rotate "to".  
#
#   For example, if we are trying to rotate from the e frame (Earth Centered Inertial) to the b frame 
#   (body frame) so that the DCM is e_2_b, then we are trying to do the following:
#
#   b_1 = cos(a_11)*e_1 + cos(a_12)*e_2 + cos(a_13)*e_3
#   b_2 = cos(a_21)*e_1 + cos(a_22)*e_2 + cos(a_23)*e_3
#   b_3 = cos(a_31)*e_1 + cos(a_32)*e_2 + cos(a_33)*e_3
# 
#   And b_1 dot e_1 = cos(a_11), b_1 dot e_2 = cos(a_12), b+1 dot e_3 = cos(a_13), etc.  This means that 
#   in going from the u to v frame, we must have the rows corresponding to a given u basis vector and 
#   the columns corresponding to a given v basis vector. 
#
# INPUTS:
#   u_i : vec3
#     The components of the i unit vector in the u frame
#   u_j : vec3
#     The components of the j unit vector in the u frame 
#   u_k : vec3
#     The components of the k unit vector in the u frame
#   v_i : vec3
#     The components of the i unit vector in the v frame
#   v_j : vec3
#     The components of the j unit vector in the v frame 
#   v_k : vec3
#     The components of the k unit vector in the v frame
#
# RETURNS:
#   u_to_v : mat3
#     The 3x3 matrix that represents the DCM between the two bases.
#
###############################################################################################################
def ReturnDCM(u_i, u_j, u_k, v_i, v_j, v_k):

  #Form the matrix that will hold the DCM
  DCM = mat3()

  #Now insert the elements
  DCM[0,0] = u_i*v_i   ;   DCM[0,1] = u_j*v_i   ;   DCM[0,2] = u_k*v_i
  DCM[1,0] = u_i*v_j   ;   DCM[1,1] = u_j*v_j   ;   DCM[1,2] = u_k*v_j
  DCM[2,0] = u_i*v_k   ;   DCM[2,1] = u_j*v_k   ;   DCM[2,2] = u_k*v_k

  #And return the DCM
  return DCM
 
###############################################################################################################
# 
# CLASS:
#   AttitudeSat 
# PURPOSE:
#   To provide a representation of the satellites orientiation  
#
###############################################################################################################

class AttitudeSat:
    # Satellite's time
    date = ephem.now()
    # Quaternion representing the satellite's orientation in Earth centered inertial coordinates
    #q = quat.quat(np.eye(3))
    # Vector representing the satellite's 

    def setasttime(self, asttime=ephem.now()):
        "Sets the satellite's time to a given astronomical time; uses current time if unspecified"
        self.date = ephem.Date(asttime)

    def gdy(self, asttime=date):
        "Returns the Gregorian Decimal Year, for a given date;  uses object date if unspecified"
        return jd2gdy(ephem.julian_date(asttime))

###############################################################################################################
# 
# CLASS:
#   CubeSat 
# PURPOSE:
#   To provide a representation of the satellite.  This will include things such as the center of mass, 
#   the solar panel angle, etc. 
#
###############################################################################################################
class CubeSat:

  #Instantiation
  def __init__(self):

    #Constants and Dimensions
    self.SolPAngle = 0                              #Angle of solar panels from z-axis in degrees 
    self.theta     = ephem.pi*self.SolPAngle/180.   #Angle of solar panels from z-axis in radians
    self.CM_vec    = vec3(0,0,0)                    #Location of center of mass w/r to geometrical center
    self.body_x    = 0.10                           #Length of cubesat body (x dimension) in cm
    self.body_y    = 0.10                           #Width of cubesat body (y dimension) in cm
    self.body_z    = 0.20                           #Height of cubesat body (z dimension) in cm
    self.NumSurfs  = 14                             #Number of surfaces used in evaluating torques

    #Moment of Inertia Tensor (in units of kg*m^2)
    self.InertiaTensor = mat3(+0.00648969 , -0.00241305, -0.001416130, \
                              -0.00241305 , +0.01380258, +0.000806190, \
                              -0.00141630 , +0.00080619, +0.014945932) 
    #For right now, keep it diagnonal
    self.InertiaTensor[0,1] = 0 ; self.InertiaTensor[0,2] = 0
    self.InertiaTensor[1,0] = 0 ; self.InertiaTensor[1,2] = 0
    self.InertiaTensor[2,0] = 0 ; self.InertiaTensor[2,1] = 0

    #Geometrical Description of Satellite --------------------------------------------------------------------
    #Surface Area Vectors (Unit vectors that will be scaled by total area)
    self.body_z_plus_face_dA  = vec3( 0,  0, +1)                           #Surface area vec body +z face 
    self.body_z_minus_face_dA = vec3( 0,  0, -1)                           #Surface area vec body -z face
    self.body_x_plus_face_dA  = vec3(+1,  0,  0)                           #Surface area vec body +x face
    self.body_x_minus_face_dA = vec3(-1,  0,  0)                           #Surface area vec body -x face
    self.body_y_plus_face_dA  = vec3( 0, +1,  0)                           #Surface area vec body +y face
    self.body_y_minus_face_dA = vec3( 0, -1,  0)                           #Surface area vec body -z face
    self.sop_x_plus_face1_dA  = vec3( sin(self.theta), 0, cos(self.theta)) #Surface area vec sol panel +x face
    self.sop_x_minus_face1_dA = vec3(-sin(self.theta), 0, cos(self.theta)) #Surface area vec sol panel -x face
    self.sop_y_plus_face1_dA  = vec3( 0, sin(self.theta), cos(self.theta)) #Surface area vec sol panel +y face
    self.sop_y_minus_face1_dA = vec3( 0,-sin(self.theta), cos(self.theta)) #Surface area vec sol panel -y face
    self.sop_x_plus_face2_dA  = -self.sop_x_plus_face1_dA                  #Opposite side of sol panel +x face 
    self.sop_x_minus_face2_dA = -self.sop_x_minus_face1_dA                 #Opposite side of sol panel -x face
    self.sop_y_plus_face2_dA  = -self.sop_y_plus_face1_dA                  #Opposite side of sol panel +y face
    self.sop_y_minus_face2_dA = -self.sop_y_minus_face1_dA                 #Opposite side of sol panel -y face

    #Surface Area Magnitude (used to scale the magnitude of the force)
    self.body_z_plus_face_A   = self.body_x*self.body_y                    #Surface  area of body +z face
    self.body_z_minus_face_A  = self.body_x*self.body_y                    #Surface  area of body -z face
    self.body_x_plus_face_A   = self.body_z*self.body_y                    #Surface  area of body +x face
    self.body_x_minus_face_A  = self.body_z*self.body_y                    #Surface  area of body -x face
    self.body_y_plus_face_A   = self.body_x*self.body_z                    #Surface  area of body +y face
    self.body_y_minus_face_A  = self.body_x*self.body_z                    #Surface  area of body -y face
    self.sop_x_plus_face1_A   = self.body_z*self.body_y                    #Surface  area of sol panel +x face
    self.sop_x_minus_face1_A  = self.body_z*self.body_y                    #Surface  area of sol panel -x face
    self.sop_y_plus_face1_A   = self.body_z*self.body_x                    #Surface  area of sol panel +y face
    self.sop_y_minus_face1_A  = self.body_z*self.body_x                    #Surface  area of sol panel -y face
    self.sop_x_plus_face2_A   = self.sop_x_plus_face1_A                    #Opposite side of sol panel +x face
    self.sop_x_minus_face2_A  = self.sop_x_minus_face1_A                   #Opposite side of sol panel -x face
    self.sop_y_plus_face2_A   = self.sop_y_plus_face1_A                    #Opposite side of sol panel +y face
    self.sop_y_minus_face2_A  = self.sop_y_minus_face1_A                   #Opposite side of sol panel -y face

    #Center of Pressure Vectors
    self.body_z_plus_face_dP  = vec3( 0, 0,  self.body_z/2-self.CM_vec.z)  #Cen. of Pressure vec body +z face
    self.body_z_minus_face_dP = vec3( 0, 0, -self.body_z/2-self.CM_vec.z)  #Cen. of pressure vec body -z face
    self.body_x_plus_face_dP  = vec3( self.body_x/2-self.CM_vec.x, 0, 0 )  #Cen. of pressure vec body +x face
    self.body_x_minus_face_dP = vec3(-self.body_x/2-self.CM_vec.x, 0, 0 )  #Cen. of pressure vec body -x face
    self.body_y_plus_face_dP  = vec3( 0, self.body_y/2-self.CM_vec.y, 0 )  #Cen. of pressure vec body +y face 
    self.body_y_minus_face_dP = vec3( 0,-self.body_y/2-self.CM_vec.y, 0 )  #Cen. of pressure vec body -y face
    self.sop_x_plus_face_dP   = vec3( self.body_x/2+(self.body_z/2)*cos(self.theta)-self.CM_vec.x,\
                                      0,\
                                     -self.body_z/2-(self.body_z/2)*sin(self.theta)-self.CM_vec.z)
    self.sop_x_minus_face_dP  = vec3(-self.body_x/2-(self.body_z/2)*cos(self.theta)-self.CM_vec.x,\
                                      0,\
                                     -self.body_z/2-(self.body_z/2)*sin(self.theta)-self.CM_vec.z)
    self.sop_y_plus_face_dP   = vec3( 0, \
                                      self.body_y/2+(self.body_z/2)*cos(self.theta)-self.CM_vec.y,\
                                     -self.body_z/2-(self.body_z/2)*sin(self.theta)-self.CM_vec.z)
    self.sop_y_minus_face_dP  = vec3( 0, \
                                     -self.body_y/2-(self.body_z/2)*cos(self.theta)-self.CM_vec.y,\
                                     -self.body_z/2-(self.body_z/2)*sin(self.theta)-self.CM_vec.z)

    
    #Make arrays to hold these quantities
    self.dA_Arr = [self.body_z_plus_face_dA,  self.body_z_minus_face_dA,\
                   self.body_x_plus_face_dA,  self.body_x_minus_face_dA,\
                   self.body_y_plus_face_dA,  self.body_y_minus_face_dA,\
                   self.sop_x_plus_face1_dA,  self.sop_x_plus_face2_dA,\
                   self.sop_x_minus_face1_dA, self.sop_x_minus_face2_dA,\
                   self.sop_y_plus_face1_dA,  self.sop_y_plus_face2_dA,\
                   self.sop_y_minus_face1_dA, self.sop_y_minus_face2_dA]

    self.A_Arr  =  [self.body_z_plus_face_A,  self.body_z_minus_face_A,\
                    self.body_x_plus_face_A,  self.body_x_minus_face_A,\
                    self.body_y_plus_face_A,  self.body_y_minus_face_A,\
                    self.sop_x_plus_face1_A,  self.sop_x_plus_face2_A,\
                    self.sop_x_minus_face1_A, self.sop_x_minus_face2_A,\
                    self.sop_y_plus_face1_A,  self.sop_y_plus_face2_A,\
                    self.sop_y_minus_face1_A, self.sop_y_minus_face2_A]
 

    self.dP_Arr = [self.body_z_plus_face_dP, self.body_z_minus_face_dP,\
                   self.body_x_plus_face_dP, self.body_x_minus_face_dP,\
                   self.body_y_plus_face_dP, self.body_y_minus_face_dP,\
                   self.sop_x_plus_face_dP,  self.sop_x_plus_face_dP,\
                   self.sop_x_minus_face_dP, self.sop_x_minus_face_dP,\
                   self.sop_y_plus_face_dP,  self.sop_y_plus_face_dP,\
                   self.sop_y_minus_face_dP, self.sop_y_minus_face_dP]
