namespace eval lockin {} {
}

proc stdConfig::lockin {} {
  controller std "\r" 5 "*?#;"
  prop readfmt "%f"
  prop startcmd "ID"
  
# remark: the period of the amplitude reading can be adjusted with the period parameter
  pollperiod 1 5

  obj Lockin7265_7270 wr -drive

#to be done from sctdriveobj
  prop sicscommand la

  prop read lockin::read
  prop write lockin::writeFreq
  prop updatexy lockin::updatexy
  prop changef lockin::changef
  prop checklimits lockin::checklimits
  prop halt lockin::halt
  prop label "frequency"
  prop fmt "%.7g"
  prop start lockin::start
  
  kids "lockin settings" {
    node remote -int out
    default 1
    prop check lockin::remote
    prop write stdSct::complete
    prop enum 1

    node x upd
    node y upd
    node step par 0.2

    node period out   
    default 1
    prop check lockin::setPeriod
    prop write stdSct::complete

    node fres upd

    node fhw upd

    node amp wr
    prop writecmd "OA.%g"
    prop readcmd "OA."
    prop read lockin::readPar
    prop update lockin::updatePar

    node refphase wr
    prop writecmd "REFP.%g"
    prop readcmd "REFP."
    prop read lockin::readPar
    prop update lockin::updatePar

    node DAC1 wr
    prop writecmd "DAC.1 %g"
    prop readcmd "DAC.1"
    prop read lockin::readPar
    prop update lockin::updatePar

 #   node sens wr
 #   prop label "sensitivity"
 #   prop read lockin::sensread
 #   prop write lockin::senswrite 
    
 #   node mod -int in
 #   prop readcmd "IMODE"
 #   prop enum 1
    
    node fileno par 1

    node filename -text par "lockin*.out"
    prop help "the name should contain '*' which is replaced by fileno"
    prop width 32

    node script -text par ""
    prop help "Use lockin::wfile to write to a file"
    prop width 32

  }

  return "Lockin 7265/7270"
}

proc lockin::wfile {filename f x y} {
  set file [open $filename a]
  puts $file "$f	$x	$y"
  close $file
  return
}

proc lockin::collect {cscript filename f x y} {
  variable collect_data[sct objectPath]
  variable collect_script[sct objectPath] $cscript
  lappend collect_data[sct objectPath] $f $x $y
  return
}

proc lockin::dumpdata {filename data} {
  clientput "--- start data dump ---"
  foreach {f x y} $data {
    clientput "$f $x $y"
  }
  clientput "--- end data dump ---"
}

proc lockin::nextfile {} {
  set no [hval [sct]/fileno]
  set filename [result exe batchpath]/[string map "* $no" [hval [sct]/filename]]
  while {[file exists $filename]} {
    incr no
    set filename [result exe batchpath]/[string map "* $no" [hval [sct]/filename]]
  }
  sct filename $filename
  hupdate [sct]/fileno $no
  variable collect_data[sct] [list ]
  return idle
}

proc lockin::remote {} {
  if {[sct target]} {
    [sct controller] reconnect
  } else {
    [sct controller] disconnect
  }
  sct update [sct target]
  return idle
}

proc lockin::dummy {f x y} {
#  clientput "FXY $f $x $y"
}

proc lockin::setPeriod {} {
  [sct controllerName] poll [sct objectPath] [sct target]
  sct update [sct target]
  return idle
}

proc lockin::doscript {f x y} {
  if {[silent idle sct status] eq "run"} {
    set script [hval [sct]/script]
    if {$script ne ""} {
      eval $script [sct filename] $f $x $y
    }
  }
}

proc lockin::endrun {} {
  variable collect_script[sct]
  variable collect_data[sct]
  sct status idle
  if {[info exists collect_script[sct]]} {
    set cmd [set collect_script[sct]]
    lappend cmd [sct filename] [set collect_data[sct]]
    eval $cmd
    unset collect_script[sct]
  }
}

proc lockin::writeFreq {} {
  if {[silent 0 sct writestatus] eq "start"} {
    if {[hvali [sct]/step] != 0.0} {
#     initiated by a run or drive
      lockin::nextfile
      sct print "run [sct objectName] from [sct freq] to [sct target]"
      sct phase 0
      sct read lockin::progress
      sct status run
      #sct status idle
      return idle
    }
    lockin::endrun
  }
  sct freq [sct target]
  set try 0
  sct send "OF.[sct target]"
  return stdSct::complete
}

proc lockin::getResult args {
  set res [split [sct result]]
  if {[ lindex [sct idn] 1 ] eq "7265"} {
    set nlines [expr [llength $args] * 2 + 3]
    if {[llength $res] != $nlines
      || ![string equal * [lindex $res end]]
      || ![string equal [sct send] [lindex $res 0]]} {
      sct try [expr [sct try] + 1]
      if {[sct try] > 3} {
        error "too many tries"
      }
      return 0
    }
    if {[sct try] > 0} {
      clientput "lockin: tried [sct try] times after communication error"
    }
    set i 2
    foreach a $args {
#clientput "C $a [lindex $res $i]"
      upvar $a var
      set var [lindex $res $i]
      incr i 2
    }
  } else {
    upvar [lindex $args 0] var
    set var [lindex  $res 0]
  }
  return 1
}

proc lockin::checklimits {} {
# if sct requested exists: a write is pending, take this one for starting
# else: take actual value for starting
  sct freq [silent [hvali [sct]] sct requested]
  if {[sct target] <= 0} {
    error "illegal frequency: [sct target]"
  }
}

proc lockin::halt {} {
  hupdate [sct] [hvali [sct]]
  hsetprop [sct] target [hvali [sct]]
  clientput "HALT [sct]"
  sct status idle
  return idle
}

proc lockin::nix args {
  return idle
}

proc lockin::read {} {
  sct try 0
  if {[ lindex [sct idn] 1 ] eq "7265"} {
    sct send "OF.;XY."
    return lockin::update3
  } else {
    sct send "OF."
    sct updatexy "lockin::updatexy 1"
    sct changef lockin::nix
    return lockin::updatef
  } 
  
}

proc lockin::progress {} {
  if {![string equal run [sct status]]} {
    sct read lockin::read
    return idle
  }
  if {[sct phase] == 0} {
    sct phase 1
    return idle
  }
  set step  [expr abs([hvali [sct]/step])]
  set target [sct target]
  set freq  [sct freq]
  set diff [expr $target - $freq]
  if {abs($freq - $target) < $step * 0.01} {
    if {[sct phase] != 2} {
      sct phase 2
    } else {
      sct read lockin::read
      lockin::endrun
      return idle
    }
  }
  if {$freq == 0 || $step == 0} {
    sct read lockin::read
    lockin::endrun
    return idle
  }
  if {abs($freq - $target) < $step} {
    set freq $target
  } elseif {$freq < $target} {
    set freq [expr $freq + $step]
  } else {
    set freq [expr $freq - $step]
  }
  set freq [format "%.9g" $freq]
  sct freq $freq
  sct try 0
  if {[ lindex [sct idn] 1 ] eq "7265"} {
    sct send "OF.;XY.;OF.$freq"
    return "lockin::update3 1"
  } else {
    sct updatexy "lockin::updatexy 1"
    sct changef "lockin::changef $freq"
#    clientput "progress $freq"
    sct send "OF."
    return lockin::updatef
  } 
}

proc lockin::update3 {{doscript 0}} {
  if {![lockin::getResult freq xy] } {
    sct send [sct send]
    return "lockin::update3 $doscript"
  }
  sct update $freq
  scan $xy "%f,%f" x y
  hupdate [sct]/x $x
  hupdate [sct]/y $y
  lockin::doscript $freq $x $y
  return idle
}

proc lockin::updatef {} {
  if {![lockin::getResult freq] } {
    sct send [sct send]
    return "lockin::updatef"
  }
  sct update $freq
  sct send "XY."
#  clientput "updatef  $freq"
  return updatexy
}

proc lockin::updatexy {{doscript 0}} {
  if {![lockin::getResult xy] } {
    sct send [sct send]
    return "lockin::updatexy $doscript"
  }
  scan $xy "%f,%f" x y
  hupdate [sct]/x $x
  hupdate [sct]/y $y
  lockin::doscript [hvali [sct]] $x $y
  return changef
}

proc lockin::changef {freq} {
  sct send "OF.$freq"
  return stdSct::complete
}

proc lockin::updatePar {} {
  if {![lockin::getResult par] } {
    sct send [sct send]
    return "lockin::updatePar"
  }
  sct update $par
  return idle
}

proc lockin::readPar {} {
  sct try 0
  return [stdSct::read]
}


proc lockin::senswrite {} {
  if {[hval [sct]/mod] == 0} {
    set dictfield V
  } elseif {[hval [sct]/mod] == 1} {
    set dictfield I
  } else {
    set dictfield Ilow
  }  
 
  return lockin::sensread
}

proc lockin::sensread {} {

  return idle
} 


proc lockin::peak {filename data} {

  set n [expr [llength $data] / 3]
  if {$n < 5} {
    clientput "can not make peak with only $n points"
    return
  }
  set m 0
  set sumx1 0
  set sumy1 0
  set sumf1 0
  set sumx2 0
  set sumy2 0
  set sumf2 0
  set n4 [expr $n / 10 + 1]
  foreach {f x y} $data {
    if {$m < $n4} {
      set sumx1 [expr $sumx1 + $x]
      set sumy1 [expr $sumy1 + $y]
      set sumf1 [expr $sumf1 + $f]
    }
    if {$m >= $n - $n4} {
      set sumx2 [expr $sumx2 + $x]
      set sumy2 [expr $sumy2 + $y]
      set sumf2 [expr $sumf2 + $f]
    }
    incr m
  }
  set ax1 [expr $sumx1 / $n4]
  set ay1 [expr $sumy1 / $n4]
  set af1 [expr $sumf1 / $n4]
  set ax2 [expr $sumx2 / $n4]
  set ay2 [expr $sumy2 / $n4]
  set af2 [expr $sumf2 / $n4]

  set file [open $filename w]
  # determining raw center
  set maxf 0
  set maxa 0
  foreach {f x y} $data {
    set bx [expr $ax1 + ($f - $af1) / double($af2 - $af1) * ($ax2 - $ax1)]
    set by [expr $ay1 + ($f - $af1) / double($af2 - $af1) * ($ay2 - $ay1)]
    set xx [expr $x-$bx]
    set yy [expr $y-$by]
    set amp  [expr pow($xx, 2) + pow($yy, 2)]
    if {$amp > $maxa} {
      set maxa $amp
      set maxf $f
    }
    puts $file "$f $xx $yy $amp"
  }

  # determining raw width
  set f1 0
  set f2 0
  set f0 0
  set a0 0
  set half [expr $maxa * 0.5]
  foreach {f x y} $data {
    set bx [expr $ax1 + ($f - $af1) / double($af2 - $af1) * ($ax2 - $ax1)]
    set by [expr $ay1 + ($f - $af1) / double($af2 - $af1) * ($ay2 - $ay1)]
    set xx [expr $x-$bx]
    set yy [expr $y-$by]
    set amp  [expr pow($xx, 2) + pow($yy, 2)]
    if {$amp >= $half && $f0 != 0} {
      if {$f1 == 0} {
        set f1 [expr $f0 + ($f- $f0) * ($half - $a0) / ($amp - $a0)]
      }
    } elseif {$f1 != 0 && $f2 == 0} {
      set f2 [expr $f0 + ($f- $f0) * ($half - $a0) / ($amp - $a0)]
    }
    set a0 $amp
    set f0 $f
  }
  set wid [expr $f2 - $f1]

  # calculate center of gravity
  set sumf 0
  set suma 0
  foreach {f x y} $data {
    if {abs($f - $maxf) < $wid * 1.5} {
      set bx [expr $ax1 + ($f - $af1) / double($af2 - $af1) * ($ax2 - $ax1)]
      set by [expr $ay1 + ($f - $af1) / double($af2 - $af1) * ($ay2 - $ay1)]
      set xx [expr $x-$bx]
      set yy [expr $y-$by]
      set amp  [expr pow($xx, 2) + pow($yy, 2)]
      set sumf [expr $sumf + ($f - $maxf) * $amp]
      set suma [expr $suma + $amp]
    }
  }
  set fstep [expr ($f - [lindex $data 0]) / ($n - 1)]
  set fmean [expr $maxf + $sumf / $suma]
  set fwhm [expr $f2 - $f1]
  set iamp [expr $suma * $fstep]
  hupdate [sct]/fres $fmean
  hupdate [sct]/fhw $fwhm
  puts $file "# peak freq,fwhm,amp,iamp = $fmean	$fwhm	$amp	$iamp"
  close $file
}
   
