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 sample {
#  command select {int=0:8 sampid} {
#    SampleNum $sampid
#  }
#}

##
# @brief Beamstop select command
# @param bs beamstop, 1,2,3,4,5 or 6
# @param bx beam position in detector coordinates
# @param bz beam position in detector coordinates
# Given
# (Xbf,Zbf) = beam pos in frame coords
# (Xbd,Zbd) = beam pos in detector coords
# (Xbbs, Zbbs) = beam pos in beamstop coords
# (Xdf,Zdf) = detector pos in frame coords
# (Xbsf,Zbsf) = beamstop pos in frame coords
# 
# (Xbf,Zbf) = (Xdf+Xbd, Zdf+Zbd) = (Xbsf+Xbbs, Zbsf+Zbbs)
# Origin of detector coords = frame origin
# 
# Detector and beamstop motor readings with beamstop disk centers
# overlapping over detector center mark.
# Xdf = 264.542  (7283813) Zdf = 0
# Xbbs = 296.291 (8054270)
# Zbbs = 259.641 (13488244)
# Xbd = dethw, Zbd = (dethh)
# 
# 264.542+dethw = Xbsf+296.291
# 0+dethh = Zbsf+259.641
# 
# Xbsf = -31.749 + dethw, Zbsf = -259.641 + dethh
proc selbs {bs {bx "UNDEF"} {bz "UNDEF"}} {
  set bsdriving false
  set dethw [expr {[SplitReply [detector_active_height_mm]]/2.0}]
  set dethh [expr {[SplitReply [detector_active_width_mm]]/2.0}]
  set Xbsf [expr -31.749 + $dethw]
  set Zbsf [expr -259.641 + $dethh]
  array set bsl [subst {
    1 [SplitReply [bs1 softlowerlim]]
    2 [SplitReply [bs2 softlowerlim]]
    3 [SplitReply [bs3 softlowerlim]]
    4 [SplitReply [bs4 softlowerlim]]
    5 [SplitReply [bs5 softlowerlim]]
    6 [SplitReply [bs6 softlowerlim]]
  }]
  if [ catch {
    switch $bs {
      "1" {
        set bsmot "bs1"
        set bs_target 93.20
        set bs_diameter 110
        set bsdownCmd "drive bs2 $bsl(2) bs3 $bsl(3) bs4 $bsl(4) bs5 $bsl(5) bs6 $bsl(6)"
      }
      "2" {
        set bsmot "bs2"
        set bs_target 86.84
        set bs_diameter 88
        set bsdownCmd "drive bs1 $bsl(1) bs3 $bsl(3) bs4 $bsl(4) bs5 $bsl(5) bs6 $bsl(6)"
      }
      "3" {
        set bsmot "bs3"
        set bs_target 93.35
        set bs_diameter 66
        set bsdownCmd "drive bs2 $bsl(2) bs1 $bsl(1) bs4 $bsl(4) bs5 $bsl(5) bs6 $bsl(6)"
      }
      "4" {
        set bsmot "bs4"
        set bs_target 86.85
        set bs_diameter 44
        set bsdownCmd "drive bs2 $bsl(2) bs3 $bsl(3) bs1 $bsl(1) bs5 $bsl(5) bs6 $bsl(6)"
      }
      "5" {
        set bsmot "bs5"
        set bs_target 93.27
        set bs_diameter 22
        set bsdownCmd "drive bs2 $bsl(2) bs3 $bsl(3) bs4 $bsl(4) bs1 $bsl(1) bs6 $bsl(6)"
      }
      "6" {
        set bsmot "bs6"
        set bs_target 86.98
        set bs_diameter 11
        set bsdownCmd "drive bs2 $bsl(2) bs3 $bsl(3) bs4 $bsl(4) bs5 $bsl(5) bs1 $bsl(1)"
      }
      default {
        error "beamstop selection must be an integer from 1 to 6"
      }
    }

    set detoff_val [SplitReply [detoff]]

    if {$bx == "UNDEF" || $bz == "UNDEF"} {
      statemon start selbs
      if {[info level] > 1} {statemon start [lindex [info level -1] 0]}
      set bsdriving true
      BeamStop -1
      BSdiam -1
      drive $bsmot $bs_target
      eval $bsdownCmd
      BeamStop $bs
      BSdiam $bs_diameter
      set bsdriving false
      statemon stop selbs
      if {[info level] > 1} {statemon stop [lindex [info level -1] 0]}
    } else {
      if { [string is double $bx] == false } {
      if { [string is double $bz] == false } {
        error "beam coordinates must be floats"
        }
      }
      set bsx_target [expr {$bx-$Xbsf+$detoff_val}]
      set bsz_target [expr {$bz-$Zbsf}]

      statemon start selbs
      if {[info level] > 1} {statemon start [lindex [info level -1] 0]}
      set bsdriving true
      BeamStop -1
      BSdiam -1
      drive $bsmot $bs_target bsx $bsx_target bsz $bsz_target
      eval $bsdownCmd
      BeamStop $bs
      BSdiam $bs_diameter
      set bsdriving false
      statemon stop selbs
      if {[info level] > 1} {statemon stop [lindex [info level -1] 0]}
    }
  } msg ] {
    if {$bsdriving} {
      statemon stop selbs
      if {[info level] > 1} {statemon stop [lindex [info level -1] 0]}
    }
    return -code error $msg
  }
}
publish selbs user
namespace eval beamstops {
  command selbsn {int=1,2,3,4,5,6 bs} {
    selbs $bs "UNDEF" "UNDEF"
  }
  command selbsxz {int=1,2,3,4,5,6 bs float bx float bz} {
    selbs $bs $bx $bz
  }
}

namespace eval optics {
	VarMake ::optics::select::section text user
	VarMake ::optics::polarizer::in   text user
	VarMake ::optics::lens::selection text user
	
  command rotary_attenuator {int=0,15,45,90,180 angle} {
    drive att $angle
  }

  command entrance_aperture {
    int=0,45,90,135,180,270 angle
  } {
    drive srce $angle
  }

# TODO Do we need this
# command sample_aperture {
#   int=25,50 size
#   text=circ,squ,open,rect shape
# } {
#   SApXmm $size
#   SApZmm $size
#   SApShape $shape
# }

##############################
##
# @brief The "guide" command uses a lookup table to setup the collimation system
# @param row, selects a row from the guide configuration table
#
# eg\n
# guide ga 
  command guide "
  text=[join [array names ::optics::guide_configuration] , ] configuration
  " {

    variable guide_configuration
    variable guide_configuration_columns

    if [ catch {

      foreach {compselection position} $guide_configuration($configuration) {
        foreach el $compselection guide $guide_configuration_columns {
          lappend to_config $guide
          lappend to_config [set ::optics::${guide}_map($el)] 
        }
        ::optics::guide -set feedback status BUSY
        set msg [eval "drive $to_config"]
        EApPosY $position
      }
      GuideConfig $configuration
    } message ] {
      ::optics::guide -set feedback status IDLE
      if {$::errorCode=="NONE"} {return $message}
      return -code error $message
    }
    ::optics::guide -set feedback status IDLE
  }
  ::optics::guide -addfb text status
  ::optics::guide -set feedback status IDLE
}

proc ::commands::isc_initialize {} {
  ::commands::ic_initialize
}