"""
Parameters:
args = [prefix ]
"""

import  ch.psi.pshell.epics.ControlledVariable as ControlledVariable

latency = 0.05
#prefix = args[0]
#start = caget("PSHELL:scanstart")
#stop = caget("PSHELL:scanstop")
#step = caget("PSHELL:scanstep")

prefix = "MINSB03-RSYS"
start = -179.0
stop = 180.0
step = 10.0

print "Prefix = ", prefix
print "Start = ", start
print "Stop = ", stop
print "Step = ", step

#rf_phase_setpoint = Channel(prefix + ":SET-VSUM-PHASE")
#rf_phase_readback = Channel(prefix + ":GET-VSUM-PHASE")
#rf_ampl_readback = Channel(prefix + ":GET-VSUM-AMPLT")
#rf_phase_setpoint =  create_channel_device(prefix + ":SET-VSUM-PHASE", type = 'd', deviceName='Phase Setpoint')
#rf_phase_readback =  create_channel_device(prefix + ":GET-VSUM-PHASE", type = 'd', deviceName='Phase Readback')

rf_phase_var =  ControlledVariable("Phase", prefix + ":SET-VSUM-PHASE", prefix + ":GET-VSUM-PHASE")
(rf_phase_var.config.minValue, rf_phase_var.config.maxValue)= (-180.0, 180.0)
rf_phase_var.config.resolution = 0.001
rf_phase_var.initialize()
rf_ampl_readback =  create_channel_device(prefix + ":GET-VSUM-AMPLT", type = 'd', deviceName='Amplitude Readback')

#add_device (rf_phase_var, True)
#add_device (rf_ampl_readback, True)


"""
scan_pos.set([])
scan_val.set([])
def after(rec):
    scan_val.append(rec.values[0])
    scan_pos.append(rec.positions[0])
"""
#TODO: this is workaround to CAS not supporting dynamic arrays
def after(rec):
   global arrpos,arrval
   #print rec.index
   arrval =  scan_val.take().tolist()
   arrpos =  scan_pos.take().tolist()
   arrval[rec.index] = rec.values[0]
   arrpos[rec.index] = rec.positions[0]
   arrval[rec.index:] = [rec.values[0]] * (len(arrval) -rec.index)
   arrpos[rec.index:] = [rec.positions[0]] * (len(arrpos) -rec.index)  
   scan_val.set(arrval)
   scan_pos.set(arrpos)

try:
    #r = lscan(pos, [rf_phase_readback, rf_ampl_readback, sim], start, stop, step , latency=0.2, after_read = after)        
    #r = lscan(rf_phase_setpoint, [rf_phase_readback, rf_ampl_readback, sim], start, stop, step , latency=0.5, after_read = after)
    sim.step = step
    r = lscan(rf_phase_var, [rf_ampl_readback, sim], start, stop, step , latency=latency, after_read = after)
    #To sample a0

    f = r.getReadable(1) # r.getReadable(2)
    f_ampl = r.getReadable(0) # r.getReadable(1)
    xdata = r.getPositions(0) # r.getReadable(0)

    #plot(r.getReadable(1), xdata = r.getPositions(0), title = "data")
    phase_fit_max = None
    try:
        (amplitude, angular_frequency, phase, phase_fit_max) = hfit(f , xdata = xdata)
    except:
        raise Exception("Fit failure")        

    start,end = min(xdata), max(xdata)
    if not (start <=phase_fit_max <=end):             
        raise Exception("Fit maximum outside scan range")

    rf_phase_var.write(phase_fit_max)
    time.sleep(latency)    
    a0 = rf_ampl_readback.read()
    power = 1.0 #TODO        
finally:
    #rf_phase_setpoint.close()   
    #rf_phase_readback.close()    
    rf_phase_var.close()
    rf_ampl_readback.close()



print ("------------------------------------")
print ("Valid fit")



energy_gain = amplitude
phase_offset = - phase_fit_max
amplitude_scale = energy_gain / a0
power_scale = power / math.pow(a0,2)

#caput(prefix + ":SET-VSUM-PHASE-OFFSET", phase_offset)
#caput(prefix + ":SET-VSUM-AMPLT-SCALE", amplitude_scale)
#caput(prefix + ":SET-VOLT-POWER-SCALE", power_scale)


#set_return(r.print())
set_return("Energy Gain: " + str(energy_gain) + "\nPhase Offset: " + str(phase_offset) +  "\nAmplitude Scale: " + str(amplitude_scale) + "\nPower Scale: "+ str(power_scale))