#! /usr/bin/env python

import sys, os, os.path
import time

import numpy as np

import dice.testbench.motor as dtm
import dice.testbench.utils.grid as dtug
import dice.testbench.multimeter.keithley6514 as multi

# --------- Parse command line arguments ------------------

def usage():
    print >>sys.stderr, \
        'usage: beam-map led=<led> current=<current> z=<z>'
    print >>sys.stderr, \
        "                    [xmin=<xmin> xmax=<xmax> ymin=<ymin> ymax=<ymax>]"
    print >>sys.stderr, \
        "                    [dx=<dx> dy=<dy>]"
    

# print sys.argv

if len(sys.argv) < 1:
    usage()
    sys.exit(1)


phd = "NIST"
phd_port = "/dev/ttyS3"
led = 19
current = 10000. # ADU
z = -1590. # mm

# xmin =     0. # mm
# xmax =   300. # mm
# ymin =     0. # mm
# ymax =   300. # mm
# zmin = -1593. # mm
# zmax =     0. # mm

xmin = 135. # mm
xmax = 165. # mm
ymin = 133. # mm
ymax = 163. # mm

# dx = 10. # mm
# dy = 10. # mm
dx = 1. # mm
dy = 1. # mm

for arg in sys.argv[1:]:
    if '=' not in arg:
        print >>sys.stderr, "error: invalid argument [%s]" % arg
        usage()
        sys.exit(2)
    eqparts = arg.split('=')
    if len(eqparts) != 2:
        print >>sys.stderr, "error: invalid argument format [%s]" % arg
        usage()
        sys.exit(3)
    
    param = eqparts[0].lower()
    if param not in ['led', 'z', 'current',
                     'xmin', 'xmax', 'ymin', 'ymax',
                     'dx', 'dy'
                     ]:
        print >>sys.stderr, "error: invalid parameter [%s]" % arg
        usage()
        sys.exit(4)

    exec(arg)
    # print arg, param, eval(param)

# --------- Init testbench motors -------------------------

B = dtm.Bench()
B.open()
B.setup()

# ---------------------------------------------------------

# --------- Init multimeter Keithley 6514 -----------------

K = multi.Multimeter(port = phd_port)

K.open()
K.reset()
# K.write("CURR:RANG 2e-7")
K.write("CURR:RANG 2e-10")
K.write("FUNC 'CURR:DC'")
K.write("SYST:ZCH OFF")

# ---------------------------------------------------------

# xy_mm = 12800.0 # steps / mm
# z_mm  =  2560.0 # steps / mm

B.sensor_move_absolute('Z', z, unit = "mm")

filename = ("PENCIL-BEAM-%02d-on-map-I=%06dADU-z=%06dmm-dxdy=%fmm%fmm-%s-%f.data" %
            ( led, current, z, dx, dy, phd, time.time() ) )
f = open(filename, "w")
print >>f, "# time   x(mm)   y(mm)    I(A)"

ipos = -1
xref = xmin + (xmax-xmin)/2
yref = ymin + (ymax-ymin)/2

xdark = xref
ydark = 300

xlast,ylast = None, None
for (x,y) in dtug.Grid(xmin, xmax, dx,
                       ymin, ymax, dy,
                       order = "faster",
                       reference = {'x':xref,'y':yref, 'period':20} ):
    print "LED map: GOTO to ", x, y
    if x != xlast: B.sensor_move_absolute('X', x, unit = "mm")
    if y != ylast: B.sensor_move_absolute('Y', y, unit = "mm")
    xlast = x
    ylast = y

    # extra delay for capacity discharge ?
    time.sleep(20)

    T = time.time()
    K.write("READ?")
    data_str = K.read()
    print "Received [%s]" % data_str
    parts = data_str.split(',')
    if (len(parts) != 3):
        print >>sys.stderr, "No data. stop."
        break

    flux = float(parts[0])
    print T, x, y, flux
    print >>f, T, x, y, flux
    f.flush()

    ipos+=1

    if (ipos==20):
         print "LED map: GOTO to ref position", xdark, ydark
         B.sensor_move_absolute('X', xdark, unit = "mm")
         B.sensor_move_absolute('Y', ydark, unit = "mm")
         xlast = xdark
         ylast = ydark
         time.sleep(20)
         T = time.time()
         K.write("READ?")
         data_str = K.read()
         print "Received [%s]" % data_str
         parts = data_str.split(',')
         if (len(parts) != 3):
             print >>sys.stderr, "No data. stop."
             break
         flux = float(parts[0])
         print T, xdark, ydark, flux
         print >>f, T, xdark, ydark, flux
         f.flush()
         ipos=0

f.close()

# ---------------------------------------------------------

K.close()
B.close()

# ---------------------------------------------------------