namespace eval magfield {
}

# general magnet handler
#  Usage in special driver <drx>:
#
#  at the beginning:
#    include drivers/magfield.tcl
#
#  within stdConfig::<drx>
#    instead of "obj ..."
#      stdConfig::magfield_obj <device descr> <script>
#
#    within first kids:
#      stdConfig::magfield_kids
#
# script must be a script or object supporting the following commands:
#   <script>                         get the persistent field
#   <script> set_field [<value>]     get/set the set_field
#   <script> leads_set               get the field corresponding to the
#                                      current in the leads
#   <script> ramp_state [<value>]    get/set the ramp_target
#                                      (0: hold, 1: goto zero, 2: goto set)
#   <script> heater [<value>]        get/set the heater (0/1)
#   <script> ramp_slow [<value>]     get/set ramp rate for ramping coil
#   <script> ramp_fast               get ramp rate for ramping leads

proc stdConfig::magfield_obj {descr script} {
  obj $descr -drive wr 1
  prop status idle
  prop write magfield::write
  prop read magfield::read
  prop checklimits magfield::checklimits
  prop check magfield::check
  prop halt magfield::halt
  prop script $script
  prop phase 0
}

proc stdConfig::magfield_kids {{bipolar 0}} {
  node statustext upd -text
  prop newline 1

  node ramp par 1
  prop newline 1

  node persistent_mode out
  default 1
  prop write magfield::write_pers
  prop enum off,on
  prop enum_ forever_off=-1,off=0,on=1
  prop label "persistent mode:"
  prop help "hidden mode -1: completely off"

  node gen -none
  kids "general magfield settings" {
    node persistent_delay par 1800
    prop help "timeout for going automatically into persistent mode"

    node tolerance par 0.0002

    node wait_switch_on par 15

    node wait_switch_off par 15

    node wait_stable_leads par 1

    node wait_stable_field par 5

    node expectend -text upd
    default ""

    node trained_pos par 0

    node trained_neg par 0
    
    node profile -text out
    default "99:0.5"
    prop width 40
    prop check magfield::check_profile
    prop write stdSct::completeUpdate
    prop help "syntax: <field1>:<ramp1> <field2>:<ramp2> ... (<ramp2> is the ramp limit from <field1> to <field2>)"

    node profile_training -text out
    default "99:0.2"
    prop width 40
    prop check magfield::check_profile
    prop write stdSct::completeUpdate
    prop help "syntax: <field1>:<ramp1> <field2>:<ramp2> ... (<ramp2> is the ramp limit from <field1> to <field2>)"

    node limit par 15

    node bipolar par $bipolar
    prop enum 1
  }
}

proc magfield::read {} {
  set p [silent "" sct mainpath]
  if {$p eq ""} {
    sct update [eval [sct script]]
  } else {
    link2me $p
  }
  catch {
    lassign [check_field [hval [sct]/gen/limit]] ok txt
    if {!$ok} {
      clientput $txt
    }
  }
  return idle
}

proc magfield::check_field {field} {
  set lim [hval [sct]/gen/limit]
  catch {
    set maxlim 0
    lassign [lindex [hval [sct]/gen/profile] end] maxlim
    if {$lim > $maxlim} {
      set lim $maxlim
      hupdate [sct]/gen/limit $lim
    }
  }
  if {$field < 0 && ![hval [sct]/gen/bipolar]} {
    return [list 0 "ERROR: target ($field) must be > 0" 0]
  } elseif {abs($field) > $lim * 1.01} {
    return [list 0 "ERROR: target ($field) above limit $lim" $lim]
  }
  return [list 1 "" $field]
}

proc magfield::check_target {} {
  lassign [check_field [sct target]] ok txt
  if {!$ok} {
    error $txt
  }
  sct new_target [sct target]
}

proc magfield::check {} {
  check_target
  sct print "run [sct sicsdev] to [sct target]"
}

proc magfield::checklimits {} {
  check_target
  sct status run  
}

proc magfield::write {} {
  [sct controller] poll [sct] 1 progress magfield::run
  hupdate [sct objectPath]/statustext ""
  return idle
}

proc magfield::halt {} {
  if {[sct status] eq "run"} {
    set current_field [eval [sct script]]
    set tol [hval [sct]/gen/tolerance]
    if {abs([sct target] - $current_field) > $tol} { 
       hset [sct] $current_field
       clientput "HALT [sct] at current field $current_field"
    } else {
       clientput "HALT [sct] at target [sct target]"
    }
    sct status idle
  }
  # eval [sct script] ramp_state 0
  return idle
}

proc magfield::write_pers {} {
  set obj [sct objectPath]
  if {[hgetpropval $obj status] eq "idle"} {
    catch {
      hsetprop $obj target [hval $obj]
      [sct controller] poll $obj 1 progress magfield::run
    }
  }
  if {[sct target] == -1} {
    sct enum forever_off=-1,off=0,on
  } else {
    sct enum off,on
  }
  hsetprop $obj last_change [DoubleTime]
  sct update [sct target]
  return idle
}

proc magfield::msg {text} {
  if {$text ne [hval [sct objectPath]/statustext]} {
    clientlog "[sct] $text"
  }
  hupdate [sct objectPath]/statustext $text
}

proc magfield::set_time_if {cond name} {
  upvar now now
  if {[uplevel expr $cond]} {
    if {[silent 0 sct $name] == 0} {
      # clientlog "TIME $name $now"
      sct $name $now
    }
  } else {
    sct $name 0
  }
}

proc magfield::run {} {
  set now [DoubleTime]
  set script [sct script]
  set tol [hval [sct]/gen/tolerance]
  set pm [hval [sct]/persistent_mode]
  set pf [eval $script]
  set tf [silent $pf sct new_target]
  if {[silent none sct target] eq "none"} {
    # make target value visible in UI
    sct target $tf
  }
  sct last_target [silent $pf sct last_target]
  set ls [eval $script leads_set]
  set sf [eval $script set_field]
  set hs [eval $script heater]
  if {$ls > [hval [sct]/gen/trained_pos]} {
    hupdate [sct]/gen/trained_pos $ls
  }
  if {$ls < [hval [sct]/gen/trained_neg]} {
    hupdate [sct]/gen/trained_neg $ls
  }

  set_time_if {$hs == 1} heater_on
  set_time_if {$hs == 0} heater_off
  set_time_if {abs($ls - $pf) < $tol * 5} leads_at_field
  set_time_if {abs($ls - $tf) < $tol * 5} leads_at_target
  if {$tf != [sct last_target]} {
    sct phase 1 ;# start a field change
    sct last_target $tf
  }
  if {$pm == 0} {
    set delay [hval [sct]/gen/persistent_delay]
    if {$now > [silent 0 sct last_change] + $delay} {
      set pm 1
    }
  }
# clientput "hs $hs phase [sct phase] l@f [sct leads_at_field] l@t [sct leads_at_target]"
  if {$hs == 0} {
    if {[sct phase] != 0 || abs($tf - $pf) > $tol || $pm != 1} {
      # we have to go to persistent field
      if {![sct leads_at_field]} {
        if {[sct phase] == 2 &&  $now > [silent $now sct start_phase2] + 300} {
          sct phase 1
          clientlog "timeout ramping to persistent field -> redo"
        }
        if {[sct phase] < 2} {
          sct phase 2 ;# ramping leads to field 
          sct start_phase2 $now
          msg "ramp to persistent field $pf"
          lassign [check_field $pf] ok
          if {!$ok} {
            clientlog "ERROR: illegal persistent field ($pf) or illegal values for limit and bipolar"
            return unpoll
          }
# clientput "set_field $pf"
          eval $script set_field $pf
          eval $script ramp_state 2 ;# ramp to set point
        }
        calc_time $pf $tf [expr [hval [sct]/gen/wait_switch_on] + 60.0 * abs($ls - $pf) / [eval $script ramp_fast]]
        return idle
      }
      if {$now < [sct leads_at_field] + [hval [sct]/gen/wait_stable_leads]} {
        msg "wait for stable leads"
        return idle
      }
      set_time_if 0 ramp_to_pers
      # we are at persistent field
      msg "at persistent field $pf"
      eval $script heater 1
      eval $script ramp_state 0 ;# hold
      sct heater_on 0 ;# register switch on time later when reading back
      return idle
    }
    # persistent field is at target
    if {abs($ls) <= $tol} {
      # we are at field and leads are zero
      msg "persistent and at target"
      sct status idle
      sct phase 0
      return unpoll
    }
    if {![sct heater_off] || $now < [sct heater_off] + [hval [sct]/gen/wait_switch_off]} {
      msg "at target - heater off and wait"
      return idle
    }
    msg "ramp leads to zero"
    eval $script ramp_state 1 ;# ramp to zero
    return idle
  }
  # heater is on
  if {[sct phase] != 0 || ![sct leads_at_target]} {
    lassign [check_field $tf] ok txt
    if {!$ok} {
      clientlog $txt
      lassign [check_field $ls] ok txt corr
      set tf $corr
      sct new_target $tf
    }
    set to_wait [expr [sct heater_on] + [hval [sct]/gen/wait_switch_on] - $now]
    if {![sct heater_on] || $to_wait > 0} {
      msg "heater on and wait"
      calc_time $pf $tf $to_wait
      return idle
    }
    sct phase 0
    set r [hval [sct]/ramp]
    set rl [calc_ramp $ls $tf]
    if {$r > $rl} {
      set r $rl
    }
    sct actual_ramp $r
    msg "ramp to target"
    eval $script ramp_slow $r
# clientput "set_field $tf"
    eval $script set_field $tf
    eval $script ramp_state 2 ;# ramp to set point
    sct last_change [DoubleTime]
    return idle
  }
  sct phase 0
  set t [expr 60 * $tol / max($tol, abs([silent [hval [sct]/ramp] sct actual_ramp]))]
  if {$now < [sct leads_at_target] + [hval [sct]/gen/wait_stable_field] + $t} {
    msg "wait for stable field"
    return idle
  }
  sct status idle ;# reached target
  sct last_target $tf
  hupdate [sct]/gen/expectend ""
  if {$pm == -1} {
    msg "staying forever in non-persistent mode"
    eval $script ramp_state 0 ;# hold
    return unpoll
  }
  if {$pm == 0} {
    msg "stay in non-persistent mode for $delay sec."
    eval $script ramp_state 0 ;# hold
    return idle
  }
  msg "at target - heater off and wait"
  eval $script heater 0
  sct heater_off 0 ;# register switch off time later when reading back
  return idle
}

proc magfield::check_profile {} {
  set oldf 0
  set oldr 999
  foreach item [sct target] {
    lassign [split $item :] f r empty 
    if {$f eq "" || $r eq "" || $empty ne ""} {
      error "[sct]: bad item: $item"
    }
    if {$r > $oldr} {
      error "[sct]: ramps must be decreasing ($oldr > $r)"
    }
    if {$r <= 0} {
      error "[sct]: ramps must be > 0 ($r)"
    }
    if {$f < $oldf} {
      error "[sct]: fields must be increasing ($oldf < $f)"
    }
    set oldf $f
    set oldr $r
  }
}

proc magfield::calc_ramp_p {profile_name field} {
  set obj [sct objectPath]
  foreach item [hval $obj/gen/$profile_name] {
    lassign [split $item :] f r
    if {abs($field) <= $f} {
      return $r
    }
  }
  return $r
}

proc magfield::calc_ramp {field target} {
  set obj [sct objectPath]
  if {$target > $field} {
    set field [expr $field + 0.001]
  } else {
    set field [expr $field - 0.001]
  }
  if {$field >= [hval $obj/gen/trained_pos] ||
      $field <= [hval $obj/gen/trained_neg]} {
    return [calc_ramp_p profile_training $field]
  }
  return [calc_ramp_p profile $field]
}

proc magfield::calc_time_p {profile_name field} {
  set obj [sct objectPath]
  set time 0
  set oldf 0
  foreach item [hval $obj/gen/$profile_name] {
    lassign [split $item :] f r
    if {abs($field) <= $f} {
      set time [expr $time + (abs($field) - $oldf) * 60.0 / $r]
      break
    }
    set time [expr $time + ($f - $oldf) * 60.0 / $r]
    set oldf 0
  }
  if {$field < 0} {
    return [expr -$time]
  }
  return $time
}

proc magfield::calc_time {field target addwait} {
  set obj [sct objectPath]
  set time $addwait
  set t_p [hval $obj/gen/trained_pos]
  if {$field > $t_p} {
    set t_p $field
  }
  set t_n [hval $obj/gen/trained_neg]
  if {$field < $t_n} {
    set t_n $field
  }
  if {$target > $t_p} {
    set time [expr $time + [calc_time_p profile_training $target] \
                         - [calc_time_p profile_training $t_p] \
                         + [calc_time_p profile $t_p] \
                         - [calc_time_p profile $field] ]
  } elseif {$target < $t_n} {
    set time [expr $time + [calc_time_p profile_training $t_n] \
                         - [calc_time_p profile_training $target] \
                         + [calc_time_p profile $field] \
                         - [calc_time_p profile $t_n] ]
  } else {
    set time [expr $time + abs([calc_time_p profile $field] \
                             - [calc_time_p profile $target]) ]
  }
  set ee [expr int([DoubleTime] + $time) + 30]
  hupdate [sct]/gen/expectend [clock format $ee -format %H:%M]
}

proc stdConfig::magfield {} {
  controller syncedprot
  variable name

  magfield_obj SIM_MAGFIELD "node_cmd /$name/sim"
  default 0
  poll 1 progress magfield::simread

  kids "magfield settings" {
    magfield_kids

    node sim upd
    default 0
    kids "sim device" {
      node set_field par 0

      node ramp_slow par 0.1

      node ramp_fast par 100

      node ramp_state par 0
      prop enum hold,to_zero,to_set

      node heater par 0
      prop enum 1

      node leads_set upd
      default 0
    }
  }

}

proc magfield::simleads {script field_out leads_out} {
  upvar $field_out f
  upvar $leads_out ls
  variable last_time

# the result (return value) is set to 1 when leas and target match (go to HOLD)

  set result 0
  set ls [eval $script leads_set]
  set h [eval $script heater]
  set m [eval $script ramp_state]
  if {$h} {
    set r [expr [eval $script ramp_slow] / 60.0]
  } else {
    set r [expr [eval $script ramp_fast] / 60.0]
  }
  set s [eval $script set_field]
  set tg $ls
  switch $m {
    1 {set tg 0}
    2 {set tg $s}
  }
  set now [format %.2f [DoubleTime]]
  if {![info exists last_time($script)]} {
    set last_time($script) $now
    set f $tg
    set ls $tg
    return $result
  }
  set dif [expr $now - $last_time($script)]
  set last_time($script) $now
  if {$ls < $tg} {
    set ls [expr $ls + $dif * $r]
    if {$ls > $tg} {
      set ls $tg
    }
  } elseif {$ls > $tg} {
    set ls [expr $ls - $dif * $r]
    if {$ls < $tg} {
      set ls $tg
    }
  } else {
    set result 1
  }
  if {$h} {
    set f $ls
  } else {
    set f [eval $script]
  }
  return $result
}

proc magfield::simread {} {
  if {[simleads [sct script] f ls]} {
    eval [sct script] ramp_state 0
  }
  hupdate [sct]/sim/leads_set $ls
  hupdate [sct]/sim $f
  return idle
}