# software pi loop
namespace eval loop {
}

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

proc stdConfig::loop args {
  variable name
  variable path
  variable node
  
  scanargs $args var -invar -outvar -outmin 0 -outmax 1 -prop 1 -int 1 -maxdelta 60 -precision 0.001 -mindelta 0

  variable hostport none
  controller syncedprot

  obj loop -drive out
  prop lastinput 0
  kids "$name control parameters" {
    node mode -int out
    default 0
    prop enum off,on,manual
    prop check loop::checkmode
    prop write stdSct::completeUpdate

    node output out
    default $outmin
    prop check loop::checkoutput
    prop write stdSct::completeUpdate

    node invar -text out
    default 0
    prop check loop::setinvar
    prop write stdSct::completeUpdate
    prop visible false

    node outvar -text par $outvar
    prop visible false
    
    node pollinput rd
    prop read stdSct::complete

    node prop par $prop
    prop help "the bigger, the less sensible"

    node int par $int
    node outmin par $outmin
    node outmax par $outmax
    
    node precision par $precision
    prop label "output precision"
    
    node maxdelta par $maxdelta

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

proc loop::checkmode {} {
  if {[sct target] == 0} {
    set output [hvali [sct parent]/outmin]
    hset [sct parent]/output $output
  }
}

proc loop::updateinput {value} {
  set prop [hvali [sct]/prop]
  set int [hvali [sct]/int]
  set outmin [hvali [sct]/outmin]
  set outmax [hvali [sct]/outmax]
  set span [expr double($outmax - $outmin)]
  set maxdelta [hvali [sct]/maxdelta]
  set mindelta [hvali [sct]/mindelta]
  set precision [hvali [sct]/precision]
  set input $value

  set effoutput [eval "result [hvali [sct]/outvar]"]
  if {[hvali [sct]/mode] == 2} {
    return idle
  }
  if {[hvali [sct]/mode] == 0} {
    # to be put into write script of ../mode
    if {$effoutput > $outmin} {
      eval "[hvali [sct]/outvar] $outmin"
    }
    return idle
  }
  set output [hvali [sct]/output]
  if {abs($effoutput - $output) > $precision} {
    clientput "$output -> $effoutput"
    set output $effoutput
  }
  set now [clock seconds]
  if {[sct lastinput] == 0} {
    set deltat 1
  } else {
    set deltat [expr $now - [sct lastinput]]
    if {$deltat < $mindelta} {
      return idle
    }
    if {$deltat > $maxdelta} {
      set deltat $maxdelta
    }
  }
  sct lastinput $now
  set lastdif [silent 0 sct lastdif]

  set dif [expr double([hvali [sct]/set] - $input)]
  set output [expr $output + $span/$prop \
               * (($dif - $lastdif) * $deltat + $dif / $int)]
  sct lastdif $dif

  if {$outmax > $outmin} {
    if {$output > $outmax} {
      set output $outmax
    } elseif {$output < $outmin} {
      set output $outmin
    }
  } else {
    if {$output < $outmax} {
      set output $outmax
    } elseif {$output > $outmin} {
      set output $outmin
    }
  }
  hset [sct]/output $output
  return idle
}

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

proc loop::setinvar {} {
  if {[silent none hgetprop [sct target] sicscommand] eq "none"} {
    set t [sct parent]/pollinput
    sct target [sct parent]/pollinput
    # clientput "Tp $t"
  } else {
    set t [sct target]
    # clientput "Tt $t"
  }
  [sct controllerName] updatescript [sct parent] loop::updateinput
  [sct controllerName] updatescript $t loop::updateinput1
  hsetprop $t loopobj [sct parent]
  hsetprop $t read loop::pollinput
  return
}

proc loop::pollinput {} {
  catch {sct update [result [hval [sct parent]/invar]]}
  return idle
}

proc loop::settarget {target} {
  updateval [sct]/mode 1
  sct print "[sct]/set = $target"
  return idle
}

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

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