# general temperature driver
namespace eval trun {
}

global startupList
if {![info exists startupList(drivers/trun.tcl)]} {
  set startupList(drivers/trun.tcl) 0
}

# this function might be used to make an object t-driveable

proc stdConfig::tdrive args {
  variable name
  variable node
  variable path
  variable ctrl
  variable fastperiod

  scanargs $args var -title -getcmd self -setcmd self -log 0 -getsample 0 -dbl 0 -limitscript 0

  set node $path
  prop read trun::read_run
# make read action fast
  $ctrl poll $node $fastperiod read read
  prop checklimits trun::checklimits
  prop halt trun::halt
  prop write trun::write
  prop status idle
  prop help $name
  prop runlabel "actual"
  prop getcmd $getcmd
  prop setcmd $setcmd
  prop getsample $getsample
  prop limitscript $limitscript
  prop initime [clock seconds]
  prop init 1
  if {$getcmd eq "self"} {
    prop selfupdate 0
  } else {
    prop selfupdate 1
  }
  prop dif 0

  kids $title {
    if {[string match {set*} $setcmd]} {
      node mainloop -text out
      default $setcmd
      prop visible false
      prop check trun::checkmainloop
      prop write stdSct::complete
    } else {
      node set out
      default 0
      prop write trun::writeset
    }

    node target upd

    node running -int upd

    node tolerance par 1

    node maxwait par 3600

    node settle par 0

    if {$log} {

      node log -text out
      prop check trun::logcmd
      prop write stdSct::complete
      prop visible false

      kids hidden {

        node mean upd

        node m2 upd

        node stddev upd

        node n upd
      }
    }
    if {$dbl} {
      node dblctrl par 0
      prop label "double control"
      prop enum 1
#      prop enum off=0,normal=1,new=2

      kids "double control settings" {
        node tshift out
        prop check trun::dblctrl_tshift
        prop write stdSct::complete
        default 0

        if {$dbl} {
          node mode par 1
        } else {
          node mode par -1
        }
        prop enum disabled=-1,inactive=0,stable=1,up=2,down=3

        node shift_up upd

        node shift_lo upd

        node t_min upd

        node t_max upd

        node int2 par 300

        node prop_up par 0.4

        node prop_lo par 0.8

      }
    }
  }
  set node $path
}

proc stdConfig::trun {{title temperature} {getcmd self} {setcmd self}} {
  variable path
  controller syncedprot 10

  pollperiod 1
  obj trun -drive wr
  stdConfig::tdrive $title $getcmd $setcmd
}


proc trun::dblctrl_filter {value name period filtimes lowerVar upperVar} {
  upvar $lowerVar lv
  upvar $upperVar uv
  set n 0
  set mn $value
  set mx $value
  foreach {tim1 tim2} $filtimes {
    incr n
    set lower [silent $value sct ${name}_mn_$n]
    set upper [silent $value sct ${name}_mx_$n]
    if {$tim1 <= 0} {
      set w1 1
    } else {
      set w1 [expr 1 - exp(- $period / double($tim1))]
    }
    if {$tim2 <= 0} {
      set w2 1
    } else {
      set w2 [expr 1 - exp(- $period / double($tim2))]
    }
    set u $mx
    set dif [expr $mx - $upper]
    if {$dif > 0} {
      set mx [expr $upper + $w1 * $dif]
    } else {
      set mx [expr $upper + $w2 * $dif]
    }
    set dif [expr $mn - $lower]
    if {$dif < 0} {
      set mn [expr $lower + $w1 * $dif]
    } else {
      set mn [expr $lower + $w2 * $dif]
    }
    sct ${name}_mx_$n $mx
    sct ${name}_mn_$n $mn
  }
  if {$upper < $mx} {
    set upper $mx
  }
  if {$lower > $mn} {
    set lower $mn
  }
  set lv $lower
  set uv $upper
  return $n
}

proc trun::dblctrl_tshift {} {
  hsetprop [sct parent [sct parent]] resetdblctrl 1
  sct update [sct target]
}

proc trun::dblctrl_crop {mn mx} {
  if {$mn > [sct tf_upper]} {
    set d [expr $mn - [sct tf_upper]]
    for {set n 0} {$n < [sct tf_n]} {incr n} {
      sct tf_mn_$n [expr [sct tf_mn_$n] + $d]
    }
  }
  if {$mx < [sct tf_lower]} {
    set d [expr $mx - [sct tf_lower]]
    for {set n 0} {$n < [sct tf_n]} {incr n} {
      sct tf_mx_$n [expr [sct tf_mx_$n] + $d]
    }
  }
}

proc trun::dblctrl {tm ts tr setpdif {active 1}} {
# active=1: active, active=2: suspended, else "sleeping"
  set d [expr $tr - $ts]
  set tshift [hvali [sct]/dblctrl/tshift]
  set flist {10 20}
  set int2 [hvali [sct]/dblctrl/int2]
  if {[silent 0 sct resetdblctrl]} {
    sct resetdblctrl 0
    set t0 [expr $ts + $tshift]
    clientput "reset double control tm $tm ts $ts tshift $tshift"
    if {$tm > $t0} {
      set t1 $tm
    } else {
      set t1 $t0
      set t0 $tm
    }
    sct tf_m_mx_1 $t1
    sct tf_m_mx_2 $t1
    sct tf_m_mx_3 $t1
    sct tf_m_mn_1 $t0
    sct tf_m_mn_2 $t0
    sct tf_m_mn_3 $t0
    sct tf_s_mx_1 $ts
    sct tf_s_mn_1 $ts
    return
  }
#  lappend flist [expr $int2 * 0.06]
#  lappend flist [expr $int2 * 0.5]
#  lappend flist [expr $int2 * 0.67]
#  lappend flist [expr $int2 * 2.0 + 10]
  lappend flist [expr $int2 * 0.3]
  lappend flist [expr $int2 * 0.5]
  lappend flist [expr $int2 * 1.0]
  lappend flist [expr $int2 * 1.5 + 10]
  sct tf_n [dblctrl_filter $tm tf_m 5 $flist mLower mUpper]
  sct tf_upper $mUpper
  sct tf_lower $mLower
  dblctrl_filter $ts tf_s 5 {10 20} sLower sUpper
  if {[silent 0 sct logfilter]} {
    clientput "$sLower $sUpper filter $mLower $mUpper $flist"
  }
  hupdate [sct]/dblctrl/t_max $mUpper
  hupdate [sct]/dblctrl/t_min $mLower
  set target [hvali [sct]/target]
  set dblmode [silent -1 hvali [sct]/dblctrl/mode]
  if {$active != 1} {
    if {$active == 0 && $dblmode > 0} {
      internalset [sct]/set $target
    }
    hupdate [sct]/dblctrl/mode 0 ;# inactive
    return
  }
  set shift_lo [expr $mLower - $sUpper]
  set shift_up [expr $mUpper - $sLower]
  set dif2 [expr $d - [sct dif]]
  if {$setpdif > 0} {
    if {$target > $ts + 1.0 && $target >= 20} {
      if {$dblmode != 2} {
        set dblmode 2 ;# up
        clientput "dblctrl mode up"
      }
    }
  } elseif {$setpdif < 0} {
    if {$tr != 0} {
      set tshift [expr $tshift / (1 - $setpdif / double($tr))]
    }
    if {$target < $ts - 1.0 && $target >= 20} {
      if {$dblmode != 3} {
        set dblmode 3 ;# down
        clientput "dblctrl mode down"
      }
    }
  }
  set td [expr $target - $ts]
  if {$dblmode != 1} { # not stable
    set tol [expr pow($td, 2) / ($target + $ts) * 2.0]
    set shift_up [expr $shift_up + $tol]
    set shift_lo [expr $shift_lo - $tol]
  }
  hupdate [sct]/dblctrl/shift_up $shift_up
  hupdate [sct]/dblctrl/shift_lo $shift_lo
#  hupdate [sct]/dblctrl/t_max [expr $ts + $shift_up - $tshift]
#  hupdate [sct]/dblctrl/t_min [expr $ts + $shift_lo - $tshift]
  if {$tshift > $shift_up} {
    set tshift $shift_up
  } elseif {$tshift < $shift_lo} {
    set tshift $shift_lo
  }
  hupdate [sct]/dblctrl/tshift $tshift

  #clientput $td
  if {$dblmode == 2} { # up
    if {$td <= 0} {
      set dblmode 1
      clientput "dblctrl mode stable"
    } else {
      set tshift [expr $tshift + [hvali [sct]/dblctrl/prop_up] * $td]
    }
  } elseif {$dblmode == 3} { # down
    if {$td >= 0} {
      set dblmode 1
      clientput "dblctrl mode stable"
    } else {
      set tshift [expr $tshift + [hvali [sct]/dblctrl/prop_lo] * $td]
    }
  }
  hupdate [sct]/dblctrl/mode $dblmode
  if {abs($tshift) > $target * 0.5} {
    if {$tshift > 0} {
      set tshift [expr $target * 0.5]
    } else {
      set tshift [expr - $target * 0.5]
    }
  }
  set setp [expr $target + $tshift]
  catch {
    set setlimit [hvali [sct]/set/limit]
    if {$target <= $setlimit && $setp > $setlimit} {
      set setp $setlimit
    }
  }
  if {abs([hvali [sct]/set] - $setp) > $setp * 1e-5} {
    if {[silent "" hgetpropval [sct]/set/reg geterror] eq ""} {
      if {[sct limitscript] ne "0"} {
        set setp [eval "[sct limitscript] $setp fix"]
      }
      internalset [sct]/set $setp
    }
  }
}

proc trun::minmax {minvar maxvar first args} {
  upvar $minvar min
  upvar $maxvar max
  set min $first
  set max $first
  foreach v $args {
    if {$v > $max} {
      set max $v
    } elseif {$v < $min} {
      set min $v
    }
  }
}

proc trun::dblctrl2 {tm ts tr setpdif {active 1}} {
  set ft [expr [sct filtertime] * 0.2]
  if {$tr != [sct oldtr]} {
    sct oldtr $tr
    set ft 6.0
  } else {
    set ft [format %.2f [expr $ft + 0.332]]
  }
  sct filtertime [expr 5 * $ft]
  set w [expr 1.0 / $ft]
  set target [hvali [sct]/target]
  set y [expr $tm - $ts]
  set yy [silent $y sct yy]
  set yy1 [silent $y sct yy1]
  set dd [silent 0 sct dd]
  set cc [silent 0 sct cc]
  set cc1 [silent 0 sct cc1]

  set yy1 [expr $yy1 + ($y - $yy1) * $w]
  set d [expr ($yy1 - $yy) * $w]
  set yy [expr $yy + $d]
  set c [expr ($d - $dd) * 2 * $w]
  set dd [expr $dd + $c]
  set cc1 [expr $cc1 + ($c - $cc1) * 4 * $w]
  set cc [expr $cc + ($cc1 - $cc) * 4 * $w]

  sct yy $yy
  sct yy1 $yy1
  sct dd $dd
  sct cc $cc
  sct cc1 $cc1

  set s [expr 7 / $w / 6]
#clientput [format "%8.4g %8.4g %8.4g %8.4g" $y $yy [expr $dd * $s] [expr 0.25 * $cc * $s * $s]]
  set tshift [hvali [sct]/dblctrl/tshift]
  set yd [expr $yy + $dd * $s]
  set ys [expr $yd + 0.25 * $cc * $s * $s]
  minmax mn mx $yy $yd $ys
  if {$mx < $tshift} {
    set tshift $mx
  } elseif {$mn > $tshift} {
    set tshift $mn
  }
  if {abs($tshift) > $target * 0.2} {
    if {$tshift > 0} {
      set tshift [expr $target * 0.2]
    } else {
      set tshift [expr - $target * 0.2]
    }
  }
  hupdate [sct]/dblctrl/tshift $tshift
  hupdate [sct]/dblctrl/shift_up $mx
  hupdate [sct]/dblctrl/shift_lo $mn
  set setp [expr $target + $tshift]
  if {abs([hvali [sct]/set] - $setp) > $setp * 1e-5} {
    if {[silent "" hgetpropval [sct]/set/reg geterror] eq ""} {
      internalset [sct]/set $setp
    }
  }
}

proc trun::read_run {} {
  if {[sct init]} {
    foreach kid [hinfo [sct]] {
      if {[string match {set*} $kid]} {
        hsetprop [sct]/[sct setcmd] setmaintarget "trun::setmaintarget [sct]"
      }
    }
    sct init 0
  }
  if {[sct status] eq "run"} {
    set running 1
  } else {
    set running 0
  }
  hupdate [sct]/running $running
  set cmd [sct getcmd]
  if {$cmd eq "self"} {
    set tmain [hvali [sct]]
  } else {
    set tmain [result "silent none $cmd"]
    if {$tmain eq "none"} {
      sct geterror "invalid"
      return idle
    } else {
      hdelprop [sct] geterror
    }
  }
  set cmd [sct getsample]
  set tsample $tmain
  set dblctrl 0
  if {$cmd ne "0"} {
    if {[regexp {hvali (.*)} $cmd -> path]} {
      if {[silent "" hgetpropval $path geterror] ne ""} {
        set cmd error
      }
    }
    catch {
      set tsample [result $cmd]
      set dblctrl [silent 0 hvali [sct]/dblctrl]
    }
  }
  if {$dblctrl} {
    if {[hval [sct]/dblctrl/mode] == 0} { # inactive -> stable
      set tol [hvali [sct]/tolerance]
      set tg [silent $tsample sct target]
      if {$tg > $tsample + $tol} {
         set dmode 2  ;# up
      } elseif {$tg < $tsample - $tol} {
         set dmode 3  ;# down
      } else {
         set dmode 1  ;# stable
      }
      hupdate [sct]/dblctrl/mode $dmode
    }
    set value $tsample
  } else {
    set value $tmain
  }
  sct tsample $tsample
  if {[sct selfupdate]} {
    set err [silent "" hvali [sct]/status]
    if {$err eq ""} {
      sct update $tsample
    } else {
      sct geterror $err
    }
  }
  if {[sct status] eq "run"} {
    if {[clock seconds] > [sct startrun] + [hvali [sct]/maxwait]} {
      clientput "timeout when driving [sct objectName]"
      sct status idle
    } elseif {abs($value - [sct target]) < [hvali [sct]/tolerance]} {
      set remaining 0
      set settle [hvali [sct]/settle]
      if {![sct intolerance]} {
        set startsettle [expr [clock seconds] - [sct settletime]]
        if {$startsettle > [sct startsettle] + 10} {
          set remaining [expr $settle - [sct settletime]]
        }
        sct startsettle $startsettle
        sct intolerance 1
      }
      if {[clock seconds] >= [sct startsettle] + [hvali [sct]/settle]} {
        sct status idle
      } elseif {$remaining > 0} {
        clientput "[sct objectName] in tolerance, wait $remaining sec."
        sct printit 1
      }
    } else {
      if {[sct intolerance]} {
        sct settletime [expr [clock seconds] - [sct startsettle]]
        sct intolerance 0
      }
      if {[sct printit]} {
        sct printit 0
        clientput "[sct objectName] out of tolerance"
      }
    }
  }
  set now [DoubleTime]
  set tr [silent none sct target]
  set prev_tr [silent $tr sct prev_tr]
  sct prev_tr $tr
  if {$tr eq "none"} {
    set tr $tmain
    sct prev_tr $tmain
  } else {
    set dif [expr $tr - $prev_tr]
    updateval_u [sct]/target $tr 0
    set dblmode [silent -1 hval [sct]/dblctrl/mode]
    if {$dblmode >= 0} { # not disabled
      if {[sct initime] == 0} {
        # initialization has finished
        if {$now < [silent 0 sct dblctrl_sleep]} {
          dblctrl $tmain $tsample $tr $dif 2
        } else {
          dblctrl $tmain $tsample $tr $dif $dblctrl
        }
      } else {
        if {[clock seconds] > [sct initime] + 30} {
          sct initime 0
        }
      }
    }
  }
  set stat [silent 0 sct statistics]
  if {$stat > 0 && $now > $stat + 10} {
    sct statistics $now
    set n [hvali [sct]/log/n]
    set mean [hvali [sct]/log/mean]
    set m2 [hvali [sct]/log/m2]
    incr n
    set delta [expr $tsample - $mean]
    set mean [expr $mean + $delta / $n]
    set m2 [expr $m2 + $delta * ($tsample - $mean)]
    hupdate [sct]/log/n $n
    hupdate [sct]/log/mean $mean
    hupdate [sct]/log/m2 $m2

    if {$n > 1} {
      hupdate [sct]/log/stddev [expr sqrt($m2/ ($n - 1))]
    }
  }
  return idle
}

proc trun::write {} {
  if {[silent 0 sct writestatus] eq "start"} {
    # initiated by a run/drive command
    sct status run
    sct startrun [clock seconds]
    sct settletime 0
    sct intolerance 0
    sct startsettle 0
    sct printit 0
    sct print "run [sct objectName] to [sct target]"
  }
  set cmd [sct setcmd]
  set setp [sct target]
  if {[silent -1 hval [sct]/dblctrl/mode] > 0} {
    set setp [expr $setp + [silent 0 hvali [sct]/dblctrl/tshift]]
  }
  if {$cmd eq "self"} {
    internalset [sct]/set $setp
  } elseif {[string match {set*} $cmd]} {
    internalset [sct]/[hval [sct]/mainloop] $setp
  } else {
    eval $cmd $setp
    hupdate [sct]/set $setp
  }
  return idle
}

proc trun::writeset {} {
  if {[hgetpropval [sct parent] setcmd] ne "self"} {
    internalset [sct objectPath] [sct target]
  }
  sct print "[sct sicscommand] = [sct target]"
  return idle
}

proc trun::checklimits {} {
  # TODO: does the following harm?
  hupdate [sct]/target [sct target]
  if {[sct limitscript] ne "0"} {
    eval "[sct limitscript] [sct target]"
  }
  return 0
}

proc trun::checkmainloop {} {
   hsetprop [sct parent] setcmd [sct target]
   if {[string match {set*} [sct target]]} {
     set channel [sctval [sct parent]/[sct target]/channel]
     set chnode [hgetpropval [sct parent] channel_$channel]
     hsetprop [sct parent] getsample "hvali $chnode"
     hsetprop [sct parent] getcmd "hvali $chnode"
  }
   sct update [sct target]
}

proc trun::halt {} {
  sct status posfault
  return idle
}

proc trun::logcmd {} {
  sct update [sct target]
  switch -- [sct target] {
    clear {
      hupdate [sct]/n 0
      hupdate [sct]/mean 0
      hupdate [sct]/m2 0
      hupdate [sct]/stddev 0
      hsetprop [sct parent] statistics 1
    }
    getmean {
      if {[hvali [sct]/n] == 0} {
        set mean [hgetpropval [sct parent] tsample]
      } else {
        set mean [hvali [sct]/mean]
      }
      sct print "[lindex [sct sicscommand] 0] mean = $mean stddev = [hvali [sct]/stddev]"
    }
  }
}

proc trun::setmaintarget {path value} {
  if {[sct] eq $path/[silent 0 sctval $path/mainloop]} {
    hsetprop $path target $value
  }
}