#some further usefull PMAC commands:
# list pc , 10    //list program counter during motion exec
# buffer          //list allocated buffers
#the rotary stuff only works from gpascii, not gpasciiCommander
#delete rotary
#define rotary 4096 //allocates mem
#open rotary      //same as open prog 0
#
# X100 Y100
# X200 Y300
# X600 Y500
# adding lines alway are acknowledged with ACK,
# if not, it means that the buffer is full
# it will acknowlegde when there is again free space.
# close
# delete rotary


#from other gpascii
#list rotary
#rotfree
#size
#b0s //start the rotary buffer in single step mode

import os, sys
sys.path.insert(0,os.path.expanduser('~/Documents/prj/SwissFEL/PBTools/'))
#import pbtools.misc.pp_comm as pp_comm   -> pp_comm.PPComm
from  pbtools.misc.pp_comm import PPComm
from  pbtools.misc.gather import Gather


class MotionBase:
  def __init__(self, comm, gather, verbose):
    self.comm=comm
    self.gather=gather
    self.verbose=verbose

  def setup_sync(self, crdId=1, prgId=2, plcId=2, mode=0, **kwargs):
    '''setup the timing synchronization for the motion program
       mode=0 : no sync at all
       mode=1 : synchronize start
       mode=2 : synchronize start and adapt motion speed
       this function generates the code blocks:
       self.sync_wait and self.sync_run
       sync_wait can be put in the program to force a timing sync
       sync_run are the commands to run the whole program
       '''
    if mode == 0:
      try:
        del self.sync_prg
      except AttributeError:
        pass
      self.sync_run = '&{crdId}b{prgId}r'''.format(prgId=prgId, crdId=crdId)

    elif mode in (1,2):
      # code block to insert in the program
      # - waits untis <flag> is false
      # - waits raising edge of <flag> and the sets DesTimeBase=ServoPeriod

      # flag ='PowerBrick[0].GpioData[0].0.1==1'
      flag0='Gate3[1].Chan[0].UserFlag==0'
      flag1='Gate3[1].Chan[1].UserFlag==0'
      prg = '''
  //L0=Sys.PhaseCount
  //send 1"sync0 %d:%d\\n",Sys.PhaseCount,Sys.PhaseCount-L0
  while({flag0}){{}}
  //send 1"sync1 %d:%d\\n",Sys.PhaseCount,Sys.PhaseCount-L0
  //L1=Sys.PhaseCount
  while({flag1}){{}}
  //PowerBrick[0].GpioData[0].16.8=255
  //send 1"sync2 %d:%d\\n",Sys.PhaseCount,Sys.PhaseCount-L1
  '''.format(plcId=plcId, crdId=crdId, flag0=flag0, flag1=flag1)
      self.sync_prg = prg
      self.sync_run = '&{crdId}b{prgId}r'''.format(prgId=prgId, plcId=plcId, crdId=crdId)
      if mode==2:
        notFlag1 = 'Gate3[1].Chan[1].UserFlag!=0'
        try:
          prop=kwargs['prop'] #proportional value to adapt speed
        except KeyError:
          prop = 1E-4
        try:
          maxJitter = kwargs['maxJitter']  # proportional value to adapt speed
        except KeyError:
          maxJitter=100
        prg='''close all buffers
disable plc {plcId}
open plc {plcId}
Coord[1].DesTimeBase=Sys.ServoPeriod //reset to 100% timebase
Coord[1].Q[0]=0 //set CryPos counter to 0

while({flag0}){{}} //wait Jungfrau start Trigger
while({flag1}){{}} //ESx detector trigger


L0=0 //CryPos counter
//L1 ServoCount event 1
//L2 jitter: neg:motion lags trigger,pos:motion leads trigger

while(L0>=0) //as long we are not at the last point
{{
  L0=Coord[1].Q[0] //CryPos counter
  while({flag1} && L0==Coord[1].Q[0]){{}} //wait for event
  L1=Sys.ServoCount   //a event happened
  L2=1000
  //send 1"event\\n"
  if(L0==Coord[1].Q[0])
  {{//it was a trigger
    //send 1"trigger %d %d\\n",L0,L1
    while(Sys.ServoCount<L1+{maxJitter}) //wait half a trigger period for event
    {{
      if(L0!=Coord[1].Q[0])
      {{//event CryPos counter
        L2=L1-Sys.ServoCount //jitter: neg:motion lags trigger
        break
      }}
    }}
  }}
  else
  {{//it was a CryPos counter
    //send 1"CryPos%d %d\\n",L0,L1
    while(Sys.ServoCount<L1+{maxJitter}) //wait half a trigger period for event
    {{
      if({notFlag1})
      {{//event trigger
        L2=Sys.ServoCount-L1 //jitter: pos:motion leads trigger
        break
      }}
    }}
  }}
  if(L2!=1000)
  {{
    L3=Sys.ServoPeriod*(1-{prop}*L2)
    //send 1"jitter %d timebase: %f\\n",L2,L3
    Coord[1].DesTimeBase=L3
  }}
}}
disable plc {plcId}
close
enable plc {plcId}
'''.format(plcId=plcId, crdId=crdId, flag0=flag0, flag1=flag1, notFlag1=notFlag1, prop=prop, maxJitter=maxJitter)
        comm=self.comm
        if comm is not None:
          gpascii=comm.gpascii
          gpascii.send_block(prg)

  def run(self):
    'runs the code sync_run which has been generated with setup_sync()'
    comm = self.comm
    gpascii = comm.gpascii
    try:
      cmd=self.sync_run
    except AttributeError:
      raise 'Need to call setup sync before'
    gpascii.send_block(cmd)