# 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:  05.07.05/Germany                                                   ****
# **** Version:        v0.5                                                               ****
# ********************************************************************************************

# ********************************************************************************************
# **** Function:  Create AGC Attitude GUI                                                 ****
# ********************************************************************************************
proc create_attitude_gui {} {
 global bo 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_Propulsion_Max_N Descent_Propulsion_Min_N DESCENT_ENGINE_FLAG
 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
 
 if {[winfo exists .att] == 1} {destroy .att}

 toplevel .att -background $colb
 wm title .att "LM Attitude & Speed Simulation"
 wm geometry .att +260+260
 wm geometry .att 300x450
 wm minsize  .att 300 450
 wm iconname .att "LM Flight Simulation"
 
 frame .att.p1 -bg $colb; frame .att.p1_f0 -bg $colb; frame .att.p1_f1 -bg $colb
 frame .att.p1_f2 -bg $colb; frame .att.p1_f3 -bg $colb; frame .att.p1_f4 -bg $colb
 frame .att.p1_f5 -bg $colb; frame .att.p1_f6 -bg $colb; frame .att.p1_f7 -bg $colb
 frame .att.p1_f8 -bg $colb; frame .att.p1_f9 -bg $colb; frame .att.p1_f10 -bg $colb
 frame .att.p1_f11 -bg $colb; frame .att.p1_f12 -bg $colb
 
 frame .att.f00a -bg $colb; frame .att.f0a -bg $colb; frame .att.f01a -bg $colb; frame .att.f1a -bg $colb; frame .att.f2a -bg $colb; frame .att.f3a -bg $colb; frame .att.f4a -bg $colb
 frame .att.f5a -bg $colb; frame .att.f6a -bg $colb; frame .att.f7a -bg $colb; frame .att.f8a -bg $colb
 frame .att.f9a -bg $colb
 pack .att.p1 .att.f00a .att.f0a .att.f01a .att.f1a .att.f2a .att.f3a .att.f4a .att.f5a -side top -anchor w
 pack .att.f6a .att.f7a .att.f8a .att.f9a -side top -anchor w
 
 pack .att.p1_f0 .att.p1_f1 .att.p1_f2 .att.p1_f3 .att.p1_f4 .att.p1_f5 .att.p1_f6 -in .att.p1 -side top -anchor w
 pack .att.p1_f10 .att.p1_f11 .att.p1_f12 -in .att.p1 -side top -anchor w
 pack .att.p1_f7 .att.p1_f8 .att.p1_f9 -in .att.p1 -side top -anchor w
 
 label .att.p1_l01 -text "SIMULATION PARAMETER:" -font ${font3} -bg $colb
 pack .att.p1_l01 -side left -in .att.p1_f0 -padx 1
 
 label .att.p1_l1 -text "SIMULATION TIME: " -font ${font1} -bg $colb
 label .att.p1_l2 -text "" -font ${font1} -bg $colb
 pack .att.p1_l1 .att.p1_l2 -side left -in .att.p1_f1 -padx 1
 
 label .att.p1_l3 -text "DESCENT THRUST:       " -font ${font1} -bg $colb
 label .att.p1_l4 -text "" -font ${font1} -bg $colb
 pack .att.p1_l3 .att.p1_l4 -side left -in .att.p1_f2 -padx 1
 
 label .att.p1_l5 -text "LM WEIGHT:            " -font ${font1} -bg $colb
 label .att.p1_l6 -text "" -font ${font1} -bg $colb
 pack .att.p1_l5 .att.p1_l6 -side left -in .att.p1_f3 -padx 1
 
 label .att.p1_l7 -text "DESCENT PROPELLANT:   " -font ${font1} -bg $colb
 label .att.p1_l8 -text "" -font ${font1} -bg $colb
 pack .att.p1_l7 .att.p1_l8 -side left -in .att.p1_f4 -padx 1
 
 label .att.p1_l9 -text "FUEL FLOW:            " -font ${font1} -bg $colb
 label .att.p1_l10 -text "" -font ${font1} -bg $colb
 pack .att.p1_l9 .att.p1_l10 -side left -in .att.p1_f5 -padx 1
 
 label .att.p1_l11 -text "ACCELERATION:         " -font ${font1} -bg $colb
 label .att.p1_l12 -text "" -font ${font1} -bg $colb
 pack .att.p1_l11 .att.p1_l12 -side left -in .att.p1_f6 -padx 1

 label .att.p1_l15 -text "RCS PROPELLANT:       " -font ${font1} -bg $colb
 label .att.p1_l16 -text "" -font ${font1} -bg $colb
 pack .att.p1_l15 .att.p1_l16 -side left -in .att.p1_f10 -padx 1
  
 label .att.p1_l13 -text "----------------------------------" -font ${font1} -bg $colb
 pack .att.p1_l13 -side left -in .att.p1_f7 -padx 1
 
 checkbutton .att.p1_cb1 -text "DESCENT ENGINE ON/OFF" -variable DESCENT_ENGINE_FLAG -relief raised -borderwidth 2 -anchor w -font ${font1} -onvalue "1" -offvalue "0" -bg $colb
 scale .att.p1_s1 -label "Thrust \[N\]" -from $Descent_Propulsion_Min_N -to $Descent_Propulsion_Max_N -length 250 -orient horizontal -bg $colb -activebackground $colb -highlightbackground $colb 
 pack .att.p1_cb1 .att.p1_s1 -side top -in .att.p1_f8 -padx 1
 
 label .att.p1_l14 -text "----------------------------------" -font ${font1} -bg $colb
 pack .att.p1_l14 -side left -in .att.p1_f9 -padx 1
 
 label .att.l0 -text "RCS IMPULSE \[MSEC\]:" -font ${font3} -bg $colb
 # pack .att.l0 -side left -in .att.f00a -anchor w -pady 2 -padx 1
 radiobutton .att.rb1 -text "14" -variable RCS_IMPULSE_TIME -value "0.014" -font ${font1} -relief raised -bg $colb -pady 2
 radiobutton .att.rb2 -text "100" -variable RCS_IMPULSE_TIME -value "0.1" -font ${font1} -relief raised -bg $colb -pady 2
 radiobutton .att.rb3 -text "200" -variable RCS_IMPULSE_TIME -value "0.2" -font ${font1} -relief raised -bg $colb -pady 2
 pack .att.l0 .att.rb1 .att.rb2 .att.rb3 -side left -in .att.f0a -anchor w -pady 2 -padx 1
 
 label .att.l01 -text "RCS MODE:         " -font ${font3} -bg $colb
 radiobutton .att.rb4 -text "2 JETS" -variable RCS_JETS -value "2" -font ${font1} -relief raised -bg $colb -pady 2
 radiobutton .att.rb5 -text "4 JETS" -variable RCS_JETS -value "4" -font ${font1} -relief raised -bg $colb -pady 2
 pack .att.l01 .att.rb4 .att.rb5 -side left -in .att.f01a -anchor w -pady 2 -padx 1
 
 label .att.l1 -text "Roll:  " -font ${font3} -bg $colb
 button .att.b1 -text " - " -command {fire_rcs_thruster ROLL_NEG $RCS_IMPULSE_TIME} -font ${font1} -bg $colb
 button .att.b2 -text " + " -command {fire_rcs_thruster ROLL_POS $RCS_IMPULSE_TIME} -font ${font1} -bg $colb
 pack .att.l1 .att.b1 .att.b2 -side left -in .att.f1a -anchor w -padx 2 -pady 2
 
 label .att.l2 -text "Pitch: " -font ${font3} -bg $colb
 button .att.b3 -text " - " -command {fire_rcs_thruster PITCH_NEG $RCS_IMPULSE_TIME} -font ${font1} -bg $colb
 button .att.b4 -text " + " -command {fire_rcs_thruster PITCH_POS $RCS_IMPULSE_TIME} -font ${font1} -bg $colb
 pack .att.l2 .att.b3 .att.b4 -side left -in .att.f2a -anchor w -padx 2 -pady 2
 
 label .att.l3 -text "Yaw:   " -font ${font3} -bg $colb
 button .att.b5 -text " - " -command {fire_rcs_thruster YAW_NEG $RCS_IMPULSE_TIME} -font ${font1} -bg $colb -pady 3
 button .att.b6 -text " + " -command {fire_rcs_thruster YAW_POS $RCS_IMPULSE_TIME} -font ${font1} -bg $colb -pady 3
 pack .att.l3 .att.b5 .att.b6 -side left -in .att.f3a -anchor w -padx 2 -pady 2
 
}


# ********************************************************************************************
# **** Function: Simulation Loop                                                          ****
# ********************************************************************************************
proc dynamic_simulation {} {
 global Simulation_Timer 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 Delta_Time DESCENT_ENGINE_FLAG Descent_Propulsion_N yawDeltaV
 global simcount2 Descent_Specific_Impulse_MS Descent_Fuel_Flow_SEC RCS_Propellant_Mass_KG
 global Delta_Theta_Roll Delta_Theta_Pitch Delta_Theta_Yaw
 global Omega_Roll Omega_Pitch Omega_Yaw 

 # -------------------------- Main Engine Simulation ----------------------------------
 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]}
 
 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_Time]
   set Descent_Propellant_Mass_KG [expr $Descent_Propellant_Mass_KG - $Descent_Fuel_Flow]
   set LM_Weight_KG [expr $LM_Weight_KG - $Descent_Fuel_Flow]
   set Descent_Acceleration [expr $Descent_Propulsion_N / $LM_Weight_KG]
   set yawDeltaV [expr $yawDeltaV + $Descent_Acceleration * $Delta_Time]
 } else {
   set DESCENT_ENGINE_FLAG 0
   set Descent_Acceleration 0
   set Descent_Fuel_Flow_SEC 0
 }
 
 set Delta_Theta_Roll [expr $Delta_Theta_Roll + $Omega_Roll * $Delta_Time]
 set Delta_Theta_Pitch [expr $Delta_Theta_Pitch + $Omega_Pitch * $Delta_Time]
 set Delta_Theta_Yaw [expr $Delta_Theta_Yaw + $Omega_Yaw * $Delta_Time]
 
 # ---- FDAI Update ----
 incr simcount2 1
 if {$simcount2 == 2} {
    set simcount2 0
    modify_pipaXYZ
    Transform_NB_SM "ROLL" -$Delta_Theta_Roll
    Transform_NB_SM "PITCH" -$Delta_Theta_Pitch
    Transform_NB_SM "YAW" -$Delta_Theta_Yaw
    set Delta_Theta_Roll 0
    set Delta_Theta_Pitch 0
    set Delta_Theta_Yaw 0
    set yawDeltaV 0
 }
}


# ********************************************************************************************
# **** Function: RCS Thruster firing                                                      ****
# ********************************************************************************************
proc fire_rcs_thruster {par1 par2} {
 global LM_Weight_KG RCS_Propellant_Mass_KG RCS_Thrust_N RCS_Specific_Impulse_MS
 global Positive_Roll_Timer Negative_Roll_Timer
 global Positive_Pitch_Timer Negative_Pitch_Timer
 global Positive_Yaw_Timer Negative_Yaw_Timer
 global Delta_Theta_Roll Delta_Theta_Pitch Delta_Theta_Yaw
 global Omega_Roll Omega_Pitch Omega_Yaw RCS_JETS Moment_of_Inertia_Factor

# -------------------------- RCS Thruster Simulation ---------------------------------
 if {$RCS_Propellant_Mass_KG <= 0} {return}
 
 set Moment_of_Inertia [expr $Moment_of_Inertia_Factor * $LM_Weight_KG]
 set Tau [expr $RCS_JETS * 1.62 * $RCS_Thrust_N]
 set Alpha [expr $Tau / $Moment_of_Inertia]
 
 # ---- Roll ----
 if {$par1 == "ROLL_NEG"} {
   set Omega_Roll [expr $Omega_Roll - [expr $Alpha * $par2]]
 }
 
 if {$par1 == "ROLL_POS"} {
   set Omega_Roll [expr $Omega_Roll + [expr $Alpha * $par2]]
 }
 
 # ---- Pitch ----
 if {$par1 == "PITCH_NEG"} {
   set Omega_Pitch [expr $Omega_Pitch - [expr $Alpha * $par2]]
 }
 
 if {$par1 == "PITCH_POS"} {
   set Omega_Pitch [expr $Omega_Pitch + [expr $Alpha * $par2]]
 }
 
 # ---- Yaw ----
 if {$par1 == "YAW_NEG"} {
   set Omega_Yaw [expr $Omega_Yaw - [expr $Alpha * $par2]]
 }
 
 if {$par1 == "YAW_POS"} {
   set Omega_Yaw [expr $Omega_Yaw + [expr $Alpha * $par2]]
 }
 
 set RCS_Fuel_Flow [expr $RCS_JETS * $RCS_Thrust_N / $RCS_Specific_Impulse_MS * $par2]
 set LM_Weight_KG [expr $LM_Weight_KG - $RCS_Fuel_Flow]
 set RCS_Propellant_Mass_KG [expr $RCS_Propellant_Mass_KG - $RCS_Fuel_Flow]
 
}


# ********************************************************************************************
# **** 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: Check Thruster Status and fire RCS if needed                             ****
# ********************************************************************************************
proc update_RCS {} {
 global bo ROLL_TIMER PITCH_TIMER YAW_TIMER ROLL_FLAG PITCH_FLAG YAW_FLAG RCS_IMPULSE_TIME
 
  set PJ1 $bo(5,1); set PJ2 $bo(5,2); set PJ3 $bo(5,3); set PJ4 $bo(5,4)
  set PJ5 $bo(5,5); set PJ6 $bo(5,6); set PJ7 $bo(5,7); set PJ8 $bo(5,8)
  
  set RJ1 $bo(6,1); set RJ2 $bo(6,2); set RJ3 $bo(6,3); set RJ4 $bo(6,4)
  set RJ5 $bo(6,5); set RJ6 $bo(6,6); set RJ7 $bo(6,7); set RJ8 $bo(6,8)
  
  # ---- Measure Pitch Thruster Time and Fire RCS after Bit went back to Zero ----
  if {$PJ1 == 1 && $PJ6 == 1 && $PJ7 == 1 && $PJ4 == 1 && $PITCH_FLAG == ""} {
     set PITCH_FLAG "NEGATIVE"
     fire_rcs_thruster "PITCH_NEG" [expr abs($PITCH_TIMER)/57.0 * $RCS_IMPULSE_TIME]
  } elseif {$PJ5 == 1 && $PJ2 == 1 && $PJ3 == 1 && $PJ8 == 1 && $PITCH_FLAG == ""} {
     set PITCH_FLAG "POSITIVE"
     fire_rcs_thruster "PITCH_POS" [expr abs($PITCH_TIMER)/57.0 * $RCS_IMPULSE_TIME]
  } elseif {$PJ1 == 0 && $PJ5 == 0} {
    set PITCH_FLAG ""
  }
  
  # ---- Measure Roll Thruster Time and Fire RCS after Bit went back to Zero ----
  if {$PJ1 == 1 && $PJ6 == 1 && $PJ3 == 1 && $PJ8 == 1 && $ROLL_FLAG == ""} {
     set ROLL_FLAG "POSITIVE"
     fire_rcs_thruster "ROLL_POS" [expr abs($ROLL_TIMER)/57.0 * $RCS_IMPULSE_TIME]
  } elseif {$PJ5 == 1 && $PJ2 == 1 && $PJ7 == 1 && $PJ4 == 1 && $ROLL_FLAG == ""} {
     set ROLL_FLAG "NEGATIVE"
     fire_rcs_thruster "ROLL_NEG" [expr abs($ROLL_TIMER)/57.0 * $RCS_IMPULSE_TIME]
  } elseif {$PJ1 == 0 && $PJ5 == 0} {
    set ROLL_FLAG ""
  }
  
  # ---- Measure Yaw Thruster Time and Fire RCS after Bit went back to Zero ----
  if {$RJ1 == 1 && $RJ5 == 1 && $RJ3 == 1 && $RJ7 == 1 && $YAW_FLAG == ""} {
     set YAW_FLAG "POSITIVE"
     fire_rcs_thruster "YAW_POS" [expr abs($YAW_TIMER)/57.0 * $RCS_IMPULSE_TIME]
  } elseif {$RJ2 == 1 && $RJ6 == 1 && $RJ4 == 1 && $RJ8 == 1 && $YAW_FLAG == ""} {
     set YAW_FLAG "NEGATIVE"
     fire_rcs_thruster "YAW_NEG" [expr abs($YAW_TIMER)/57.0 * $RCS_IMPULSE_TIME]
  } elseif {$RJ1 == 0 && $RJ2 == 0} {
    set YAW_FLAG ""
  }

}
