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

phase =  ControlledVariable("Phase", "SINEG01-RSYS:SET-VSUM-PHASE-SIM", "SINEG01-RSYS:GET-VSUM-PHASE-SIM")
phase.config.minValue   =-45.0
phase.config.maxValue   = 360.0
phase.config.deadband = 0.5


#st = Stream("ICTstream", dispatcher)
#q  = st.addScalar("Charge", "SINEG01-DICT215:B1_CHARGE-SIM", 1, 0)
#st.initialize()
#st.start(True)
q =  Channel("SINEG01-DICT215:B1_CHARGE-SIM", type = 'd', alias='ICT-Q')


if get_exec_pars().source == CommandSource.ui:
    start   = 0.0
    stop    = 360.0
    step    = 5.0
    nb      = 1
    lat     = 0.005
else:
    start = args[0]
    stop  = args[1]
    step =  args[2]
    nb   =  int(args[3])
    lat  =  args[4]
    

class AnalogOutput(RegisterBase):    
    def doRead(self):
        return self.val if hasattr(self, 'val') else 0.0 

    def doWrite(self, val):
        self.val = val

class AnalogInput(ReadonlyRegisterBase):
    def doRead(self):
        time.sleep(0.001)
        self.val = to_array(self.calc(), 'd')
        return self.val
class SinusoidSample(AnalogInput):
    def calc(self):
        self.x = self.x + 0.1 if hasattr(self, 'x') else 0.0
        noise = (random.random() - 0.5) / 10.0
        return math.sin(self.x)  + noise        

add_device(AnalogOutput("ao1"), True)
add_device(SinusoidSample("ai1"), True)

r = lscan(ao1, ai1, start, stop, step, latency=lat)

#Setting the return value
#set_return(50.0)
y = r.getReadable(0) 
x = r.getPositions(0) 

index_max = y.index(max(y))
phase_ref = x[index_max]

set_return(phase_ref)