######################################################
#
#NAME
# GravityBoomConfigurationForSatelliteControl.py
#
#PURPOSE:
# This is just a setup file to configure the 
# angular velocity and moment of inertia tensor to 
# simulate a gravity stabilized satellite for the 
# SatelliteControl.py script.  See that file for more 
# details.
#
######################################################

#Set up the moment of inertia tensor to be concentrated along the z axis
cxbn.InertiaTensor[0,0] = 1.000 
cxbn.InertiaTensor[0,1] = 0.000
cxbn.InertiaTensor[0,2] = 0.000
cxbn.InertiaTensor[1,0] = 0.000
cxbn.InertiaTensor[1,1] = 1.000 
cxbn.InertiaTensor[1,2] = 0.000
cxbn.InertiaTensor[2,0] = 0.000
cxbn.InertiaTensor[2,1] = 0.000
cxbn.InertiaTensor[2,2] = 0.001

#Artificially set the omega's to zero
omega.x=0 ; omega.y=0 ; omega.z=0
omega_t0=omega ; omega_t1=omega

#Use only the gravity gradient torque
UseGGTorque = 1
UseAeroTorque = 0
    
#Align the satellite so that the -z axis aligns with nadir vector
th_gg = math.acos(r_eci.z/r_eci.length())
ph_gg = math.atan2(r_eci.y,r_eci.x)
e_2_b = mat3(+cos(th_gg)*cos(ph_gg) , +cos(th_gg)*sin(ph_gg) , -sin(th_gg) , \
             -sin(ph_gg)            , +cos(ph_gg)            ,  0          , \
             +sin(th_gg)*cos(ph_gg) , +sin(th_gg)*sin(ph_gg) , +cos(th_gg))

#Figure out what the r_eci vector looks like in body coordinates (should be [0,0,1])
r_eci_body = e_2_b*r_eci

#Get the initial velocity vector to do a rotation of the satellite
Temp_dt    = 0.00001
TempEpDate = EpDate+ephem.second*Temp_dt
CXBN.compute(TempEpDate)
r_eci_t0   = r_eci
r_eci_t1   = getSatECI(CXBN, c)
v_eci_t0   = 1./Temp_dt*(r_eci_t1-r_eci_t0)

#Try to get the DCM between the inertial frame and the reference frame
u_i = vec3(1,0,0) ; u_j = vec3(0,1,0) ; u_k = vec3(0,0,1)

#Normalize these vectors
v_k = r_eci_t0.normalize()
v_j = v_eci_t0.normalize()
v_i = v_j.cross(v_k)

#Get the velocity in the body frame (should have no z component)
v_eci_t0_body = e_2_b*v_eci_t0

#Get the DCM
DCM = ReturnDCM(u_i, u_j, u_k, v_i, v_j, v_k)
pdb.set_trace()


#Now calculate the quaternion for this matrix
quat_initial = quat(e_2_b)
quat_initial = quat_initial.normalize()

#Turn this into the four vector
q_t0[3] = quat_initial.w
q_t0[0] = quat_initial.x
q_t0[1] = quat_initial.y
q_t0[2] = quat_initial.z
q_t1    = q_t0 
