# Copyright 2005 Stephan Hotto <stephan.hotto@web.de>
#
# This file is part of yaAGC.
#
# yaAGC is free software; you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation; either version 2 of the License, or
# (at your option) any later version.
#
# yaAGC is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with yaAGC; if not, write to the Free Software
# Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

# ********************************************************************************************
# **** Module:         AGC_Attitude.tcl                                                   ****
# **** Main Program:   lm_simulator                                                       ****
# **** Author:         Stephan Hotto                                                      ****
# **** Date/Location:  16.10.05/Germany                                                   ****
# **** Version:        v0.7                                                               ****
# ********************************************************************************************

# ********************************************************************************************
# **** Function:  Create AGC Attitude GUI                                                 ****
# ********************************************************************************************
proc create_attitude_gui {} {
 global bo font11 font11b font1 font2 font3 colb
 global IMUX_ANGLE IMUY_ANGLE IMUZ_ANGLE PIPAX_COUNT PIPAY_COUNT PIPAZ_COUNT
 global ROLL PITCH YAW RCS_IMPULSE_TIME xVelocity yVelocity zVelocity
 global DESCENT_ENGINE_FLAG Descent_Propulsion_Min_N Descent_Propulsion_Max_N Descent_Propellant_Mass_KG
 global Positive_Roll_Timer Negative_Roll_Timer
 global Positive_Pitch_Timer Negative_Pitch_Timer
 global Positive_Yaw_Timer Negative_Yaw_Timer RCS_IMPULSE_TIME RCS_JETS
 global ASCENT_ENGINE_FLAG LM_CONFIG
 
 if {[winfo exists .att] == 1} {destroy .att}

 toplevel .att -background $colb
 wm title .att "Simulation Monitor & Control"
 wm geometry .att +770+460
 wm geometry .att 270x380
 wm minsize  .att 270 380
 wm iconname .att "LM Flight Simulation"
 
 frame .att.f1 -bg $colb; frame .att.f2 -bg $colb; frame .att.f3 -bg $colb; frame .att.f4 -bg $colb
 frame .att.f5 -bg $colb; frame .att.f6 -bg $colb; frame .att.f7 -bg $colb; frame .att.f8 -bg $colb
 frame .att.f9 -bg $colb; frame .att.f10 -bg $colb; frame .att.f11 -bg $colb; frame .att.f12 -bg $colb
 frame .att.f13 -bg $colb; frame .att.f14 -bg $colb; frame .att.f15 -bg $colb; frame .att.f16 -bg $colb
 frame .att.f17 -bg $colb; frame .att.f18 -bg $colb; frame .att.f19 -bg $colb; frame .att.f20 -bg $colb
 
 pack .att.f1 .att.f2 .att.f3 .att.f4 .att.f5 .att.f6 .att.f7 .att.f8 .att.f9 .att.f10 -side top -anchor w
 pack .att.f11 .att.f12 .att.f13 .att.f14 .att.f15 .att.f16 .att.f17 .att.f18 .att.f19 .att.f20 -side top -anchor w
 
 label .att.p1_l01 -text "SIMULATION PARAMETER:" -font ${font11b} -bg $colb
 pack .att.p1_l01 -side left -in .att.f1 -padx 1
 
 label .att.p1_l1 -text "SIMULATION TIME: " -font ${font11} -bg $colb
 label .att.p1_l2 -text "" -font ${font11} -bg $colb
 pack .att.p1_l1 .att.p1_l2 -side left -in .att.f2 -padx 1
 
 label .att.p1_l5 -text "LM WEIGHT:            " -font ${font11} -bg $colb
 label .att.p1_l6 -text "" -font ${font11} -bg $colb
 pack .att.p1_l5 .att.p1_l6 -side left -in .att.f3 -padx 1
 
 label .att.p1_20 -text "---------- Descent Engine ----------" -font ${font11b} -bg $colb
 pack .att.p1_20 -side top -in .att.f4 -padx 1
 
 label .att.p1_l3 -text "THRUST:               " -font ${font11} -bg $colb
 label .att.p1_l4 -text "" -font ${font11} -bg $colb
 pack .att.p1_l3 .att.p1_l4 -side left -in .att.f5 -padx 1
 
 label .att.p1_l7 -text "PROPELLANT:           " -font ${font11} -bg $colb
 label .att.p1_l8 -text "" -font ${font11} -bg $colb
 pack .att.p1_l7 .att.p1_l8 -side left -in .att.f6 -padx 1
 
 label .att.p1_l9 -text "FUEL FLOW:            " -font ${font11} -bg $colb
 label .att.p1_l10 -text "" -font ${font11} -bg $colb
 pack .att.p1_l9 .att.p1_l10 -side left -in .att.f7 -padx 1
 
 label .att.p1_l11 -text "ACCELERATION:         " -font ${font11} -bg $colb
 label .att.p1_l12 -text "" -font ${font11} -bg $colb
 pack .att.p1_l11 .att.p1_l12 -side left -in .att.f8 -padx 1
 
 checkbutton .att.p1_cb1 -text "DESCENT ENGINE ON/OFF" -variable DESCENT_ENGINE_FLAG -relief raised -borderwidth 2 -anchor w -font ${font11} -onvalue "1" -offvalue "0" -bg $colb
 pack .att.p1_cb1 -side left -in .att.f9 -padx 1 -pady 2
 
 scale .att.p1_s1 -label "Thrust \[N\]" -from $Descent_Propulsion_Min_N -to $Descent_Propulsion_Max_N -length 220 -orient horizontal -bg $colb -activebackground $colb -highlightbackground $colb -font ${font11b} 
 pack .att.p1_s1 -side left -in .att.f10 -padx 1 -pady 2
 
 label .att.p1_29 -text "-------- Stage Separation ----------" -font ${font11b} -bg $colb
 pack .att.p1_29 -side top -in .att.f11 -padx 1
 
 button .att.p1_b1 -text "SEPARATE ASCENT STAGE" -command {set LM_CONFIG "ASCENT"; set Descent_Propellant_Mass_KG 0; set LM_Weight_Descent_KG 0; .att.p1_cb2 configure -state active; .att.p1_cb1 configure -state disable; .att.p1_b1 configure -state disable -text "ASCENT STAGE SEPARATED"} -font ${font11} -fg red -bg $colb -pady 1
 pack .att.p1_b1 -side top -in .att.f12 -padx 1 -pady 2
 
 label .att.p1_21 -text "---------- Ascent Engine -----------" -font ${font11b} -bg $colb
 pack .att.p1_21 -side top -in .att.f13 -padx 1
 
 label .att.p1_l23 -text "PROPELLANT:           " -font ${font11} -bg $colb
 label .att.p1_l24 -text "" -font ${font11} -bg $colb
 pack .att.p1_l23 .att.p1_l24 -side left -in .att.f15 -padx 1
 
 label .att.p1_l25 -text "FUEL FLOW:            " -font ${font11} -bg $colb
 label .att.p1_l26 -text "" -font ${font11} -bg $colb
 pack .att.p1_l25 .att.p1_l26 -side left -in .att.f16 -padx 1
 
 label .att.p1_l27 -text "ACCELERATION:         " -font ${font11} -bg $colb
 label .att.p1_l28 -text "" -font ${font11} -bg $colb
 pack .att.p1_l27 .att.p1_l28 -side left -in .att.f17 -padx 1
 
 checkbutton .att.p1_cb2 -text "ASCENT ENGINE ON/OFF" -variable ASCENT_ENGINE_FLAG -relief raised -borderwidth 2 -anchor w -font ${font11} -onvalue "1" -offvalue "0" -bg $colb -state disabled
  pack .att.p1_cb2 .att.p1_cb2 -side left -in .att.f18 -padx 1 -pady 2

 label .att.p1_22 -text "--------------- RCS ----------------" -font ${font11b} -bg $colb
 pack .att.p1_22 -side top -in .att.f19 -padx 1
 
 label .att.p1_l15 -text "RCS PROPELLANT:     " -font ${font11} -bg $colb
 label .att.p1_l16 -text "" -font ${font11} -bg $colb
 pack .att.p1_l15 .att.p1_l16 -side left -in .att.f20 -padx 1
  
}


# ********************************************************************************************
# **** Function: Read Joystick Position par1 = Channel (octal)                            ****
# ********************************************************************************************
proc read_joystick {par1} {
 global cdata ROLL_TIMER PITCH_TIMER YAW_TIMER

 if {$par1 != 31} {
    set joy_dec [convert_bin15_dec $cdata($par1)]
    if {$joy_dec > 57} {set joy_dec [expr $joy_dec - 32767]}
    if {$par1 == 170} {set ROLL_TIMER $joy_dec}
    if {$par1 == 167} {set YAW_TIMER $joy_dec}
    if {$par1 == 166} {set PITCH_TIMER $joy_dec}
 } else {set joy_dec 0}
 
 #.text insert end "Joystick: $par1 $cdata($par1)  $joy_dec\n"
 #.text yview moveto 1
    
}


# ********************************************************************************************
# **** Function: Simulation Loop                                                          ****
# ********************************************************************************************
proc dynamic_simulation {} {
 global LM_Weight_KG Descent_Propulsion_Max_N Descent_Propulsion_Min_N
 global Descent_Propellant_Mass_KG Propellant_Flow_Max_KGS Descent_Thrust_Procent
 global Descent_Acceleration DESCENT_ENGINE_FLAG Descent_Propulsion_N yawDeltaV
 global simcount2 Descent_Specific_Impulse_MS Descent_Fuel_Flow_SEC RCS_Propellant_Mass_KG
 global Omega_Roll Omega_Pitch Omega_Yaw
 global LM_Weight_Descent_KG LM_Weight_Ascent_KG Ascent_Propellant_Mass_KG 
 global Ascent_Fuel_Flow_SEC Ascent_Acceleration ASCENT_ENGINE_FLAG Ascent_Thrust_N
 global Ascent_Specific_Impulse_MS Delta_Time2
 
 # -------------------------- Main Engine Simulation ----------------------------------
 set LM_Weight_KG [expr $LM_Weight_Ascent_KG + $LM_Weight_Descent_KG]
 
 catch {set Descent_Propulsion_N [.att.p1_s1 get]}
 set Descent_Thrust_Procent [expr $Descent_Propulsion_N / $Descent_Propulsion_Max_N]

 catch {.att.p1_l2 configure -text [format "%10.1f SEC" $Simulation_Timer]}
 catch {.att.p1_l4 configure -text [format "%5.1f " [expr 100 * $Descent_Thrust_Procent]]%}
 catch {.att.p1_l6 configure -text [format "%5.0f KG" $LM_Weight_KG]}
 catch {.att.p1_l8 configure -text [format "%5.0f KG" $Descent_Propellant_Mass_KG]}
 catch {.att.p1_l10 configure -text [format "%5.2f KG/S" $Descent_Fuel_Flow_SEC]}
 catch {.att.p1_l12 configure -text [format "%5.3f M/S*S" $Descent_Acceleration]}
 catch {.att.p1_l16 configure -text [format "%5.1f KG" $RCS_Propellant_Mass_KG]}
 catch {.att.p1_l24 configure -text [format "%5.0f KG" $Ascent_Propellant_Mass_KG]}
 catch {.att.p1_l26 configure -text [format "%5.1f KG" $Ascent_Fuel_Flow_SEC]}
 catch {.att.p1_l28 configure -text [format "%5.3f M/S*S" $Ascent_Acceleration]}

 if {$DESCENT_ENGINE_FLAG == 1 && $Descent_Propellant_Mass_KG > 0} {
   set Descent_Fuel_Flow_SEC [expr $Descent_Propulsion_N / $Descent_Specific_Impulse_MS]
   set Descent_Fuel_Flow [expr $Descent_Fuel_Flow_SEC * $Delta_Time2]
   set Descent_Propellant_Mass_KG [expr $Descent_Propellant_Mass_KG - $Descent_Fuel_Flow]
   set LM_Weight_Descent_KG [expr $LM_Weight_Descent_KG - $Descent_Fuel_Flow]
   set Descent_Acceleration [expr $Descent_Propulsion_N / $LM_Weight_KG]
   set yawDeltaV [expr $yawDeltaV + $Descent_Acceleration * $Delta_Time2]
 } else {
   set DESCENT_ENGINE_FLAG 0
   set Descent_Acceleration 0
   set Descent_Fuel_Flow_SEC 0
 }
 
 if {$ASCENT_ENGINE_FLAG == 1 && $Ascent_Propellant_Mass_KG > 0} {
   set Ascent_Fuel_Flow_SEC [expr $Ascent_Thrust_N / $Ascent_Specific_Impulse_MS]
   set Ascent_Fuel_Flow [expr $Ascent_Fuel_Flow_SEC * $Delta_Time2]
   set Ascent_Propellant_Mass_KG [expr $Ascent_Propellant_Mass_KG - $Ascent_Fuel_Flow]
   set LM_Weight_Ascent_KG [expr $LM_Weight_Ascent_KG - $Ascent_Fuel_Flow]
   set Ascent_Acceleration [expr $Ascent_Thrust_N / $LM_Weight_Ascent_KG]
   set yawDeltaV [expr $yawDeltaV + $Ascent_Acceleration * $Delta_Time2]
 } else {
   set ASCENT_ENGINE_FLAG 0
   set Ascent_Acceleration 0
   set Ascent_Fuel_Flow_SEC 0
 }
 
 set Delta_Theta_Roll [expr $Omega_Roll * $Delta_Time2]
 set Delta_Theta_Pitch [expr $Omega_Pitch * $Delta_Time2]
 set Delta_Theta_Yaw [expr $Omega_Yaw * $Delta_Time2]
 
 Transform_NB_SM "ROLL" -$Delta_Theta_Roll
 Transform_NB_SM "PITCH" -$Delta_Theta_Pitch
 Transform_NB_SM "YAW" -$Delta_Theta_Yaw
 
 incr simcount2 1
 if {$simcount2 == 2} {
    set simcount2 0
    modify_pipaXYZ
    catch {move_fdai_marker}
    set yawDeltaV 0
 }
 
}


# ********************************************************************************************
# **** Function: Check AGC Thruster Status and fire dedicated RCS Thruster                ****
# ********************************************************************************************
proc update_RCS {} {
 global bo RCS_Propellant_Mass_KG JET_FLAG
 global Q4U Q4D Q3U Q3D 
 global Q2U Q2D Q1U Q1D
 global Q3A Q4F Q1F Q2A
 global Q2L Q3R Q4R Q1L
 
  if {$RCS_Propellant_Mass_KG <= 0 || $JET_FLAG == 0} {return}
  
  # ---- V-Axis 2 Thruster ----
  if {$Q1U == 0 && $Q1D == 0 && $Q2D == 1 && $Q3U == 0 && $Q3D == 0 && $Q4U == 1} {
     fire_rcs_thruster "V_POS" 2
  } elseif {$Q1U == 0 && $Q1D == 0 && $Q2U == 1 && $Q3U == 0 && $Q3D == 0 && $Q4D == 1} {
     fire_rcs_thruster "V_NEG" 2
  } 
  
  # ---- U-Axis 2 Thruster ----
  if {$Q1D == 1 && $Q2U == 0 && $Q2D == 0 && $Q3U == 1 && $Q4U == 0 && $Q4D == 0} {
     fire_rcs_thruster "U_POS" 2
  } elseif {$Q1U == 1 && $Q2U == 0 && $Q2D == 0 && $Q3D == 1 && $Q4U == 0 && $Q4D == 0} {
     fire_rcs_thruster "U_NEG" 2
  }
   
  # ---- YAW 4 Thruster----
  if {$Q1F == 1 && $Q2L == 1 && $Q3A == 1 && $Q4R == 1} {
     fire_rcs_thruster "YAW_POS" 4
  } elseif {$Q1L == 1 && $Q2A == 1 && $Q3R == 1 && $Q4F == 1} {
     fire_rcs_thruster "YAW_NEG" 4
  }
  
  # ---- YAW 2 Thruster 1st Pair ----
  if {$Q1F == 0 && $Q2L == 1 && $Q3A == 0 && $Q4R == 1} {
     fire_rcs_thruster "YAW_POS" 2
  } elseif {$Q1L == 0 && $Q2A == 1 && $Q3R == 0 && $Q4F == 1} {
     fire_rcs_thruster "YAW_NEG" 2
  }
  
  # ---- YAW 2 Thruster 2nd Pair ----
  if {$Q1F == 1 && $Q2L == 0 && $Q3A == 1 && $Q4R == 0} {
     fire_rcs_thruster "YAW_POS" 2
  } elseif {$Q1L == 1 && $Q2A == 0 && $Q3R == 1 && $Q4F == 0} {
     fire_rcs_thruster "YAW_NEG" 2
  } 

  # ---- Pitch 4 Thruster----
  if {$Q1U == 1 && $Q2D == 1 && $Q3D == 1 && $Q4U == 1} {
     fire_rcs_thruster "PITCH_NEG" 4
  } elseif {$Q1D == 1 && $Q2U == 1 && $Q3U == 1 && $Q4D == 1} {
     fire_rcs_thruster "PITCH_POS" 4
  }
  
   # ---- Pitch 2 Thruster (Minimum Impulse Mode)----
  if {$Q1U == 0 && $Q1D == 0 && $Q2U == 0 && $Q2D == 0 && $Q3D == 1 && $Q4U == 1} {
     fire_rcs_thruster "PITCH_NEG" 2
  } elseif {$Q1D == 1 && $Q2U == 1 && $Q3U == 0 && $Q3D == 0 && $Q4U == 0 && $Q4D == 0} {
     fire_rcs_thruster "PITCH_POS" 2
  }
    
  # ---- Roll 4 Thruster ----
  if {$Q1D == 1 && $Q2D == 1 && $Q3U == 1 && $Q4U == 1} {
     fire_rcs_thruster "ROLL_POS" 4
  } elseif {$Q1U == 1 && $Q2U == 1 && $Q3D == 1 && $Q4D == 1} {
     fire_rcs_thruster "ROLL_NEG" 4
  }
  
  # ---- Roll 2 Thruster (Minimum Impulse Mode) ----
  if {$Q1D == 1 && $Q2U == 0 && $Q2D == 0 && $Q3U == 0 && $Q3D == 0 && $Q4U == 1} {
     fire_rcs_thruster "ROLL_POS" 2
  } elseif {$Q1U == 0 && $Q1D == 0 && $Q2U == 1 && $Q3D == 1 && $Q4U == 0 && $Q4D == 0} {
     fire_rcs_thruster "ROLL_NEG" 2
  }
  
}


# ********************************************************************************************
# **** Function: RCS Thruster firing par1=Direction; par2=Number of Thruster              ****
# ********************************************************************************************
proc fire_rcs_thruster {par1 par2} {
 global LM_Weight_KG LM_Weight_Ascent_KG RCS_Propellant_Mass_KG RCS_Thrust_N RCS_Specific_Impulse_MS
 global Omega_Roll Omega_Pitch Omega_Yaw Delta_Time LM_CONFIG a b c PI
 global RAD_TO_DEG

 # -------------------------- RCS Thruster Simulation ---------------------------------
 set m [expr $LM_Weight_KG / 65535.0]
 set Alpha [expr $RAD_TO_DEG * 0.125 * ($b($LM_CONFIG,YAW) + $a($LM_CONFIG,YAW)/($m + $c($LM_CONFIG,YAW)))]
 set DeltaOmega_yaw [expr $par2 * $Alpha * $Delta_Time]
 
 set Alpha [expr $RAD_TO_DEG * 0.125 * ($b($LM_CONFIG,PITCH) + $a($LM_CONFIG,PITCH)/($m + $c($LM_CONFIG,PITCH)))]
 set DeltaOmega_pitch [expr $par2 * $Alpha * $Delta_Time]
 
 set Alpha [expr $RAD_TO_DEG * 0.125 * ($b($LM_CONFIG,ROLL) + $a($LM_CONFIG,ROLL)/($m + $c($LM_CONFIG,ROLL)))]
 set DeltaOmega_roll [expr $par2 * $Alpha * $Delta_Time]
 
 # ---- V-Axis ----
 if {$par1 == "V_POS"} {
   set Omega_Pitch [expr $Omega_Pitch - $DeltaOmega_pitch * 0.707]
   set Omega_Roll [expr $Omega_Roll + $DeltaOmega_roll * 0.707]
 }
 
 if {$par1 == "V_NEG"} {
   set Omega_Pitch [expr $Omega_Pitch + $DeltaOmega_pitch * 0.707]
   set Omega_Roll [expr $Omega_Roll - $DeltaOmega_roll * 0.707]
 }
 
 # ---- U-Axis ----
 if {$par1 == "U_POS"} {
   set Omega_Pitch [expr $Omega_Pitch + $DeltaOmega_pitch * 0.707]
   set Omega_Roll [expr $Omega_Roll + $DeltaOmega_roll * 0.707]
 }
 
 if {$par1 == "U_NEG"} {
   set Omega_Pitch [expr $Omega_Pitch - $DeltaOmega_pitch * 0.707]
   set Omega_Roll [expr $Omega_Roll - $DeltaOmega_roll * 0.707]
 }
 
 # ---- Roll ----
 if {$par1 == "ROLL_NEG"} {
   set Omega_Roll [expr $Omega_Roll - $DeltaOmega_roll]
 }
 
 if {$par1 == "ROLL_POS"} {
   set Omega_Roll [expr $Omega_Roll + $DeltaOmega_roll]
 }
 
 # ---- Pitch ----
 if {$par1 == "PITCH_NEG"} {
   set Omega_Pitch [expr $Omega_Pitch - $DeltaOmega_pitch]
 }
 
 if {$par1 == "PITCH_POS"} {
   set Omega_Pitch [expr $Omega_Pitch + $DeltaOmega_pitch]
 }
 
 # ---- Yaw ----
 if {$par1 == "YAW_NEG"} {
   set Omega_Yaw [expr $Omega_Yaw - $DeltaOmega_yaw]
 }
 
 if {$par1 == "YAW_POS"} {
   set Omega_Yaw [expr $Omega_Yaw + $DeltaOmega_yaw]
 }
 
 set RCS_Fuel_Flow [expr $par2 * $RCS_Thrust_N / $RCS_Specific_Impulse_MS * $Delta_Time]
 set LM_Weight_Ascent_KG [expr $LM_Weight_Ascent_KG - $RCS_Fuel_Flow]
 set RCS_Propellant_Mass_KG [expr $RCS_Propellant_Mass_KG - $RCS_Fuel_Flow]
 
}

