namespace eval wiggle {
}

proc stdConfig::wiggle {} {
  controller syncedprot 10
  
  obj wiggle -drive out
  prop checklimits wiggle::checklimits
  prop halt wiggle::halt
  prop write wiggle::write
  prop status idle
  prop read stdConfig::complete
  kids "wiggle settings" {
    node period par 2

    node eperiod upd
    prop label "effective period"

    node amp par 0.001
    prop label amplitude

    node n par 10

    node shape par 6
    prop help "4 for saw tooth, 6 for sine-like"

    node maxramp par 0.5

  }
}

proc wiggle::checklimits {} {
  return 0
}

proc wiggle::waitext {} {
  if {[result mf external] == 2} {
    return 0
  }
  if {[result mf external] != 1} {
    mf external 1
  }
  return 1
}

proc wiggle::write {} {
  sct status run

  # wiggle around target [Tesla] with amplitude +- /wiggle/amp [Tesla]
  # minimum period 2 sec
  # number of times /wiggle/n
  
  sct cnt 0

  sct oldramp [result mf ramp]
  set amp [hvali [sct]/amp]
  set period [hvali [sct]/period]
  set maxr [hvali [sct]/maxramp]
  set shape [hvali [sct]/shape]

  if {$period < 2} {
    set period 2
  }
  set ramp [expr $amp * $shape * 60.0 / $period]
  if {$ramp > $maxr} {
    set period [expr $period * $ramp / $maxr]
    set ramp $maxr
  }
#  clientput "ramp $ramp period $period"
  hupdate [sct]/eperiod $period

  mf ramp $ramp
  mf persmode 0
  set mf1 [expr [sct target] + $amp]
  run mf $mf1
  clientput "run mf to $mf1"

  [sct controller] poll [sct] [expr $period * 0.5] progress wiggle::saw
  return idle
}

proc wiggle::saw {} {
  if {[sct status] ne "run"} {
    return unpoll
  }
  if {[wiggle::waitext]} {
    return idle
  }
  set amp [hvali [sct]/amp]
  set cnt [sct cnt]
  sct cnt [expr $cnt + 1]
  set maxcnt [expr 2 * [hvali [sct]/n]]
#  clientput "cnt$cnt"
  if {$cnt >= $maxcnt} {
    mf external 3
    mf ramp [sct oldramp]
    run mf [sct target]
    sct status idle
    return unpoll
  }
  if {$cnt == 0} {
    mf send A1
    return idle
  }
  if {$cnt % 2} {
    set f [expr [sct target] - $amp]
  } else {
    set f [expr [sct target] + $amp]
  }
clientput "mf send J[format %.5f $f]"
  mf send J[format %.5f $f]
  return idle
}

proc wiggle::halt {} {
  mf external 3
  mf ramp [sct oldramp]
  run mf [sct target]
  sct status idle
  return idle
}