source $cfPath(commands)/commands_common.tcl

namespace eval motor {
# is_homing_list = comma separated list of motors which are safe to send "home"
    variable is_homing_list ""
}

namespace eval beam {
  command attenuator {text=in,out,osc pos} {
    switch $pos {
      "in" {
        bat send POS=0
      }
      "out" {
        bat send POS=-1
      }
      "osc" {
        bat send POS=1
      }
      default {
        return -code error "ERROR: valid values are in, out, osc"
      }
    }
  }
}
::beam::attenuator -addfb text BeamAttenuator

namespace eval exp_mode {
    variable valid_modes  
    variable guide_for_mode		;#guide element for a specific mode
    variable c1ht_pos
    #0=polarisation
    #1=mt
    #2=focussing
    #3=DB
    #4=Single
    variable c1ht_pos
    set valid_modes [list SB DB FOC MT POL]
    set c1ht_pos [list 1057 806.7 557.1 200 200]
    
    command set_mode "text=[join $valid_modes ,] arg " {		;#need to change all softzero's
	global ::exp_mode::valid_modes 
	    if {[lsearch $::exp_mode::valid_modes $arg] == -1} {
	    	Clientput "Mode is: $::exp_mode::valid_modes - (polarisation,mt,focussing,DB,single)"
		 	return -code error "Mode is: $::exp_mode::valid_modes - (polarisation,mt,focussing,DB,single)"
	    } else {
	    	if { [catch {::exp_mode::set_guide_element $arg} errMsg] } {
  				Clientput $errMsg
				return -code error $errMsg
		   }
		 mode $arg
	    }
            omega -1
            twotheta -1
	    return -code ok
    }
    
	
    command omega_2theta { float arg1 float arg2 } {
        #need to set omega first.  This is because
        #for Single bounce the twotheta positions depend on the angle of incidence
	if {[catch {::exp_mode::set_omega $arg1} errMsg]} {return -code error $errMsg}
        if {[catch {::exp_mode::set_two_theta $arg2} errMsg]} {return -code error $errMsg}
        return -code ok
    }

}

##
# @brief Drive c1ht and set guide_element parameter
#
# @param arg is the user mode
proc ::exp_mode::set_guide_element { arg } {
  global ::exp_mode::c1ht_pos
    global ::exp_mode::valid_modes

    if {[lsearch $::exp_mode::valid_modes $arg] == -1} {
      Clientput "Mode is: $::exp_mode::valid_modes - (polarisation,mt,focussing,DB,single)"
        return -code error "Mode is: $::exp_mode::valid_modes - (polarisation,mt,focussing,DB,single)"
    }

  if {[catch {::exp_mode::checkMotionAndDrive c1ht [lindex $c1ht_pos [lsearch $::exp_mode::valid_modes $arg]]} errMsg]} {
    return -code error $errMsg
  } else {
    guide_element $arg
      return -code ok	
  }
}	

proc ::exp_mode::set_omega { arg } {
  set expmode [SplitReply [mode]]
    if {[lsearch $::exp_mode::valid_modes $expmode] == -1} {
      Clientput "Please set the mode first"
        return -code error "Please set the mode first"
    }

  if {$arg<0} {
    return -code error "omega must be greater than 1"
  }

  if {[catch {::exp_mode::set_guide_element $expmode} errMsg]} {
    return -code error $errMsg		;#make sure the guide element is moved.
  }
  set argrad [deg2rad $arg]	;#position in radians

    switch $expmode {
      SB {
        if {[catch {::exp_mode::checkMotionAndDrive m1ro [expr $arg/2.]} errMsg]} {return -code error $errMsg}

        set d1 [expr [SplitReply [slit3_distance]] - [SplitReply [guide1_distance]]]
          set d2 [expr [SplitReply [sample_distance]] - [SplitReply [guide1_distance]]]
          set h1 [expr -1. * $d1 * tan($argrad)]
          set h2 [expr -1. * $d2 * tan($argrad)]

          if {[catch {checkMotionAndDrive st3vt $h1} errMsg]} {return -code error $errMsg}
        if {[catch {checkMotionAndDrive sz $h2} errMsg]} {return -code error $errMsg}
      }
      DB {
        return -code error "ERROR: set_omega not yet defined for DB"
          set temp [deg2rad 2.4]
          set offset [expr 600*sin($temp)]
          set arg 4.8		;#fixed angle

          set d1 [expr [SplitReply [slit3_distance]] - [SplitReply [guide2_distance]]]
          set d2 [expr [SplitReply [sample_distance]] - [SplitReply [guide2_distance]]]
          set h1 [expr -1. * $d1 * tan($argrad) - $offset] 
          set h2 [expr -1. * $d1 * tan($argrad) - $offset]

          if { [catch {checkMotionAndDrive st3vt $h1} errMsg]} {return -code error $errMsg}
        if { [catch {checkMotionAndDrive sz $h2} errMsg]} {return -code error $errMsg}
      }
      FOC {
        if { [catch {checkMotionAndDrive sth $arg} errMsg]}	{return -code error $errMsg}
      }
      MT {
        if { [catch {checkMotionAndDrive sth $arg} errMsg]}	{return -code error $errMsg}
      }
      default {
        return -code error "omega driving not specified for that mode"	
      }
    }

  omega $arg
    return -code ok
}
publish ::exp_mode::set_omega user
	
proc ::exp_mode::set_two_theta { arg } {
  set expmode [SplitReply [mode]]
    set expomega [SplitReply [omega]]

    if {[lsearch $::exp_mode::valid_modes $expmode] == -1} {
      return -code error "please set the mode and omega first"	
    }
  if {$expomega == "NaN"} {
    return -code error "please set omega first"
  }
  if {$arg<0} {
    return -code error "two_theta is less than 0"
  }

  set argrad [deg2rad $arg]	;#position in radians
    set omegarad [deg2rad $expomega]

    Clientput $expmode
    switch $expmode {
      SB {
        set d1 [expr [SplitReply [slit4_distance]] - [SplitReply [sample_distance]]]
          set d2 [expr [SplitReply [slit4_distance]] - [SplitReply [guide1_distance]]]
          set h1 [expr -1. * $d2 * tan($omegarad)]
          set b  [expr $d1 / cos($omegarad)]
          set c  [expr $d1 / cos($argrad-$omegarad)]
          set h2 [expr sqrt(pow($b,2) + pow($c,2) - 2*$b*$c*cos($argrad))]

          set d3 [expr [SplitReply [dy]]]
          set d4 [expr [SplitReply [dy]] + [SplitReply [sample_distance]] - [SplitReply [guide1_distance]]]
          set h3 [expr -1. * $d4 * tan($omegarad)]
          set b  [expr $d3 / cos($omegarad)]
          set c  [expr $d3 / cos($argrad-$omegarad)]
          set h4 [expr sqrt(pow($b,2) + pow($c,2) - 2*$b*$c*cos($argrad))] 
          if { [catch {checkMotionAndDrive dz [expr $h3 + $h4]} errMsg]} {return -code error $errMsg}
        if { [catch {checkMotionAndDrive st4vt [expr $h2 + $h1]} errMsg]} {return -code error $errMsg}
      }
      DB {
        return -code error "ERROR: set_omega not yet defined for DB"
          set temp [deg2rad 2.4]
          set offset [expr 600*sin($temp)]
          set expomega 4.8
          set omegarad [deg2rad $expomega]

          set d1 [expr [SplitReply [slit4_distance]] - [SplitReply [sample_distance]]]
          set d2 [expr [SplitReply [slit4_distance]] - [SplitReply [guide2_distance]]]
          set h1 [expr -1. * $d2 * tan($omegarad) - $offset]
          set b  [expr $d1 / cos($omegarad)]
          set c  [expr $d1 / cos($argrad-$omegarad)]
          set h2 [expr sqrt(pow($b,2) + pow($c,2) - 2*$b*$c*cos($argrad))]

          set d3 [expr [SplitReply [dy]]]
          set d4 [expr [SplitReply [dy]] + [SplitReply [sample_distance]] - [SplitReply [guide2_distance]]]
          set h3 [expr -1. * $d4 * tan($omegarad) - $offset]
          set b  [expr $d3 / cos($omegarad)]
          set c  [expr $d3 / cos($argrad-$omegarad)]
          set h4 [expr sqrt(pow($b,2) + pow($c,2) - 2*$b*$c*cos($argrad))] 
          if { [catch {checkMotionAndDrive dz [expr $h3 + $h4]} errMsg]} {return -code error $errMsg}
        if { [catch {checkMotionAndDrive st4vt [expr $h2 + $h1]} errMsg]} {return -code error $errMsg}

      }
      FOC {
        set d1 [SplitReply [dy]]
          set d2 [expr [SplitReply [slit4_distance]] - [SplitReply [sample_distance]]]
          set h1 [expr $d1 * tan($argrad)]
          set h2 [expr $d2 * tan($argrad)]
          if { [catch {checkMotionAndDrive dz $h1} errMsg]} {return -code error $errMsg}
        if { [catch {checkMotionAndDrive st4vt $h2} errMsg]} {return -code error $errMsg}
      }
      MT {
        set d1 [SplitReply [dy]]
          set d2 [expr [SplitReply [slit4_distance]] - [SplitReply [sample_distance]]]
          set h1 [expr $d1 * tan($argrad)]
          set h2 [expr $d2 * tan($argrad)]
          if { [catch {checkMotionAndDrive dz $h1} errMsg]} {return -code error $errMsg}
        if { [catch {checkMotionAndDrive st4vt $h2} errMsg]} {return -code error $errMsg}
      }
      default {
        return -code error "two_theta not defined for that mode: $expmode"
      }
    }
  twotheta $arg
    return -code ok
}
publish ::exp_mode::set_two_theta user

proc ::exp_mode::checkMotion { scan_variable target } {
  set motor_list [sicslist type motor]

    if {[lsearch $motor_list $scan_variable]==-1} {
      return -code error "you tried to drive a motor that doesn't exist"   
    }
  if {[catch {	
    ::scan::check_limit $scan_variable hardlowerlim $target
      ::scan::check_limit $scan_variable hardupperlim $target
      ::scan::check_limit $scan_variable softlowerlim $target
      ::scan::check_limit $scan_variable softupperlim $target
  }]} {
    return -code error $::errorInfo
  }
  return -code ok
}
publish ::exp_mode::checkMotion user

proc ::exp_mode::checkMotionAndDrive { scan_variable target } {
  set motor_list [sicslist type motor]
#lappend motorlist [sicslist type configurablevirtualmotor]

    set precision [SplitReply [$scan_variable precision]]           

    if {[catch {	
      ::exp_mode::checkMotion $scan_variable $target
    }]} {
      return -code error $::errorInfo
    } else {
      drive $scan_variable $target
        set position [SplitReply [$scan_variable]]			
        if {[expr [expr $position-$target] > abs($precision)]} {
          return -code error "move of: $scan_variable did not reach required precision"
        } else {
          Clientput "New $scan_variable Position: $position"
            Clientput "Driving finished successfully"
        }
      return -code ok	
    }
}
publish ::exp_mode::checkMotionAndDrive user

proc ::exp_mode::deg2rad { arg } {
  set pi 3.1415926535897931
    return [expr $pi * $arg / 180.]
}

proc ::exp_mode::rad2deg { arg } {
  set pi 3.1415926535897931
    return [expr 180. * $arg / $pi]
}

##
# @brief Commands initialisation procedure
proc ::commands::isc_initialize {} {
  ::commands::ic_initialize
}