#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:
        #frequence jitter 50Hz Swissgrid:
        #https://www.swissgrid.ch/de/home/operation/grid-data/current-data.html#
        notFlag1 = 'Gate3[1].Chan[1].UserFlag!=0'
        try:
          prop=kwargs['prop'] #proportional value to adapt speed
        except KeyError:
          prop = .5E-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 syncCnt counter to 0

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


{syncCnt}=0 //sync counter, sent when motion is on a crystal
//{evnt} current PhaseCount event
//{jitter} jitter: neg:motion lags trigger,pos:motion leads trigger
//{syncEnvt} Prev syncEnvt PhaseCount
{sync2sync}=0 // last syncEnvt-syncEnvt PhaseCount
{dtb}=Sys.ServoPeriod //DesTimeBase
//{tmp} temp variable

while({syncCnt}>=0) //as long we are not at the last point
{{
  {syncCnt}=Coord[1].Q[0] //sync counter
  while({flag1} && {syncCnt}==Coord[1].Q[0]){{}} //wait for event
  {evnt}=Sys.PhaseCount   //a event happened
  {jitter}=1000
  //send 1"event\\n"
  if({syncCnt}==Coord[1].Q[0])
  {{//it was a trigger
    //send 1"trigger %d %d %d\\n",{syncCnt},{evnt},{sync2sync}
    while(Sys.PhaseCount<{evnt}+{maxJitter}) //wait half a trigger period for event
    {{
      if({syncCnt}!=Coord[1].Q[0])
      {{//event syncCnt
        {tmp}=Sys.PhaseCount
        {sync2sync}={tmp}-{syncEnvt};{syncEnvt}={tmp}
        {jitter}={evnt}-{tmp} //jitter: neg:motion lags trigger
        break
      }}
    }}
  }}
  else
  {{//it was a syncCnt
    {sync2sync}={evnt}-{syncEnvt};{syncEnvt}={evnt}
    //send 1"syncCnt %d %d %d\\n",{syncCnt},{evnt},{sync2sync}
    while(Sys.PhaseCount<{evnt}+{maxJitter}) //wait half a trigger period for event
    {{
      if({notFlag1})
      {{//event trigger
        {jitter}=Sys.PhaseCount-{evnt} //jitter: pos:motion leads trigger
        break
      }}
    }}
  }}
  if({jitter}!=1000 && {sync2sync}>0)
  {{
    {dtb}=Sys.ServoPeriod*(1-{prop}*{jitter})
    //{dtb}={dtb}*(1-{jitter}/{sync2sync}*.5)
    //send 1"syncCnt %d jitter %d sync2sync %d timebase: %f\\n",{syncCnt},{jitter},{sync2sync},{dtb}
    Coord[1].DesTimeBase={dtb}
  }}
}}
disable plc {plcId}
close
enable plc {plcId}
'''.format(plcId=plcId, crdId=crdId, flag0=flag0, flag1=flag1, notFlag1=notFlag1, prop=prop, maxJitter=maxJitter,
           syncCnt='L0', evnt='L1', jitter='L2', syncEnvt='L3', sync2sync='L4', dtb='L5', tmp='L6')
        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)