# 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_IMU.tcl                                                        ****
# **** Main Program:   lm_simulator                                                       ****
# **** Author:         Stephan Hotto                                                      ****
# **** Date/Location:  05.07.05/Germany                                                   ****
# **** Version:        v0.5                                                               ****
# ********************************************************************************************

# ********************************************************************************************
# **** Function:  Create AGC IMU GUI                                                      ****
# ********************************************************************************************
proc create_imugui {} {
 global bo font0 font1 font2 font3 font4 colb colb2 colb3 widgtM
 global IMUX_ANGLE IMUY_ANGLE IMUZ_ANGLE 2PI PI
 global offsetx offsety V MU

 if {[winfo exists .imu] == 1} {destroy .imu}

 toplevel .imu -background $colb2
 wm title .imu "AGC FDAI / IMU"
 wm geometry .imu +405+460
 wm geometry .imu 315x335
 wm minsize  .imu 315 335
 wm iconname .imu "AGC FDAI/IMU"
 
 frame .imu.fma -bg $colb2; frame .imu.fmb -bg $colb2
 pack .imu.fma .imu.fmb -side top -anchor n -pady 2 -padx 2
 
 # -------------------- Rotation FDAI - Scale Canvas ----------------------------------
 canvas .imu.c2 -width 300 -height 300 -relief flat -bg $colb3 -borderwidth 0 -selectborderwidth 0 -insertborderwidth 0 -highlightbackground $colb2 -highlightcolor $colb3
 pack  .imu.c2 -side top -in .imu.fma
 
 # -------------------- m/s or f/s switch --------------------------
 radiobutton .imu.rb1 -text "Meter/Second" -variable MU -value "ms" -command {move_fdai_marker} -font ${font1} -relief raised -bg $colb
 radiobutton .imu.rb2 -text "Feet/Second" -variable MU -value "fs" -command {move_fdai_marker} -font ${font1} -relief raised -bg $colb
 pack .imu.rb1 .imu.rb2 -side left -in .imu.fmb -padx 5
 
 # ------------------- Create Pitch Scale ---------------------------------------------
 for {set i -90} {$i <= 450} {set i [expr $i + 30]} {
   if {$i >= 180 && $i <= 360 || $i <= 0} {set col "black"} else {set col "white"}
   set widgtM(PITCH_$i) [.imu.c2 create line 0 0 0 0 -fill $col -width 1]
   if {$i < 360} {set j $i} else {set j [expr $i - 360]}
   if {$i < 0} {set j [expr $i + 360]}
   set widgtM(PITCH_TXT_$i) [.imu.c2 create text 0 0 -text $j -anchor s -font $font0 -fill $col]
 }
 
 # -------------------- Create Z-Axis ---------------------
 set widgtM(zAxisLM) [.imu.c2 create line 0 0 0 0 -fill blue]
 set widgtM(zAxisLL) [.imu.c2 create line 0 0 0 0 -fill red -width 5]
 set widgtM(zAxisLR) [.imu.c2 create line 0 0 0 0 -fill red -width 5]
 
 set j 0
 foreach i {-60 -30 0 30 60} {
    set widgtM(zAxis_$i) [.imu.c2 create line 0 0 0 0 -fill blue]
    if {$i < 0} {set j [expr $i + 360]} else {set j $i}
    set widgtM(zAxis_TXT_$i) [.imu.c2 create text 0 0 -text $j -anchor n -font $font0 -fill blue]
 }
 
 # -------------------- Create Circle, Scaling and Roll Indicator ---------------------
 .imu.c2 create oval [expr $offsetx-160] [expr $offsety-160] [expr $offsetx+160] [expr $offsety+160] -outline gray80 -width 120
 set widgtM(RollMarker) [.imu.c2 create line 0 0 0 0 -fill black -arrow last -arrowshape {18 18 7}]
 
 for {set i 0} {$i < 360} {set i [expr $i + 10]} {
 
   if {$i==0 || $i==30 || $i==60 || $i==90 || $i==120 || $i==180 || $i==210 || $i==240 || $i==270 || $i==300 || $i==330 || $i==360 || $i==30} {set mag 1.05} else {set mag 1}
   set j [expr $i/180.0*$PI]
   set xp1 [expr $offsetx + 100 * sin($j+$PI)]
   set xp2 [expr $offsetx + $mag * 110 * sin($j+$PI)]
   set yp1 [expr $offsety + 100 * cos($j+$PI)]
   set yp2 [expr $offsety + $mag * 110 * cos($j+$PI)]
   .imu.c2 create line $xp1 $yp1 $xp2 $yp2 -fill white -width 2
 }
  
 .imu.c2 create oval [expr $offsetx-100] [expr $offsety-100] [expr $offsetx+100] [expr $offsety+100] -outline black -width 4
 
 # ------------------- Create Cross Mark -------------------------------
 .imu.c2 create line [expr $offsetx-10] [expr $offsety] [expr $offsetx+10] [expr $offsety] -fill white -width 1
 .imu.c2 create line [expr $offsetx] [expr $offsety-10] [expr $offsetx] [expr $offsety+10] -fill white -width 1
 
 # -------- Numerical IMU Angle Values ------------------
 # ---- Gimbal Angles ----
 .imu.c2 create text 40 15 -text "X:" -anchor e -font $font3 -fill black
 .imu.c2 create text 140 15 -text "Y:" -anchor e -font $font3 -fill black
 .imu.c2 create text 240 15 -text "Z:" -anchor e -font $font3 -fill black
 
 set widgtM(xValue) [.imu.c2 create text 42 15 -text "---.--" -anchor w -font $font1 -fill black]
 set widgtM(yValue) [.imu.c2 create text 142 15 -text "---.--" -anchor w -font $font1 -fill black]
 set widgtM(zValue) [.imu.c2 create text 242 15 -text "---.--" -anchor w -font $font1 -fill black]
 
 # ---- PIPA Values m/s----
 .imu.c2 create text 20 280 -text "X:" -anchor e -font $font3 -fill black
 .imu.c2 create text 120 280 -text "Y:" -anchor e -font $font3 -fill black
 .imu.c2 create text 220 280 -text "Z:" -anchor e -font $font3 -fill black
 
 set widgtM(xPIPA) [.imu.c2 create text 20 280 -text "----.-" -anchor w -font $font1 -fill black]
 set widgtM(yPIPA) [.imu.c2 create text 120 280 -text "----.-" -anchor w -font $font1 -fill black]
 set widgtM(zPIPA) [.imu.c2 create text 220 280 -text "----.-" -anchor w -font $font1 -fill black]

 # ---- GIMBAL LOCK Flag ----
 set widgtM(gimbalLock) [.imu.c2 create text $offsetx $offsety -text "" -anchor s -font $font4 -fill orange]
 
 # -------- Set Marker to current Angles ---------------
 move_fdai_marker
}


# *********************************************************************************************
# **** Function: Move FDAI Marker (par1 = X or Y or Z; par2 = Angle in Deg)                ****
# *********************************************************************************************
proc move_fdai_marker {} {
 global widgtM offsetx offsety V
 global IMUX_ANGLE IMUY_ANGLE IMUZ_ANGLE PI
 global xVelocity yVelocity zVelocity MU MeterToFeet gimbalLock
 
 if {[winfo exists .imu] != 1} {return}
 
 if {$gimbalLock == 1} {
   .imu.c2 itemconfigure $widgtM(gimbalLock) -text "GIMBAL LOCK"
 } else {
   .imu.c2 itemconfigure $widgtM(gimbalLock) -text ""
 }
   
 .imu.c2 itemconfigure $widgtM(xValue) -text [format "%6.2f" $IMUX_ANGLE] 
 .imu.c2 itemconfigure $widgtM(yValue) -text [format "%6.2f" $IMUY_ANGLE]
 .imu.c2 itemconfigure $widgtM(zValue) -text [format "%6.2f" $IMUZ_ANGLE]
 
 if {$MU == "ms"} {
   .imu.c2 itemconfigure $widgtM(xPIPA) -text [format "%7.1fm/s" $xVelocity] 
   .imu.c2 itemconfigure $widgtM(yPIPA) -text [format "%7.1fm/s" $yVelocity]
   .imu.c2 itemconfigure $widgtM(zPIPA) -text [format "%7.1fm/s" $zVelocity]
 } else {
   .imu.c2 itemconfigure $widgtM(xPIPA) -text [format "%7.1ff/s" [expr $xVelocity*$MeterToFeet]] 
   .imu.c2 itemconfigure $widgtM(yPIPA) -text [format "%7.1ff/s" [expr $yVelocity*$MeterToFeet]]
   .imu.c2 itemconfigure $widgtM(zPIPA) -text [format "%7.1ff/s" [expr $zVelocity*$MeterToFeet]]
 }
 
 # ------------------- Rotate Roll Marker ------------------------------
 set xp1 [expr $offsetx + 90 * sin($IMUX_ANGLE/180.0*$PI+$PI)]
 set xp2 [expr $offsetx + 95 * sin($IMUX_ANGLE/180.0*$PI+$PI)]
 set yp1 [expr $offsety + 90 * cos($IMUX_ANGLE/180.0*$PI+$PI)]
 set yp2 [expr $offsety + 95 * cos($IMUX_ANGLE/180.0*$PI+$PI)]
 .imu.c2 coords $widgtM(RollMarker) $xp1 $yp1 $xp2 $yp2
 
 # ------------------- Move Pitch Scale ---------------------------------------------
 for {set i -90} {$i <= 450} {set i [expr $i + 30]} {
  set xpt 0
  set xp1 -50
  set yp1 [expr -$i + $IMUY_ANGLE]
  set xp2 50
  set yp2 [expr -$i + $IMUY_ANGLE]
 
   # ------------------- Rotate X, Y --------------------------------------------------
   set xpt_r [expr $xpt * cos(-$IMUX_ANGLE/180.0*$PI) - $yp1 * sin(-$IMUX_ANGLE/180.0*$PI)]
   set ypt_r [expr $xpt * sin(-$IMUX_ANGLE/180.0*$PI) + $yp1 * cos(-$IMUX_ANGLE/180.0*$PI)]
   
   set xp1_r [expr $xp1 * cos(-$IMUX_ANGLE/180.0*$PI) - $yp1 * sin(-$IMUX_ANGLE/180.0*$PI)]
   set yp1_r [expr $xp1 * sin(-$IMUX_ANGLE/180.0*$PI) + $yp1 * cos(-$IMUX_ANGLE/180.0*$PI)]
 
   set xp2_r [expr $xp2 * cos(-$IMUX_ANGLE/180.0*$PI) - $yp2 * sin(-$IMUX_ANGLE/180.0*$PI)]
   set yp2_r [expr $xp2 * sin(-$IMUX_ANGLE/180.0*$PI) + $yp2 * cos(-$IMUX_ANGLE/180.0*$PI)]
 
   # ------------------- Translate to the offset ---------------------------------------
   set xpt_r [expr $offsetx + $xpt_r]
   set ypt_r [expr $offsetx + $ypt_r]
   set xp1_r [expr $offsetx + $xp1_r]
   set yp1_r [expr $offsety + $yp1_r]
   set xp2_r [expr $offsetx + $xp2_r]
   set yp2_r [expr $offsety + $yp2_r]
   
   # ------------------- Draw Pitch Marks ---------------------------------------------  
   .imu.c2 coords $widgtM(PITCH_TXT_$i) $xpt_r $ypt_r
   .imu.c2 coords $widgtM(PITCH_$i) $xp1_r $yp1_r $xp2_r $yp2_r
 }
 
 # -------------------- Move Z-Axis ---------------------
 if {$IMUZ_ANGLE > 180} {set tmpZAngle [expr $IMUZ_ANGLE - 360]} else {set tmpZAngle $IMUZ_ANGLE}
 set xpLM1 [expr -70 - $tmpZAngle]
 set ypLM1 0
 set xpLM2 [expr 70 - $tmpZAngle]
 set ypLM2 0
 set xpLL1 [expr -90 - $tmpZAngle]
 set ypLL1 0
 set xpLL2 [expr -70 - $tmpZAngle]
 set ypLL2 0
 set xpLR1 [expr 70 - $tmpZAngle]
 set ypLR1 0
 set xpLR2 [expr 90 - $tmpZAngle]
 set ypLR2 0
 
 set xpLM1_r [expr $xpLM1 * cos(-$IMUX_ANGLE/180.0*$PI) - $ypLM1 * sin(-$IMUX_ANGLE/180.0*$PI)]
 set ypLM1_r [expr $xpLM1 * sin(-$IMUX_ANGLE/180.0*$PI) + $ypLM1 * cos(-$IMUX_ANGLE/180.0*$PI)]
 set xpLM2_r [expr $xpLM2 * cos(-$IMUX_ANGLE/180.0*$PI) - $ypLM2 * sin(-$IMUX_ANGLE/180.0*$PI)]
 set ypLM2_r [expr $xpLM2 * sin(-$IMUX_ANGLE/180.0*$PI) + $ypLM2 * cos(-$IMUX_ANGLE/180.0*$PI)]
 
 set xpLL1_r [expr $xpLL1 * cos(-$IMUX_ANGLE/180.0*$PI) - $ypLL1 * sin(-$IMUX_ANGLE/180.0*$PI)]
 set ypLL1_r [expr $xpLL1 * sin(-$IMUX_ANGLE/180.0*$PI) + $ypLL1 * cos(-$IMUX_ANGLE/180.0*$PI)]
 set xpLL2_r [expr $xpLL2 * cos(-$IMUX_ANGLE/180.0*$PI) - $ypLL2 * sin(-$IMUX_ANGLE/180.0*$PI)]
 set ypLL2_r [expr $xpLL2 * sin(-$IMUX_ANGLE/180.0*$PI) + $ypLL2 * cos(-$IMUX_ANGLE/180.0*$PI)]
 
 set xpLR1_r [expr $xpLR1 * cos(-$IMUX_ANGLE/180.0*$PI) - $ypLR1 * sin(-$IMUX_ANGLE/180.0*$PI)]
 set ypLR1_r [expr $xpLR1 * sin(-$IMUX_ANGLE/180.0*$PI) + $ypLR1 * cos(-$IMUX_ANGLE/180.0*$PI)]
 set xpLR2_r [expr $xpLR2 * cos(-$IMUX_ANGLE/180.0*$PI) - $ypLR2 * sin(-$IMUX_ANGLE/180.0*$PI)]
 set ypLR2_r [expr $xpLR2 * sin(-$IMUX_ANGLE/180.0*$PI) + $ypLR2 * cos(-$IMUX_ANGLE/180.0*$PI)]

 set xpLM1_r [expr $offsetx + $xpLM1_r]
 set ypLM1_r [expr $offsety + $ypLM1_r]
 set xpLM2_r [expr $offsetx + $xpLM2_r]
 set ypLM2_r [expr $offsety + $ypLM2_r]
 set xpLL1_r [expr $offsetx + $xpLL1_r]
 set ypLL1_r [expr $offsety + $ypLL1_r]
 set xpLL2_r [expr $offsetx + $xpLL2_r]
 set ypLL2_r [expr $offsety + $ypLL2_r]
 set xpLR1_r [expr $offsetx + $xpLR1_r]
 set ypLR1_r [expr $offsety + $ypLR1_r]
 set xpLR2_r [expr $offsetx + $xpLR2_r]
 set ypLR2_r [expr $offsety + $ypLR2_r]
 
 .imu.c2 coords $widgtM(zAxisLM) $xpLM1_r $ypLM1_r $xpLM2_r $ypLM2_r
 .imu.c2 coords $widgtM(zAxisLL) $xpLL1_r $ypLL1_r $xpLL2_r $ypLL2_r
 .imu.c2 coords $widgtM(zAxisLR) $xpLR1_r $ypLR1_r $xpLR2_r $ypLR2_r
  
 foreach i {-60 -30 0 30 60} {
    set xp1 [expr $i - $tmpZAngle]
    set yp1 -2
    set xp2 [expr $i - $tmpZAngle]
    set yp2 3
    set xpt [expr $i - $tmpZAngle]
    set ypt 10
    
    set xp1_r [expr $xp1 * cos(-$IMUX_ANGLE/180.0*$PI) - $yp1 * sin(-$IMUX_ANGLE/180.0*$PI)]
    set yp1_r [expr $xp1 * sin(-$IMUX_ANGLE/180.0*$PI) + $yp1 * cos(-$IMUX_ANGLE/180.0*$PI)]
    set xp2_r [expr $xp2 * cos(-$IMUX_ANGLE/180.0*$PI) - $yp2 * sin(-$IMUX_ANGLE/180.0*$PI)]
    set yp2_r [expr $xp2 * sin(-$IMUX_ANGLE/180.0*$PI) + $yp2 * cos(-$IMUX_ANGLE/180.0*$PI)]
    set xpt_r [expr $xpt * cos(-$IMUX_ANGLE/180.0*$PI) - $ypt * sin(-$IMUX_ANGLE/180.0*$PI)]
    set ypt_r [expr $xpt * sin(-$IMUX_ANGLE/180.0*$PI) + $ypt * cos(-$IMUX_ANGLE/180.0*$PI)]
    
    set xp1_r [expr $offsetx + $xp1_r]
    set yp1_r [expr $offsety + $yp1_r]
    set xp2_r [expr $offsetx + $xp2_r]
    set yp2_r [expr $offsety + $yp2_r]
    set xpt_r [expr $offsetx + $xpt_r]
    set ypt_r [expr $offsety + $ypt_r]

    .imu.c2 coords $widgtM(zAxis_$i) $xp1_r $yp1_r $xp2_r $yp2_r
    .imu.c2 coords $widgtM(zAxis_TXT_$i) $xpt_r $ypt_r
 }

}


# *********************************************************************************************
# **** Function: Gyro Fine Align (will be called in case of Channel 0177 output)           ****
# *********************************************************************************************
proc gyro_fine_align {} {
  global bo cdata FA_ANGLE log_in_flag PI
  global IMUX_ANGLE IMUY_ANGLE IMUZ_ANGLE
  
  set gyro_sign_minus  [string index $cdata(177) 0]
  set gyro_selection_a [string index $cdata(177) 1]
  set gyro_selection_b [string index $cdata(177) 2]
  set gyro_enable      [string index $cdata(177) 3]
  
  if {$gyro_sign_minus == 1} {set sign -1} else {set sign 1}
  set gyro_pulses [expr $sign * [convert_bin_dec [string range $cdata(177) 4 14]]]
   
  set bogen [expr -$gyro_pulses * $FA_ANGLE / 180.0 * $PI]
  
  if {$gyro_selection_a == 0 && $gyro_selection_b == 1} {
     if {$log_in_flag == 1} {.text insert end "X-GYRO: $gyro_pulses\n"}
     modify_gimbal_angle [expr $gyro_pulses * $FA_ANGLE] 0 0
     #getRotationMatrixX "GYRO" $bogen
     #modify_gimbal_angle_CT X $bogen
  }
  
  if {$gyro_selection_a == 1 && $gyro_selection_b == 0} {
     if {$log_in_flag == 1} {.text insert end "Y-GYRO: $gyro_pulses\n"}
     modify_gimbal_angle 0 [expr $gyro_pulses * $FA_ANGLE] 0
     #getRotationMatrixY "GYRO" $bogen
     #modify_gimbal_angle_CT Y $bogen
  }
  
  if {$gyro_selection_a == 1 && $gyro_selection_b == 1} {
     if {$log_in_flag == 1} {.text insert end "Z-GYRO: $gyro_pulses\n"}
     modify_gimbal_angle 0 0 [expr $gyro_pulses * $FA_ANGLE]
     #getRotationMatrixZ "GYRO" $bogen
     #modify_gimbal_angle_CT Z $bogen
  }

  .text yview moveto 1
  
  # ---- Velocity Vector Update ----
  Transform_NB_SM "ROLL" 0
}


# *************************************************************************************************
# **** Function: Gyro Coarse Align (will be called in case of Channel 0174; 0175; 0176 output) ****
# *************************************************************************************************
proc gyro_coarse_align {par1} {
 global cdata CA_ANGLE log_in_flag bo
 
 set sign [string index $cdata($par1) 0]
 if {$sign == 0} {set sign 1} else {set sign -1}
 set cdu_pulses [expr $sign * [convert_bin_dec [string range $cdata($par1) 1 14]]]
 
 # ---- Coarse Align Enable ----
 if {$bo(12,4) == 1} {
   if {$par1 == 174} { 
      if {$log_in_flag == 1} {.text insert end "DRIVE CDU X: $cdu_pulses\n"}
      modify_gimbal_angle [expr $cdu_pulses * $CA_ANGLE] 0 0
   }
 
   if {$par1 == 175} { 
      if {$log_in_flag == 1} {.text insert end "DRIVE CDU Y: $cdu_pulses\n"}
      modify_gimbal_angle 0 [expr $cdu_pulses * $CA_ANGLE] 0
   }
 
   if {$par1 == 176} { 
      if {$log_in_flag == 1} {.text insert end "DRIVE CDU Z: $cdu_pulses\n"}
      modify_gimbal_angle 0 0 [expr $cdu_pulses * $CA_ANGLE]
   }
 } else {
   if {$par1 == 174} { 
      if {$log_in_flag == 1} {.text insert end "ERROR X: $cdu_pulses\n"}
   }
 
   if {$par1 == 175} { 
      if {$log_in_flag == 1} {.text insert end "ERROR Y: $cdu_pulses\n"}
   }
 
   if {$par1 == 176} { 
      if {$log_in_flag == 1} {.text insert end "ERROR Z: $cdu_pulses\n"}
   }
 }
 
 .text yview moveto 1
 
 # ---- Velocity Vector Update ----
 Transform_NB_SM "ROLL" 0
}


# *************************************************************************************************
# **** Function: Modify a specific IMU Delta Gimbal-Angle par1=X; par2=Y; par3=Z               ****
# *************************************************************************************************
proc modify_gimbal_angle {par1 par2 par3} {
 global IMUX_ANGLE IMUY_ANGLE IMUZ_ANGLE ANGLE_INCR
 global PIMUX PIMUY PIMUZ bo gimbalLock
 
 if {$gimbalLock == 1} {return}
 
 set sign 0
 set ang_delta 0
 
 if {$bo(12,5) == 1} {return}
 
 # ---- Modify X-Axis ----
 if {$par1 < 0} {set sign  -1} else {set sign 1}
 set IMUX_ANGLE [expr $IMUX_ANGLE + $par1]
 if {$IMUX_ANGLE < 0}   {set IMUX_ANGLE [expr $IMUX_ANGLE + 360]}
 if {$IMUX_ANGLE >= 360} {set IMUX_ANGLE [expr $IMUX_ANGLE - 360]}
   
 set ang_delta [expr abs($IMUX_ANGLE - $PIMUX)]
 if {$ang_delta > 180} {set ang_delta [expr abs($ang_delta - 360)]}
   
 while {$ang_delta >= [expr $ANGLE_INCR + 0.000001]} {
   set PIMUX [expr $PIMUX + $sign * ${ANGLE_INCR}]
   set ang_delta [expr $ang_delta - $ANGLE_INCR]
   if {$PIMUX < 0}   {set PIMUX [expr $PIMUX + 360]}
   if {$PIMUX >= 360} {set PIMUX [expr $PIMUX - 360]}
   if {$sign == "1"} {write_socket "PCDU" 32} else {write_socket "MCDU" 32}
 }
 
 # ---- Modify Y-Axis ----
 if {$par2 < 0} {set sign  -1} else {set sign 1}
 set IMUY_ANGLE [expr $IMUY_ANGLE + $par2]
 if {$IMUY_ANGLE < 0}   {set IMUY_ANGLE [expr $IMUY_ANGLE + 360]}
 if {$IMUY_ANGLE >= 360} {set IMUY_ANGLE [expr $IMUY_ANGLE - 360]}
   
 set ang_delta [expr abs($IMUY_ANGLE - $PIMUY)]
 if {$ang_delta > 180} {set ang_delta [expr abs($ang_delta - 360)]}
   
 while {$ang_delta >= [expr $ANGLE_INCR + 0.000001]} {
   set PIMUY [expr $PIMUY + $sign * ${ANGLE_INCR}]
   set ang_delta [expr $ang_delta - $ANGLE_INCR]
   if {$PIMUY < 0}   {set PIMUY [expr $PIMUY + 360]}
   if {$PIMUY >= 360} {set PIMUY [expr $PIMUY - 360]}
   if {$sign == "1"} {write_socket "PCDU" 33} else {write_socket "MCDU" 33}
 }

 # ---- Modify Z-Axis ----
 if {$par3 < 0} {set sign  -1} else {set sign 1}
 set IMUZ_ANGLE [expr $IMUZ_ANGLE + $par3]
 if {$IMUZ_ANGLE < 0}   {set IMUZ_ANGLE [expr $IMUZ_ANGLE + 360]}
 if {$IMUZ_ANGLE >= 360} {set IMUZ_ANGLE [expr $IMUZ_ANGLE - 360]}
   
 set ang_delta [expr abs($IMUZ_ANGLE - $PIMUZ)]
 if {$ang_delta > 180} {set ang_delta [expr abs($ang_delta - 360)]}
   
 while {$ang_delta >= [expr $ANGLE_INCR + 0.000001]} {
   set PIMUZ [expr $PIMUZ + $sign * ${ANGLE_INCR}]
   set ang_delta [expr $ang_delta - $ANGLE_INCR]
   if {$PIMUZ < 0}   {set PIMUZ [expr $PIMUZ + 360]}
   if {$PIMUZ >= 360} {set PIMUZ [expr $PIMUZ - 360]}
   if {$sign == "1"} {write_socket "PCDU" 34} else {write_socket "MCDU" 34}
 }
 
 if {$IMUZ_ANGLE > 70.1 && $IMUZ_ANGLE < 289.9} {set gimbalLock 1; .text insert end "GIMBAL LOCK!!!\n"; .text yview moveto 1}

 catch {move_fdai_marker}
}


# *************************************************************************************************
# **** Function: Modify PIPA Values to match simulated Speed                                   ****
# *************************************************************************************************
proc modify_pipaXYZ {} {
 global PIPAX_COUNT PIPAY_COUNT PIPAZ_COUNT xVelocity yVelocity zVelocity
 global PIPA_INCR yawDeltaV V
 
 # ---- Calculate X,Y,Z Velocity out of the yawDeltaV ----
 set deltaVX [expr $V(ACC_PIPA,y) * $yawDeltaV]
 set deltaVY [expr $V(ACC_PIPA,x) * $yawDeltaV]
 set deltaVZ [expr $V(ACC_PIPA,z) * $yawDeltaV]
 
 set xVelocity [expr $xVelocity + $deltaVX]
 set yVelocity [expr $yVelocity + $deltaVY]
 set zVelocity [expr $zVelocity + $deltaVZ]
 
 # ---- Modify X-Counter ----
 set counts [expr ($xVelocity - $PIPA_INCR * $PIPAX_COUNT) / $PIPA_INCR]
 for {set i 1} {$i <= [expr abs($counts)]} {incr i 1} {
   if {$counts < 0} {incr PIPAX_COUNT -1; write_socket "MINC" 37}
   if {$counts > 0} {incr PIPAX_COUNT 1; write_socket "PINC" 37}
 }
 
 # ---- Modify Y-Counter ----
 set counts [expr ($yVelocity - $PIPA_INCR * $PIPAY_COUNT) / $PIPA_INCR]
 for {set i 1} {$i <= [expr abs($counts)]} {incr i 1} {
   if {$counts < 0} {incr PIPAY_COUNT -1; write_socket "MINC" 40}
   if {$counts > 0} {incr PIPAY_COUNT 1; write_socket "PINC" 40}
 }
 
 # ---- Modify Z-Counter ----
 set counts [expr ($zVelocity - $PIPA_INCR * $PIPAZ_COUNT) / $PIPA_INCR]
 for {set i 1} {$i <= [expr abs($counts)]} {incr i 1} {
   if {$counts < 0} {incr PIPAZ_COUNT -1; write_socket "MINC" 41}
   if {$counts > 0} {incr PIPAZ_COUNT 1; write_socket "PINC" 41}
 }

 catch {move_fdai_marker}
}



# *************************************************************************************************
# **** Function: Transform Stable Member Based to Gimbal Axes                                  ****
# ****           Rotation around: par1=Axis par2=Angle (Delta Angle in Rad)                    ****
# *************************************************************************************************
proc modify_gimbal_angle_CT {par1 par2} {
 global IMUX_ANGLE IMUY_ANGLE IMUZ_ANGLE
 global PI 2PI RA

 set IMUX_ANGLE_b [expr $IMUX_ANGLE/180.0 * $PI]
 set IMUY_ANGLE_b [expr $IMUY_ANGLE/180.0 * $PI]
 set IMUZ_ANGLE_b [expr $IMUZ_ANGLE/180.0 * $PI]
 
 getRotationMatrixX "GYROX" -$IMUX_ANGLE_b
 getRotationMatrixY "GYROY" -$IMUY_ANGLE_b
 getRotationMatrixZ "GYROZ" -$IMUZ_ANGLE_b
 
 multiplyMatrix "RES1" "GYROY" "GYRO"
 multiplyMatrix "RES2" "GYROZ" "RES1"
 multiplyMatrix "RES" "GYROX" "RES2"
 
 getRotationAnglesXZY "RES"
 
 set tmpx -$RA(RES,x)
 set tmpy -$RA(RES,y)
 set tmpz -$RA(RES,z)
 
 if {$tmpx < 0}   {set tmpx [expr $tmpx + $2PI]}
 if {$tmpx >= $2PI} {set tmpx [expr $tmpx - $2PI]}
 if {$tmpy < 0}   {set tmpy [expr $tmpy + $2PI]}
 if {$tmpy >= $2PI} {set tmpy [expr $tmpy - $2PI]}
 if {$tmpz < 0}   {set tmpz [expr $tmpz + $2PI]}
 if {$tmpz >= $2PI} {set tmpz [expr $tmpz - $2PI]}
 
 set deltax_b [expr $tmpx - $IMUX_ANGLE_b ]
 set deltay_b [expr $tmpy - $IMUY_ANGLE_b ]
 set deltaz_b [expr $tmpz - $IMUZ_ANGLE_b ]
 
 if {$deltax_b < -$PI}   {set deltax_b [expr $deltax_b + $2PI]}
 if {$deltax_b > $PI} {set deltax_b [expr $deltax_b - $2PI]}
 if {$deltay_b < -$PI}   {set deltay_b [expr $deltay_b + $2PI]}
 if {$deltay_b > $PI} {set deltay_b [expr $deltay_b - $2PI]}
 if {$deltaz_b < -$PI}   {set deltaz_b [expr $deltaz_b + $2PI]}
 if {$deltaz_b > $PI} {set deltaz_b [expr $deltaz_b - $2PI]}

 set deltax [expr $deltax_b/$PI*180.0]
 set deltay [expr $deltay_b/$PI*180.0]
 set deltaz [expr $deltaz_b/$PI*180.0]
 
 catch {modify_gimbal_angle $deltax $deltay $deltaz}
}


# *************************************************************************************************
# **** Function: Transform Navigational Based to Stable Member Based Rotation                  ****
# ****           Rotation around: par1=LM Axis par2=Angle (Delta Angle in Degree)              ****
# ****           Angle incements should be small to keep precission                            ****
# *************************************************************************************************
proc Transform_NB_SM {par1 par2} {
 global PI PI2 2PI R RA V
 global IMUX_ANGLE IMUY_ANGLE IMUZ_ANGLE
 
 set bogen [expr $par2/180.0 * $PI]
 
 if {$par1 == "ROLL"}  {getRotationMatrixY "T" $bogen}
 if {$par1 == "PITCH"} {getRotationMatrixX "T" $bogen}
 if {$par1 == "YAW"}   {getRotationMatrixZ "T" $bogen}

 set IMUX_ANGLE_b [expr $IMUX_ANGLE/180.0 * $PI]
 set IMUY_ANGLE_b [expr $IMUY_ANGLE/180.0 * $PI]
 set IMUZ_ANGLE_b [expr $IMUZ_ANGLE/180.0 * $PI]
 
 getRotationMatrixX "IMUY" -$IMUY_ANGLE_b
 getRotationMatrixY "IMUX" -$IMUX_ANGLE_b
 getRotationMatrixZ "IMUZ" -$IMUZ_ANGLE_b
 
 multiplyMatrix "RES1" "IMUX" "T"
 multiplyMatrix "RES2" "IMUZ" "RES1"
 multiplyMatrix "RES" "IMUY" "RES2"
 
 getRotationAnglesXZY "RES"
 
 set tmpx -$RA(RES,y)
 set tmpy -$RA(RES,x)
 set tmpz -$RA(RES,z)
 
 if {$tmpx < 0}   {set tmpx [expr $tmpx + $2PI]}
 if {$tmpx >= $2PI} {set tmpx [expr $tmpx - $2PI]}
 if {$tmpy < 0}   {set tmpy [expr $tmpy + $2PI]}
 if {$tmpy >= $2PI} {set tmpy [expr $tmpy - $2PI]}
 if {$tmpz < 0}   {set tmpz [expr $tmpz + $2PI]}
 if {$tmpz >= $2PI} {set tmpz [expr $tmpz - $2PI]}
 
 set deltax_b [expr $tmpx - $IMUX_ANGLE_b ]
 set deltay_b [expr $tmpy - $IMUY_ANGLE_b ]
 set deltaz_b [expr $tmpz - $IMUZ_ANGLE_b ]
 
 if {$deltax_b < -$PI}   {set deltax_b [expr $deltax_b + $2PI]}
 if {$deltax_b > $PI} {set deltax_b [expr $deltax_b - $2PI]}
 if {$deltay_b < -$PI}   {set deltay_b [expr $deltay_b + $2PI]}
 if {$deltay_b > $PI} {set deltay_b [expr $deltay_b - $2PI]}
 if {$deltaz_b < -$PI}   {set deltaz_b [expr $deltaz_b + $2PI]}
 if {$deltaz_b > $PI} {set deltaz_b [expr $deltaz_b - $2PI]}

 set deltax [expr $deltax_b/$PI*180.0]
 set deltay [expr $deltay_b/$PI*180.0]
 set deltaz [expr $deltaz_b/$PI*180.0]
 
 # --------- Find X,Y,Z IMU Components of the Acceleration Vector along the YAW-AXIS ----
 multiplyMatrixByVector ACC_PIPA ACC_YAW RES
 
 catch {modify_gimbal_angle $deltax $deltay $deltaz}
}


# *************************************************************************************************
# **** Function: Create Rotation Matrix X, Y & Z                                               ****
# *************************************************************************************************
proc getRotationMatrixX {p1 angle} {
 global R

 set R($p1,11) 1
 set R($p1,12) 0
 set R($p1,13) 0
 set R($p1,21) 0
 set R($p1,22) [expr cos($angle)]
 set R($p1,23) [expr -sin($angle)]
 set R($p1,31) 0
 set R($p1,32) [expr sin($angle)]
 set R($p1,33) [expr cos($angle)]
}
 
proc getRotationMatrixY {p1 angle} {
 global R

 set R($p1,11) [expr cos($angle)]
 set R($p1,12) 0
 set R($p1,13) [expr sin($angle)]
 set R($p1,21) 0
 set R($p1,22) 1
 set R($p1,23) 0
 set R($p1,31) [expr -sin($angle)]
 set R($p1,32) 0
 set R($p1,33) [expr cos($angle)]
} 

proc getRotationMatrixZ {p1 angle} {
 global R
 
 set R($p1,11) [expr cos($angle)]
 set R($p1,12) [expr -sin($angle)]
 set R($p1,13) 0
 set R($p1,21) [expr sin($angle)]
 set R($p1,22) [expr cos($angle)]
 set R($p1,23) 0
 set R($p1,31) 0
 set R($p1,32) 0
 set R($p1,33) 1
}


# *************************************************************************************************
# **** Function: Matrix Multiplication                                                         ****
# *************************************************************************************************
proc multiplyMatrix {r1 p1 p2} {
 global R

 set R($r1,11) [expr $R($p1,11) * $R($p2,11) + $R($p1,12) * $R($p2,21) + $R($p1,13) * $R($p2,31)]
 set R($r1,12) [expr $R($p1,11) * $R($p2,12) + $R($p1,12) * $R($p2,22) + $R($p1,13) * $R($p2,32)]
 set R($r1,13) [expr $R($p1,11) * $R($p2,13) + $R($p1,12) * $R($p2,23) + $R($p1,13) * $R($p2,33)]
 set R($r1,21) [expr $R($p1,21) * $R($p2,11) + $R($p1,22) * $R($p2,21) + $R($p1,23) * $R($p2,31)]
 set R($r1,22) [expr $R($p1,21) * $R($p2,12) + $R($p1,22) * $R($p2,22) + $R($p1,23) * $R($p2,32)]
 set R($r1,23) [expr $R($p1,21) * $R($p2,13) + $R($p1,22) * $R($p2,23) + $R($p1,23) * $R($p2,33)]
 set R($r1,31) [expr $R($p1,31) * $R($p2,11) + $R($p1,32) * $R($p2,21) + $R($p1,33) * $R($p2,31)]
 set R($r1,32) [expr $R($p1,31) * $R($p2,12) + $R($p1,32) * $R($p2,22) + $R($p1,33) * $R($p2,32)]
 set R($r1,33) [expr $R($p1,31) * $R($p2,13) + $R($p1,32) * $R($p2,23) + $R($p1,33) * $R($p2,33)]
}


# *************************************************************************************************
# **** Function: Multiply Matrix by Vector                                                     ****
# *************************************************************************************************
proc multiplyMatrixByVector {r1 p1 p2} {
 global R V
 
 set V($r1,x) [expr $V($p1,x) * $R($p2,11) + $V($p1,y) * $R($p2,12) + $V($p1,z) * $R($p2,13)]
 set V($r1,y) [expr $V($p1,x) * $R($p2,21) + $V($p1,y) * $R($p2,22) + $V($p1,z) * $R($p2,23)]
 set V($r1,z) [expr $V($p1,x) * $R($p2,31) + $V($p1,y) * $R($p2,32) + $V($p1,z) * $R($p2,33)]
}


# *************************************************************************************************
# **** Function: Calculate Rotation Angles XZY                                                 ****
# *************************************************************************************************
proc getRotationAnglesXZY {p1} {
 global R RA PI

 set RA($p1,z) [expr asin(-$R($p1,12))]
 set RA($p1,y) [expr atan2($R($p1,13),$R($p1,11))]
 set RA($p1,x) [expr atan2($R($p1,32),$R($p1,22))]
}


# *************************************************************************************************
# **** Function: Calculate Rotation Angles ZYX                                                 ****
# *************************************************************************************************
proc getRotationAnglesZYX {p1} {
 global R RA PI
 
 set RA($p1,y) [expr asin(-$R($p1,31))]
 set RA($p1,x) [expr atan2($R($p1,32),$R($p1,33))]
 set RA($p1,z) [expr atan2($R($p1,21),$R($p1,11))]
}
