# software pi loop
# new version (old version has strange parameters)

namespace eval piloop {
}

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

proc stdConfig::piloop args {
  variable name
  variable path
  variable node
  
  scanargs $args var -invar -outvar -outmin 0 -outmax 1 -prop 1 -int 10 \
    -maxdelta 60 -precision 0.001 -inpfunc 0 -outfunc 0 -title $name \
    -custinp log -custout log -span 0 -deadband 0 \
    -outlinear 1e-6 -inplinear 1e-3 -ramptime 0 -smoothtime 0 -ramptol 0.05

# custinp, custout:
#   custom input / output function
#   log: allow choice between linear and logarithmic
#   none: only linear
#   else: a custom function. the last arguments are either
#      <value>                  forward conversion
#      inv <value>              inverted conversion

  variable hostport none
  controller dumprot

  if {$span == 0} {
    if {$custout eq "none"} {
      set span [expr $outmax - $outmin]
    }
  }

  obj loop -drive out
  prop lastinput 0
  prop label setpoint
  prop custinp $custinp
  prop custout $custout
  prop span [expr double($span)]

  kids "$title control parameters" {
    node mode -int out
    default 0
    prop enum off,on
    prop check piloop::checkmode
    prop write stdSct::completeUpdate

    node reg upd
    prop geterror "off"
    
    node output out
    default $outmin
    prop check piloop::checkoutput
    prop write stdSct::completeUpdate

    node ramptime par $ramptime
    prop help {time for ramping [sec] deltax = 1 or a factor 10}

    node ramptol par $ramptol
    prop help {log of max. deviation when ramping}

    node smoothtime par $smoothtime
    prop help {time for smoothing ramp near setpoint}

    node invar -text out
    default 0
    prop check piloop::setinvar
    prop write stdSct::completeUpdate
    prop visible false
    
    node outvar -text par $outvar
    prop visible false
    
    node prop par $prop
    prop help {smaller means more sensitive. a change of 'prop' on input\
               -> a change of 100 % or a factor 10 on output}

    node int par $int
    prop help "integration time (sec)"

    node outmin par $outmin
    prop help "output for maximal decrease of input var."

    node outmax par $outmax
    prop help "output for maximal increase of input var."
    
    if {$custinp eq "log"} {
      node inpfunction par -int $inpfunc
      prop label "input function"
      prop enum linear,logarithmic

      node inplinear par $inplinear
      prop help "function gets linear below about this value"
    }

    if {$custout eq "log"} {
      node outfunction par -int $outfunc
      prop label "output function"
      prop enum linear,exponential

      node outlinear par $outlinear
      prop help "function gets linear below about this value"
    }

    node precision par $precision
    prop label "output precision"
    
    node deadband par $deadband

    node maxdelta par $maxdelta
  }
  stdConfig::tdrive "$name control parameters" self piloop::settarget
#  hset $path/setcmd piloop::settarget
  hset $path/invar $invar
}

proc piloop::checkmode {} {
  if {[sct target] == 0} {
    set outmin [hvali [sct parent]/outmin]
    set effoutput [silent $outmin eval "result [hvali [sct parent]/outvar]"]
    if {$effoutput > $outmin} {
      hset [sct parent]/output $outmin
    }
    updateerror [sct parent]/reg off
  } elseif {[hvali [sct]] == 0} {
    hupdate [sct parent]/reg [hvali [sct parent]]
    set outvar_start [silent "" sct @outvar_start]
    if {$outvar_start ne ""} {
      eval $outvar_start
    }
  }
}

proc piloop::updateinput {value} {

  set maxdelta [hvali [sct]/maxdelta]
  set now [DoubleTime]
  if {[sct lastinput] == 0} {
    set deltat 1
  } else {
    set deltat [expr $now - [sct lastinput]]
    if {$deltat > $maxdelta} {
      set deltat $maxdelta
    }
  }
  set timestep [silent 1 sct timestep]
  if {$deltat < $timestep * 0.9 && $timestep > 0.01} {
    set timestep [expr $timestep * 0.9]
  } else {
    set timestep $deltat
  }
  sct timestep $timestep
  sct lastinput $now
  if {[hvali [sct]/mode] == 0} {
    return idle
  }

  set prop [hvali [sct]/prop]
  set int [hvali [sct]/int]
  set outmin [hvali [sct]/outmin]
  set outmax [hvali [sct]/outmax]
  set precision [hvali [sct]/precision]
  set ramptime [hvali [sct]/ramptime]
  set smoothtime [hvali [sct]/smoothtime]
  set reg [hvali [sct]/reg]
  set input $value
  set soll [hvali [sct]/set]
  set deadband [hvali [sct]/deadband]
  if {abs($soll - $input) <= $deadband} {
    return idle
  }

  set effoutput [eval "result [hvali [sct]/outvar]"]
  set output [hvali [sct]/output]
  set span [sct span]
  if {[sct custout] eq "log"} {
    set outlinear [hvali [sct]/outlinear]
    set outfunction [hvali [sct]/outfunction]
    if {$outfunction == 1} {
      set output [expr log10($output + $outlinear)]
      set effoutput [expr log10($effoutput + $outlinear)]
      if {$span == 0} {
        set span 1.0
      }
    } else {
    }
  } elseif {[sct custout] ne "none"} {
    set output [eval [sct custout] inv $output]
    set effoutput [eval [sct custout] inv $effoutput]
  }
  set lastdif [silent 0 sct lastdif]
  if {$span == 0} {
    set span [expr double($outmax - $outmin)]
  }

  set otol [silent $precision sct otol]
  if {$output > $effoutput + $otol} {
#clientput "$otol [expr $output - $effoutput + $otol]"
    set output [expr $effoutput + $otol]
  } elseif {$output < $effoutput - $otol} {
#clientput "$otol [expr $output - ($effoutput - $otol)]"
    set output [expr $effoutput - $otol]
  }

  if {[sct custinp] eq "log"} {
    set inplinear [hvali [sct]/inplinear]
    set inpfunction [hvali [sct]/inpfunction]
    if {$inpfunction == 1} {
      set soll [expr log10($soll + $inplinear)]
      set input [expr log10($input + $inplinear)]
      set reg [expr log10($reg + $inplinear)]
    }
  } elseif {[sct custinp] ne "none"} {
    set soll [eval [sct custinp] $soll]
    set input [eval [sct custinp] $input]
    set reg [eval [sct custinp] $reg]

  }

  if {$ramptime == 0} {
    set reg $soll
  } else {
    set step [expr $deltat / double($ramptime)]
    set dist [expr abs($reg - $soll)]
    if {$dist * $ramptime < $smoothtime } {
      set step [expr $step * sqrt($dist * $ramptime / $smoothtime)]
    }
    set tol [hvali [sct]/ramptol]
    if {$tol * $ramptime < $timestep} {
      set tol [expr $timestep/double($ramptime)]
    }
    if {$soll > $input} {
      set newreg [expr $reg + $step]
      if {$newreg > $input + $tol} {
        set newreg [expr $input + $tol]
      }
      if {$newreg > $soll} {
        set reg $soll
      } elseif {$newreg > $reg} {
        set reg $newreg
      }
    } else {
      set newreg [expr $reg - $step]
      if {$newreg < $input - $tol} {
        set newreg [expr $input - $tol]
      }
      if {$newreg < $soll} {
        set reg $soll
      } elseif {$newreg < $reg} {
        set reg $newreg
      }
    }
  }
  set dif [expr double($reg - $input)]
  set l [llength $lastdif]
  set difdif [expr ($dif - [lindex $lastdif 0]) / $l]
  lappend lastdif $dif
  sct lastdif [lrange $lastdif end-5 end]
  
  set do [expr $span/double($prop) * ($difdif + $deltat * $dif / double($int))]
  set output [expr $output + $do]
#clientput [format "out %12.5g dd %12.5g dtd %12.5g  s/p %12.5g" $output $difdif [expr $deltat * $dif / double($int)] [expr $span/double($prop)]]

  sct otol [expr 2 * abs($do) + $precision]

  if {[sct custinp] eq "log"} {
    if {$inpfunction == 1} {
      set reg [expr pow(10, $reg) - $inplinear]
    }
  } elseif {[sct custinp] ne "none"} {
    set reg [eval [sct custinp] inv $reg]
  }
  updateval [sct]/reg $reg
  if {[sct custout] eq "log"} {
    if {$outfunction == 1} {
      set output [expr pow(10, $output) - $outlinear]
#clientput $output
    }
  } elseif {[sct custout] ne "none"} {
    set output [eval [sct custout] $output]
  }
  if {$outmax > $outmin} {
    if {$output > $outmax} {
      set output $outmax
    } elseif {$output < $outmin} {
      set output $outmin
    }
  } else {
    if {$output > $outmin} {
      set output $outmin
    } elseif {$output < $outmax} {
      set output $outmax
    }
  }
  if {[catch {hset [sct]/output $output} msg]} {
    hsetprop [sct]/reg geterror $msg
  }
  return idle
}

# updatescript on the remote input node
proc piloop::updateinput1 {value} {
  set loopobj [sct loopobj]
  if {[silent 0 sct geterror] eq "0"} {
    updateval $loopobj $value
  }
  return
}

proc piloop::setinvar {} {
  catch {
    [sct controllerName] killupdatescript [hval [sct]] piloop::updateinput1
  }
  [sct controllerName] updatescript [sct target] piloop::updateinput1
  [sct controllerName] updatescript [sct parent] piloop::updateinput
  hsetprop [sct target] loopobj [sct parent]
  return
}

proc piloop::settarget {target} {
#  clientput "settarget $target == [silent 0 sct @offvalue]"
  if {$target == [silent 0 sct @offvalue]} {
    hset [sct]/mode 0
  } else {
    hset [sct]/mode 1
  }
  sct print "[sct]/set = $target"
  return idle
}

proc piloop::updtarget {} {
  sct print "[sct]/set = [sct target]"
  return idle
}

proc piloop::checkoutput {} {
  eval "[hvali [sct parent]/outvar] [sct target]"
}