#!/usr/bin/env python
# *-----------------------------------------------------------------------*
# |                                                                       |
# |  Copyright (c) 2016 by Paul Scherrer Institute (http://www.psi.ch)    |
# |                                                                       |
# |              Author Thierry Zamofing (thierry.zamofing@psi.ch)        |
# *-----------------------------------------------------------------------*

#Testing the interferometer 
'''
zamofing_t@ganymede:~/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/cfg$ gpasciiCommander --host $PPMAC -i
$$$***
!common()
!SAR-EXPMX1()
enable plc 1
Motor[1].MaxSpeed=25
Motor[2].MaxSpeed=25

X -12000 ..12000
Y -12000 .. 12000
Gather.Enable=0
Gather.Items=6
Gather.MaxSamples=1000000
Gather.Period=10
Gather.Addr[0]=Motor[1].DesPos.a
Gather.Addr[1]=Motor[2].DesPos.a
Gather.Addr[2]=Motor[1].ActPos.a
Gather.Addr[3]=Motor[2].ActPos.a
Gather.Addr[4]=Motor[6].ActPos.a
Gather.Addr[5]=Motor[7].ActPos.a
open prog 2
  linear abs
  X-12000 Y-12000
  dwell 10
  Gather.Enable=2
  X-12000 Y12000
  dwell 100
  X-12000 Y-12000
  dwell 100
  X12000 Y-12000
  dwell 100
  X-12000 Y-12000
  dwell 1000
Gather.Enable=0
close
#1..7j/
&1
b2r

download data with
PPMAC=SAR-CPPM-EXPMX1
PBGatherPlot -m24 -v7 --host $PPMAC --dat gather.txt

cat /tmp/gather.txt

Motor[2].MaxSpeed=20;Motor[3].MaxSpeed=20
Motor[2].MaxSpeed=5;Motor[3].MaxSpeed=5
Motor[2].MaxSpeed=2
Motor[3].MaxSpeed=2
b2r



'''

import os, sys, json, time
import numpy as np
import matplotlib as mpl
import matplotlib.pyplot as plt
import subprocess as sprc
import telnetlib

class Interferometer():
  def __init__(self,**kwargs):
    for k,v in kwargs.iteritems():
      setattr(self,k,v)


  def gen_prog(self,prgId=2,mode=0,**kwargs):
    '''
    kwargs:
      acq_per    : acquire period: acquire data all acq_per servo loops (default=1)
      mode=0: test motion
        xrng:
        yrng:
        spd:
    '''
    prg=[]
    acq_per=kwargs.get('acq_per',10)
    gather={"MaxSamples":1000000, "Period":acq_per}
    #Sys.ServoPeriod is dependent of !common() macro
    ServoPeriod= .2 #0.2ms
    #ServoPeriod = .05
    self.meta = {'timebase': ServoPeriod*gather['Period']}
    channels=["Motor[1].DesPos","Motor[2].DesPos","Motor[1].ActPos","Motor[2].ActPos","Motor[6].ActPos","Motor[7].ActPos"]
    prg.append('Gather.Enable=0')
    prg.append('Gather.Items=%d'%len(channels))
    for k,v in gather.iteritems():
      prg.append('Gather.%s=%d'%(k,v))
    for i,c in enumerate(channels):
      prg.append('Gather.Addr[%d]=%s.a'%(i,c))


    prg.append('open prog %d'%(prgId))
    prg.append('  P1000=0')
    # this uses Coord[1].Tm and limits with MaxSpeed

    #******** mode -1 ********
    if mode==0: #### jog all motors 10000um (or 10000 mdeg)
      xRng = kwargs.get('xRng', (-12000,12000))
      yRng = kwargs.get('yRng', (-12000,12000))
      spd = kwargs.get('spd', 10)
      prg.append('Motor[1].MaxSpeed=%f'%spd)
      prg.append('Motor[2].MaxSpeed=%f'%spd)
      prg.append('''
  linear abs
  X%g Y%g
  dwell 10
  Gather.Enable=2
  X%g Y%g
  dwell 100
  X%g Y%g
  dwell 100
  X%g Y%g
  dwell 100
  X%g Y%g
  dwell 100
  Gather.Enable=0
'''%(xRng[0],yRng[0],xRng[0],yRng[1],xRng[0],yRng[0],xRng[1],yRng[0],xRng[0],yRng[0]))

    prg.append('  P1000=1')
    prg.append('close')
    prg.append('&1\nb%dr\n'%prgId)

    self.download(prg,mode=3,file='/tmp/prg.cfg')


  def download(self,prg=None,mode=0,file='/tmp/interfero.cfg'):
    '''
      mode bits:
       bit 0 (1): wait execution finished (P1000==1)
       bit 1 (2): gather and save data
       bit 2 (4): ...
    '''
    host=self.host
    if prg is not None and self.verbose & 4:
      for ln in prg:
        print(ln)

    if file is not None and prg is not None:
      fh=open(file,'w')
      fh.write('\n'.join(prg))
      fh.close()
    if host is not None:
      # ***download and start the program***
      cmd ='gpasciiCommander --host '+host+' '+ file
      print(cmd)
      p = sprc.Popen(cmd, shell=True)#, stdout=sprc.PIPE, stderr=sprc.STDOUT)
      #res=p.stdout.readlines();  print res
      retval = p.wait()

      if mode &1:
        # ***wait program finished P1000=1***
        com=GpasciiCommunicator().connect(host,prompt='# ')
        ack=GpasciiCommunicator.gpascii_ack
        sys.stdout.write('wait execution...');sys.stdout.flush()
        while(True):
          #Gather.MaxLines calculates maximum numbewr of gathering into memory
          com.write('P1000\n')
          val=com.read_until(ack)
          #print val
          val=int(val[val.find('=')+1:].rstrip(ack))
          if val==1:break
          #com.write('Gather.Index\n')
          #val=com.read_until(ack)
          #print val
          time.sleep(.2)
          sys.stdout.write('.');sys.stdout.flush()
      if mode &2:
        fnRmt = '/var/ftp/gather/out.txt'
        fnLoc = '/tmp/gather.txt'
        print('\ngather data to %s...' % fnRmt)
        p = sprc.Popen(('ssh', 'root@' + host, 'gather ', '-u', fnRmt), shell=False, stdin=sprc.PIPE, stdout=sprc.PIPE,
                       stderr=sprc.PIPE)
        res = p.wait()
        if res:
          print('ssh failed. ssh root@%s to open a session' % host)
          return

        print('transfer data to %s...' % fnLoc)
        p = sprc.Popen(('scp', 'root@' + host + ':' + fnRmt, fnLoc), shell=False, stdin=sprc.PIPE, stdout=sprc.PIPE,
                       stderr=sprc.PIPE)
        res = p.wait()
        self.rec = np.genfromtxt(fnLoc, delimiter=' ')

  def show_pos(self):
    rec=self.rec
    #"M1.DesPos","M2.DesPos","M1.ActPos","M2.ActPos","M6.ActPos","M7.ActPos"]
    #idx     0        1        2          3        4        5
    f1=plt.figure()
    ax1 = f1.add_subplot(1,1,1)

    x=range(len(rec[:,0]))

    hl=ax1.plot(x,-(rec[:,0]-rec[0,0]),'k')
    hl=ax1.plot(x,-(rec[:,2]-rec[0,2]),'r')
    hl=ax1.plot(x,-(rec[:,4]-rec[0,4])/100,'m')
    hl=ax1.plot(x,rec[:,1]-rec[0,1],'k')
    hl=ax1.plot(x,rec[:,3]-rec[0,3],'b')
    hl=ax1.plot(x,-(rec[:,5]-rec[0,5])/100,'c')
    #hl=ax1.plot(x,-(rec[:,0]-rec[0,0]),'k')
    #hl=ax1.plot(x,-(rec[:,2]-rec[0,2]),'r')
    #hl=ax1.plot(x,-(rec[:,4]-rec[0,4])/100,'m')
    #hl=ax1.plot(x,rec[:,1]-rec[0,1],'k')
    #hl=ax1.plot(x,rec[:,3]-rec[0,3],'b')
    #hl=ax1.plot(x,-(rec[:,5]-rec[0,5])/100,'c')

    plt.show()

  def show_poserr(self):
    rec=self.rec
    #"M1.DesPos","M2.DesPos","M1.ActPos","M2.ActPos","M6.ActPos","M7.ActPos"]
    #idx     0        1        2          3        4        5
    f1=plt.figure()
    ax1 = f1.add_subplot(1,1,1)

    t=range(len(rec[:,0]))
    actYerr=(rec[:,2]-rec[:,0]);actYerr-=actYerr[0]
    actXerr=(rec[:,3]-rec[:,1]);actXerr-=actXerr[0]
    infYerr=(rec[:,4]-rec[:,0]);infYerr-=infYerr[0]
    infXerr=(rec[:,5]-rec[:,1]);infXerr-=infXerr[0]

    hl=ax1.plot(t,actYerr,'r',label='actYerr')
    hl=ax1.plot(t,actXerr,'b',label='actXerr')
    hl=ax1.plot(t,infYerr,'m',label='infYerr')
    hl=ax1.plot(t,infXerr,'c',label='infXerr')

    #plt.legend(handles=h)
    ax1.legend()
    plt.show()

class GpasciiCommunicator():
  '''Communicates with the Delta Tau gpascii programm
  '''
  gpascii_ack="\x06\r\n"
  gpascii_inp='Input\r\n'

  def connect(self, host, username='root', password='deltatau',prompt='ppmac# ',verbose=0):
    p=telnetlib.Telnet(host)
    s=p.read_until('login: ')
    if verbose: print(s)
    p.write(username+'\n')
    s =p.read_until('Password: ')
    if verbose: print(s)
    p.write(password+'\n')
    s =p.read_until(prompt)  # command prompt
    if verbose: print(s)
    p.write('gpascii -2\n')  # execute gpascii command
    s=p.read_until(self.gpascii_inp)
    if verbose: print(s)
    return p


if __name__=='__main__':

  ifm=Interferometer(verbose = 255,host='SAR-CPPM-EXPMX1')
  os.chdir(os.path.join(os.path.dirname(__file__), '../cfg'))
  ifm.download(['$$$***','!common()','!SAR-EXPMX1()','#1..7j/','enable plc 1','Motor[1].MaxSpeed=25','Motor[2].MaxSpeed=25'])
  raw_input('press return when homed')
  #ifm.gen_prog(mode=0)
  #ifm.show_poserr()
  #ifm.gen_prog(mode=0,xRng=(-10000,10000),yRng=(-10000,10000),spd=10)
  ifm.gen_prog(mode=0,xRng=(-5000,5000),yRng=(-5000,5000),spd=5)
  ifm.show_poserr()
  #ifm.gen_prog(mode=0,xRng=(-2000,2000),yRng=(4000,8000),spd=5)
  #ifm.show_poserr()

  #ifm.rec = np.genfromtxt('/scratch/gather.txt', delimiter=' ')
  #ifm.show_pos()
