# furnace control (for now used by FI blue ILL type furnace)
# for Arduino fictrl_11 (not fictrl_12!!)

namespace eval fictrl {
}

if {![namespace exists lsc]} {
  source drivers/lsc.tcl
}

proc stdConfig::fictrl {body} {
  variable name
  variable node
  variable path

  controller std
  prop startcmd "s=1 vers"

  pollperiod 5 5

  array set arguments $body
  # arguments(voltscale) voltage at 100 %
  # arguments(min_resist) switch off when resistivity is lower than this
  # arguments(max_resist) switch off when resistivity is bigger than this

  obj FICTRL -drive wr
  # @switch_on: set by check_set for switching on control
  prop @switch_on 0
  prop @voltscale $arguments(voltscale)
  prop @min_resist $arguments(min_resist)
  prop @max_resist $arguments(max_resist)
  prop @offset_I $arguments(current_offset)
  prop @offset_U $arguments(voltage_offset)
  prop @offset_h 0
  prop @power_supply_off 0

  if {[info proc stdConfig::tdrive] eq ""} {
    namespace eval :: {
      source drivers/trun.tcl
    }
  }
  tdrive $name -log 1
  hsetprop $path/set check fictrl::check_set

  kids "fi control ($name)" {
    foreach ch {1 2 3 4} {
      node t$ch upd

      kids "settings channel $ch" {

        node raw rd
        prop readcmd v$ch
        prop readfmt v$ch=%g
        prop update fictrl::update_v
        prop channel $ch

        node curve out -text
        prop width 32
        prop model 0
        prop check fictrl::curve
        prop write stdSct::completeUpdate

        kids "calibration" {
          hfactory $path/points plain mugger floatvarar 1
        }

        node valid rd
        prop readcmd s$ch
        prop readfmt s$ch=%d
      }
    }

    node tref rd
    prop readcmd t2
    prop update fictrl::update_ref
    # prop readfmt "s$ch=%g"
    prop old 0

    node tout rd
    prop readcmd t1
    prop update fictrl::update_out
    # prop readfmt "s$ch=%g"

    node ctrlmode wr
    prop writecmd s=%d
    prop readcmd s
    prop readfmt s=%d
    prop enum ok,off,illegal_channel,no_sensor,no_waterflow,above_alarm,bad_vacuum,power_supply_off
    prop update fictrl::update_c

    node ramp par 20

    node smooth par 60

    node prop par 0.5

    node int par 0.1

    node powerset out
    default 0
    prop check fictrl::check_power
    prop write stdSct::completeUpdate

    node power upd

    node resist upd
    default 0.025

    node maxpower par 150

    node maxheater wr
    prop writecmd m=%.7g
    prop readcmd m
    prop readfmt m=%g

    node output wr
    prop writecmd o=%.7g
    prop readcmd o
    prop readfmt o=%g

    node ctrlchan wr -int
    prop writecmd c=%d
    prop readcmd c
    prop readfmt c=%d

    node interlock_state rd -int
    prop readcmd is
    prop readfmt is=%d
    prop enum ok=0,no_waterflow=1
    # ,no_vacuum=2,no_water_no_vacuum=3

    node interlock_mask wr -int
    prop readcmd im
    prop readfmt im=%d
    prop writecmd im=%d
    prop enum check_water_flow=0,no_check=1
    # ,check_vacuum_only=2,check_all=3

    node sramp upd

    node slope upd

    node v_htr upd

    node i_htr upd

    node htr rd
    prop readcmd "h U I"
    prop update fictrl::update_power

    node powerprop upd

  }
}

proc fictrl::check_power {} {
  set p [sct target]
  if {$p > 0} {
    set r [hval [sct parent]/resist]
    set s [sct @voltscale]
    set output [format %.2f [expr 100 * sqrt($p * $r) / $s]]
    # clientput "output $output" 
    hset [sct parent]/output $output
  } else {
    # clientput "output 0" 
    hset [sct parent]/output 0
  }
}

proc fictrl::update_power {} {
  array set keyval {h 0 U 0 I 0}
  foreach item [sct result] {
    lassign [split $item =] key val
    set keyval($key) [expr $val + [silent 0 sct @offset_$key]]
  }
  sct update $keyval(h)
  set v $keyval(U)
  set i $keyval(I)
  set pow [expr max(0, $v * $i)]
  set vout [expr $keyval(h) * 0.01 * [sct @voltscale]]
  # set vpercent [expr $v * 100.0 / [sct @voltscale]]
  # difference output voltage % - read voltage %
  if {abs($v - $vout) > 0.5} {
    clientput "v $v vout $vout last [silent "" sct last_v]"
    if {abs($v - [silent 100 sct last_v]) < 0.02} {
      enum_decode [sct parent]/ctrlmode power_supply_off new
      sct @power_supply_off 1
      if {$new != [sctval [sct parent]/ctrlmode]} {
        hset [sct parent]/ctrlmode $new
      }
      return idle
    }
    sct last_v $v
    set pow [expr max(0, $vout * $i)]
  }
  sct @power_supply_off 0
  if {[sctval [sct parent]/ctrlmode]} {
    updateval [sct parent]/power 0
    return idle
  }
  updateval [sct parent]/v_htr $v
  updateval [sct parent]/i_htr $i
  updateval [sct parent]/power $pow
  if {$i > 1 && $v > 0.05} {
    set r [hval [sct parent]/resist]
    set r1 [expr ($v - 0.005) / double($i + 0.1)]
    set r2 [expr ($v + 0.005) / double($i - 0.1)]
    if {$r1 > [sct @max_resist]} {
      clientput "ERROR: resistance $r1 too high"
    }
    if {$r2 < [sct @min_resist]} {
      clientput "ERROR: resistance $r2 too low"
    }
    if {$r < $r1} {
#      set r $r1
      set r [expr $r * 0.9 + $r1 * 0.1]
    } elseif {$r > $r2} {
      set r [expr $r * 0.9 + $r2 * 0.1]
#      set r $r2
    }
    hupdate [sct parent]/resist $r
  }
  return idle
}

proc fictrl::check_set {} {
  sct @switch_on 1
  sct @power_supply_off 0
  hupdate [sct parent]/status ""
  hset [sct parent]/ctrlmode 0
}

proc fictrl::cmds {} {
  set base [sct objectPath]
  sct cmd_p $base
  foreach node [hlist $base] {
    catch {
      set cmd [hgetpropval $base/$node cmd]
      sct cmd_$cmd $base/$node
      lappend query $cmd
    }
  }
  return [join $query " "]
}

proc fictrl::to_kelvin {ch value} {
  if {[hvali [sct objectPath]/t$ch/curve] eq ""} {
    return $value
  }
  set tref [hvali [sct objectPath]/tref]
  set vref [interpolate [hvali [sct objectPath]/t$ch/curve/points] 1 $tref]
  set v [expr $value + $vref]
  return [interpolate [hvali [sct objectPath]/t$ch/curve/points] 0 $v extrapolate]
}

proc fictrl::to_mvolt {ch value} {
  if {[hvali [sct objectPath]/t$ch/curve] eq ""} {
    return $value
  }
  set tref [hvali [sct objectPath]/tref]
  set vref [interpolate [hvali [sct objectPath]/t$ch/curve/points] 1 $tref]
  set vval [interpolate [hvali [sct objectPath]/t$ch/curve/points] 1 $value]
  return [expr $vval - $vref]
}

proc fictrl::ctrl_slope {} {
  if {[sct @switch_on]} {
    if {[hvali [sct parent]/ctrlmode]} {
      hupdate [sct parent]/powerset 0
    }
    set init "s=0 "
  } elseif {[hvali [sct parent]/ctrlmode]} {
    # error state
    return idle
  } else {
    set init ""
  }
  set ch [hvali [sct parent]/ctrlchan]
  if {$ch == 0} {
    return idle
  }

  # set ta [expr [hval [sct parent]/t$ch] + 20]
  set ta 1370
  set valarm [to_mvolt $ch $ta]
  sct @switch_on 0
  sct send "${init}a=$valarm c=$ch dx vx sx"
  return fictrl::ctrl_do
}

proc fictrl::ctrl_do {} {
  array set keyval {dx 0 vx 0 sx 0}
  foreach item [sct result] {
    lassign [split $item =] key val
    set keyval($key) $val
  }
  set dt $keyval(dx)
  if {$dt == 0} {
    # no delta time available (happend on init)
    return idle
  }
  set ch [hvali [sct parent]/ctrlchan]
  # clientput [array get keyval]
  set tist [to_kelvin $ch $keyval(vx)]

  # convert voltage slope to T slope
  set v1 [to_mvolt $ch [expr $tist - 10]]
  set v2 [to_mvolt $ch [expr $tist + 10]]
  set slope [expr $keyval(sx) * 20.0 / ($v2 - $v1)]

  set tg [hval [sct parent]/target]
  set ramp [hval [sct parent]/ramp]
  set out [hval [sct parent]/powerset]
  set prop [hval [sct parent]/prop]
  set int [hval [sct parent]/int]
  updateval [sct parent] $tist
  set sramp [expr abs($tist - $tg) * 60.0 / [hval [sct parent]/smooth]]
  if {$sramp > $ramp} {
    set sramp $ramp
  }
  if {$tg < $tist} {
    set sramp [expr -$sramp]
  }
  hupdate [sct parent]/sramp $sramp
  # filter slope
  set slope [expr [silent $slope hval [sct parent]/slope] * 0.9 + $slope * 0.1]
  hupdate [sct parent]/slope $slope
  set last_dif [silent 0 sct last_dif]
  set dif [expr $sramp - $slope]
  # clientput "$dif slope $slope sramp $sramp"
  sct last_dif $dif
  set out [expr $out + ($dif * $dt / double($int) + $dif - $last_dif) * $prop]
  set maxpower [hval [sct parent]/maxpower]
  set r [hval [sct parent]/resist]
  set s [sct @voltscale]
  hset [sct parent]/maxheater [expr sqrt($maxpower * $r) * 100.0 / $s]
  if {$out < 0} {
    if {$dif > 0} {
      set out 0
    } elseif {$out < -$maxpower*10} {
      set out [expr -$maxpower*10]
    }
  } elseif {$out > $maxpower} {
    if {$dif < 0} {
      set out $maxpower
    } elseif {$out > $maxpower * 11} {
      set out [expr $maxpower * 11]
    }
  }
  # clientput "dif $dif powerset $out"

  hset [sct parent]/powerset [format %.5g $out]
  hupdate [sct parent]/powerprop [expr $prop * $dif]
  return idle
}

proc fictrl::update_ref {} {
  [sct controller] queue [sct] slow fictrl::ctrl_slope
  set v [lindex [split [sct result] =] 1]
  set t [format %.1f [expr $v + 273.15]]
  sct update $t

  return idle
}

proc fictrl::update_out {} {
  set v [lindex [split [sct result] =] 1]
  sct update [format %.1f [expr $v + 273.15]]
  return idle
}

proc fictrl::update_v {} {
  stdSct::update
  updateval [sct parent] [to_kelvin [sct channel] [hvali [sct]]]
  return idle
}

proc fictrl::update_c {} {
  set value [stdSct::scanresult]
  set old_status [hvali [sct objectPath]/status]
  enum_decode [sct] $value _ old_mode
  set new $old_mode
  lassign [split [hvali /pv/sps] ","] pstate
  set pstate 1 ;# HACK: when comm to TPG does not work
  if {$pstate eq ""} {
    set pstate 0
  }
  if {!$pstate} {
    # bad_vacuum
    enum_decode [sct] bad_vacuum num
    sct update $num
    if {$old_mode eq "ok" || $old_mode eq "off"} {
       hset [sct] $num
       set new bad_vacuum
    }
    hsetprop [sct parent]/v_htr geterror powersupply_switched_off
    hsetprop [sct parent]/i_htr geterror powersupply_switched_off
  } else {
    sct update $value
    catch {
      hdelprop [sct parent]/v_htr geterror
      hdelprop [sct parent]/i_htr geterror
    }
  }
  if {$new eq "power_supply_off"} {  # HACK
      set new "ok"
  }
  if {$new ne "ok"} {
    switch $new {
      bad_vacuum {
        if {$pstate} { set new "off after bad_vacuum" }
      }
      no_waterflow {
        if {[hval [sct parent]/interlock_state] == 0} { set new "off after no_waterflow" }
      }
      power_supply_off {
        if {![sct @power_supply_off]} { set new "off after power_supply_off" }
      }
    }
    if {$new ne $old_status} {
      hupdate [sct parent]/powerset 0
      if {[lindex $new end] ne [lindex $old_status end]} {
        clientput "ERROR: $new"
      }
    }
    hupdate [sct objectPath]/status $new
  } elseif {$old_status != ""} {
    hupdate [sct objectPath]/status ""
  }
  return idle
}

proc fictrl::write {} {
  set p [expr [sct target]/double([silent 1 sct fact])]
  sct send "[sct cmd]=$p [fictrl::cmds]"
  #return fictrl::update
  return stdSct::complete
}

proc fictrl::curve {} {
  if {[sct requested] ne "" && [sct requested] ne "raw"} {
    lsc::read_curve
  }
}