#!/usr/bin/env python
# *-----------------------------------------------------------------------*
# |                                                                       |
# |  Copyright (c) 2016 by Paul Scherrer Institute (http://www.psi.ch)    |
# |                                                                       |
# |              Author Thierry Zamofing (thierry.zamofing@psi.ch)        |
# *-----------------------------------------------------------------------*
'''
shape an optimal path with given points

verbose bits:
  1 basic info
  4 list program
  4 upload progress
  8 plot gather path

  prog_1(),plot_1():  position dependent friction(=current) Move multiple times with different speeds
                      generate friction lut and save to friction.mat
  prog_2(),plot_2():  plot position speed acceleration. Move multiple times with different accelerations

Acquired time is:MaxSamples*Period*.2
'''

import os, sys, json
import numpy as np
import matplotlib as mpl
import matplotlib.pyplot as plt
import subprocess as sprc
import telnetlib, re
from utilities import *

os.environ['PATH']='/home/zamofing_t/scripts/:'+os.environ['PATH']

class MoveRecord:
    def __init__(self,args):
      fn='/tmp/moverecord'
      self.fnPrg=fn+'.prg'
      self.fnNpz=fn+'.npz'
      #cfg = {"sequencer": ['prog_1(host="SAROP11-CPPM-MOT6871",acq_per=10)', 'plot_1()']}
      #cfg = {"sequencer": ['plot_1()']}
      #cfg = {"sequencer": ['prog_2(host="SAROP11-CPPM-MOT6871",acq_per=10)', 'plot_2()']}
      #cfg = {"sequencer": ['plot_2()']}
      cfg = {"sequencer": ['prog_3(host="SAROP11-CPPM-MOT6871",acq_per=10)', 'plot_2()']}
      self.cfg=dotdict(cfg)
      self.args=args

    def run(self):
      print('args='+str(self.args))
      print('cfg='+str(self.cfg))
      try:
        sequencer= self.cfg.pop('sequencer')
      except KeyError:
        print('no command sequence to execute')
      else:
        for cmd in sequencer:
          print '>'*5+' '+cmd+' '+'<'*5
          eval('self.' + cmd)

    def prg_prolog(self):
      prg=self.prg
      gather=self.gather
      channels=self.channels
      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'%(self.prgId))

    def prg_epilog(self):
      fnPrg=self.fnPrg
      fnNpz=self.fnNpz
      host=self.host
      prg=self.prg

      prg.append('close')
      prg.append('&1\nb%dr\n'%self.prgId)
      if self.args.verbose & 4:
        for ln in prg:
          print(ln)

      if fnPrg is not None:
        fh=open(fnPrg,'w')
        fh.write('\n'.join(prg))
        fh.close()
        if host is not None:
          cmd ='gpasciiCommander --host '+host+' '+ fnPrg
          print cmd
          p = sprc.Popen(cmd, shell=True)#, stdout=sprc.PIPE, stderr=sprc.STDOUT)
          #res=p.stdout.readlines();  print res
          retval = p.wait()
          #gather -u /var/ftp/gather/out.txt
          cmd ='PBGatherPlot -m24 -v7 --host '+host
          print cmd
          p = sprc.Popen(cmd, shell=True)#, stdout=sprc.PIPE, stderr=sprc.STDOUT)
          retval = p.wait()

          meta = self.meta
          channels = self.channels
          fnLoc='/tmp/gather.txt'
          self.rec=rec=np.genfromtxt(fnLoc, delimiter=' ')
          # rec=Motor[1].ActPos,Motor[2].ActPos,Motor[3].ActPos,Motor[1].DesPos,Motor[2].DesPos,Motor[3].DesPos
          # res=rot.ActPos,y.ActPos,x.ActPos,rot.DesPos,y.DesPos,x.DesPos
          # idx     0        1        2          3        4        5
          if fnNpz:
            np.savez_compressed(fnNpz, rec=rec, prg=self.prg, channels=self.channels, meta=meta)

    def prog_1(self,prgId=2,host=None,acq_per=1):
      '''
      kwargs:
        acq_per    : acquire period: acquire data all acq_per servo loops (default=1)
        prgId      : used program number
        file       : used filename to save if None, not saved
        host       : host to send and execute program, if None, nothing is executed
      '''
      ServoPeriod= .2 #0.2ms Sys.ServoPeriod is dependent of !common() macro

      self.prgId=prgId
      self.file=file
      self.host=host
      self.gather={"MaxSamples":1000000, "Period":acq_per}
      self.channels=channels=["Motor[3].ActPos","Motor[3].DesPos","Motor[3].PhasePos","Motor[3].idMeas","Motor[3].iqMeas"]
      self.meta=meta = {'timebase': ServoPeriod*self.gather['Period']}

      self.prg=prg=[]
      #prg.append('#1..3$')
      #prg.append('#1..3j/')

      self.prg_prolog()
      prg=self.prg

      prg.append('  linear abs')
      prg.append('jog3=300')
      prg.append('dwell 100')
      prg.append('Gather.Enable=2')
      #for spd in (5,10,20,30,40):
      #for spd in (10,20):
      for spd in (10,10,10,10,20,20,20,20,20,20):
        prg.append('Motor[3].JogSpeed=%d'%spd)
        #prg.append('jog3=27000')
        prg.append('jog3=28500')
        prg.append('dwell 100')
        prg.append('jog3=300')
        prg.append('dwell 100')
      prg.append('Gather.Enable=0')

      self.prg_epilog()
      rec=self.rec
      meta['HomePos']=rec[0,1]-300 #rec[0,1]= first DesPos, 300 um is the start position of the motion
      print meta
      if self.fnNpz:
        np.savez_compressed(self.fnNpz, rec=rec, prg=prg, channels=channels, meta=meta)

      #!mx-stage()
      #  &1 #1->0
      #  &1 #2->0
      #  #3$
      #  an anschlag (kabel) bewegen
      #  #3hmz
      #  Motor[3].PhasePos=1200
      #  #3j/
      #  &1p


    @staticmethod
    def onclick(event):
        print 'button=%s, x=%d, y=%d, xdata=%f, ydata=%f'%(
            event.button, event.x, event.y, event.xdata, event.ydata)
        obj=event.canvas.figure.obj


    def plot_1(self):
      try:
        rec=self.rec
        prg=self.prg
        channels=self.channels
        meta=self.meta
      except AttributeError:
        print 'load file %s'%self.fnNpz
        fh=np.load(self.fnNpz)
        rec=fh['rec']
        prg=list(fh['prg'])
        channels=list(fh['channels'])
        meta=fh['meta'].item()

      actPos=rec[:,0]-meta['HomePos']
      desPos=rec[:,1]-meta['HomePos']
      desVel=np.diff(desPos)


      time=np.arange(0,rec.shape[0])*meta['timebase']

      fig=plt.figure();
      fig.obj=self; cid=fig.canvas.mpl_connect('button_press_event', self.onclick)
      fig.canvas.set_window_title('position')
      ax = fig.add_subplot(1,1,1)
      hl=[]
      hl+=ax.plot(time,actPos,'r-',label=channels[0])
      hl+=ax.plot(time,desPos,'g-',label=channels[1])
      hl+=ax.plot(time,rec[:,2],'b-',label=channels[2])
      ax.xaxis.set_label_text('ms')
      ax.yaxis.set_label_text('um')
      legend = ax.legend(loc='upper right', shadow=True)

      fig=plt.figure();
      fig.obj=self; cid=fig.canvas.mpl_connect('button_press_event', self.onclick)
      fig.canvas.set_window_title('current')
      ax = fig.add_subplot(1,1,1)
      hl=[]
      sz=100; weights = np.repeat(1.0, sz) / sz
      v = np.convolve(rec[:,3], weights, 'same')
      hl+=ax.plot(time,v,'r-',label=channels[3])
      v = np.convolve(rec[:, 4], weights, 'same')
      hl+=ax.plot(time,v,'g-',label=channels[4])
      ax.xaxis.set_label_text('ms')
      ax.yaxis.set_label_text('current in bits')
      legend = ax.legend(loc='upper right', shadow=True)
      print 'abs average ',channels[4], np.abs(rec[:, 4]).mean()

      fig=plt.figure();
      fig.obj=self; cid=fig.canvas.mpl_connect('button_press_event', self.onclick)
      fig.canvas.set_window_title('friction(=current) at position current')
      ax = fig.add_subplot(1,1,1)
      hl=[]
      sz=10; weights = np.repeat(1.0, sz) / sz
      curr = np.convolve(rec[:,4], weights, 'same')

      hl+=ax.plot(desPos,curr,'y-',label=channels[4])

      sz=100; weights = np.repeat(1.0, sz) / sz

      idx=desVel[:-1]>0
      arg=np.argsort(desPos[idx])
      p1=desPos[idx][arg];c1=curr[idx][arg]
      c1 = np.convolve(c1, weights, 'same')

      idx=desVel[:-1]<0
      arg=np.argsort(desPos[idx])
      p2=desPos[idx][arg];c2=curr[idx][arg]
      c2 = np.convolve(c2, weights, 'same')

      hl+=ax.plot(p1,c1,'g-',label=channels[4]+' vel>0')
      hl+=ax.plot(p2,c2,'r-',label=channels[4]+' lut')
      #hl+=ax.plot(p1,c1-c1.mean(),'g-',label=channels[4]+' vel>0')
      #hl+=ax.plot(p2,c2-c2.mean(),'r-',label=channels[4]+' vel<0')

      cAll=c1;pAll=p1
      cAll=cAll-cAll.mean()
      #p=p[::100];c=c[::100] # poor selection of points
      p=np.arange(500,28000,100) #np.arange(500,28000,128)
      c=np.interp(p,pAll,cAll)

      hl+=ax.plot(p,c,'b.',label=channels[4]+' vel<0')
      FfricLut=np.array([p,c]).T

      Ffric=np.array([c1.mean(),c2.mean()])
      print 'Avg current forward:',Ffric[0],'Avg current backward:',Ffric[1]
      #print 'FfricLut',FfricLut
      print '//positions '+'%g,%g,%g'%tuple(p[0:3]),'...%g,%g,%g'%tuple(p[-3:])
      print 'float lutCur[%d]={'%len(p)
      for i in range(len(p)):
        print '%g,'%(c[i]),
      print '};'
      import scipy.io
      fn='/home/zamofing_t/afs/ESB-MX/data/friction.mat'
      #scipy.io.savemat(fn,mdict={'Ffric':Ffric,'FfricLut':FfricLut})
      print '\n\nuncomment line above to saved to matlab file',fn


      #ax.xaxis.set_label_text(channels[1])
      ax.yaxis.set_label_text('current in bits: '+channels[4])
      legend = ax.legend(loc='upper right', shadow=True)

      plt.show()

    def prog_2(self,prgId=2,host=None,acq_per=1):
      '''
      kwargs:
        acq_per    : acquire period: acquire data all acq_per servo loops (default=1)
        prgId      : used program number
        file       : used filename to save if None, not saved
        host       : host to send and execute program, if None, nothing is executed
      '''
      ServoPeriod= .2 #0.2ms Sys.ServoPeriod is dependent of !common() macro

      self.prgId=prgId
      self.file=file
      self.host=host
      self.gather={"MaxSamples":1000000, "Period":acq_per}
      self.channels=["Motor[3].ActPos","Motor[3].DesPos","Motor[3].PhasePos","Motor[3].idMeas","Motor[3].iqMeas"]
      self.meta=meta = {'timebase': ServoPeriod*self.gather['Period']}

      self.prg=prg=[]
      #prg.append('#1..3$')
      #prg.append('#1..3j/')

      self.prg_prolog()
      prg=self.prg

      prg.append('  linear abs')
      prg.append('Motor[3].JogTs=0')
      prg.append('Motor[3].JogSpeed=10')
      prg.append('Motor[3].JogTa=-2.5')

      prg.append('jog3=10000')
      prg.append('dwell 100')
      prg.append('Gather.Enable=2')
      for spd in (5,10,20,30,40):
        prg.append('Motor[3].JogSpeed=%d'%spd)
      #for acc in (-2.5,-1.25,-.5,-.25):
      #  prg.append('Motor[3].JogTa=%g'%acc)
        prg.append('jog3=17000')
        prg.append('dwell 100')
        prg.append('jog3=10000')
        prg.append('dwell 1000')
      prg.append('Gather.Enable=0')

      self.prg_epilog()
      #  &1 #1->0
      #  &1 #2->0
      #  #3$
      #  an anschlag (kabel) bewegen
      #  #3hmz
      #  Motor[3].PhasePos=1200   # or for motor 2: Motor[2].PhasePos=300
      #  #3j/
      #  &1p


      #folgende parameter setzen: Motor[3].HomePos
      meta['HomePos']=-223.8   #Motor[3].HomePos
      meta['PhasePos']=1200  #Motor[3].PhasePos


    def plot_2(self):
      try:
        rec=self.rec
        prg=self.prg
        channels=self.channels
        meta=self.meta
      except AttributeError:
        fh=np.load(self.fnNpz)
        rec=fh['rec']
        prg=list(fh['prg'])
        channels=list(fh['channels'])
        meta=fh['meta'].item()

      time=np.arange(0,rec.shape[0])*meta['timebase']

      fig=plt.figure();
      fig.obj=self; cid=fig.canvas.mpl_connect('button_press_event', self.onclick)
      fig.canvas.set_window_title('position')
      ax = fig.add_subplot(1,1,1)
      hl=[]
      p_avg=rec[:,1].mean();
      p=rec[:,0]-p_avg
      hl+=ax.plot(time,p,'r-',label=channels[0])
      #d=np.diff(p)*100
      #hl+=ax.plot(time[1:],d,'r-',label='vel of '+channels[0])
      #a=np.diff(d)*100
      #hl+=ax.plot(time[2:],a,'r-',label='acc of '+channels[0])

      p=rec[:,1]-p_avg
      hl+=ax.plot(time,p,'g-',label=channels[1])
      d=np.diff(p)*50
      hl+=ax.plot(time[1:],d,'b-',label='vel of '+channels[0])
      a=np.diff(d)*50
      hl+=ax.plot(time[2:],a,'m-',label='acc of '+channels[0])

      #hl+=ax.plot(time,rec[:,2],'b-',label=channels[2])
      ax.xaxis.set_label_text('ms')
      ax.yaxis.set_label_text('um')
      legend = ax.legend(loc='upper right', shadow=True)

      fig=plt.figure();
      fig.obj=self; cid=fig.canvas.mpl_connect('button_press_event', self.onclick)
      fig.canvas.set_window_title('current')
      ax = fig.add_subplot(1,1,1)
      hl=[]
      sz=100; weights = np.repeat(1.0, sz) / sz
      v = np.convolve(rec[:,3], weights, 'same')
      hl+=ax.plot(time,v,'r-',label=channels[3])
      v = np.convolve(rec[:, 4], weights, 'same')
      hl+=ax.plot(time,v,'g-',label=channels[4])
      hl+=ax.plot(time,p/100.,'y-',label=channels[1])
      hl+=ax.plot(time,(rec[:,0]-p_avg)/100.,'g-',label=channels[0])
      hl+=ax.plot(time,(rec[:,0]-rec[:,1]),'c-',label='PosError')
      ax.xaxis.set_label_text('ms')
      ax.yaxis.set_label_text('current in bits')
      legend = ax.legend(loc='upper right', shadow=True)
      print 'abs average ',channels[4], np.abs(rec[:, 4]).mean()
      print 'abs average pos Error',np.abs(rec[:,0]-rec[:,1]).mean()
      plt.show()


    def prog_3(self,prgId=2,host=None,acq_per=1):
      '''
      kwargs:
        acq_per    : acquire period: acquire data all acq_per servo loops (default=1)
        prgId      : used program number
        file       : used filename to save if None, not saved
        host       : host to send and execute program, if None, nothing is executed
      '''
      ServoPeriod= .2 #0.2ms Sys.ServoPeriod is dependent of !common() macro

      self.prgId=prgId
      self.file=file
      self.host=host
      self.gather={"MaxSamples":1000000, "Period":acq_per}
      self.channels=channels=["Motor[3].ActPos","Motor[3].DesPos","Motor[3].PhasePos","Motor[3].idMeas","Motor[3].iqMeas"]
      self.meta=meta = {'timebase': ServoPeriod*self.gather['Period']}

      self.prg=prg=[]
      #prg.append('#1..3$')
      #prg.append('#1..3j/')

      self.prg_prolog()
      prg=self.prg

      fh=open('/tmp/shapepath.prg','r')
      lines=fh.read().splitlines()
      fh.close()
      prg.extend(lines[11:-3])
      self.prg_epilog()
      #print '\n'.join(prg)
      rec=self.rec
      lines[12]
      x0=float(re.match('X(\S+)\sY\S+',lines[12]).group(1))
      meta['HomePos']=rec[0,1]-x0 #rec[0,1]= first DesPos is x0, the start position of the motion
      print meta
      if self.fnNpz:
        np.savez_compressed(self.fnNpz, rec=rec, prg=prg, channels=channels, meta=meta)


      #  &1 #1->0
      #  &1 #2->0
      #  #3$
      #  an anschlag (kabel) bewegen
      #  #3hmz
      #  Motor[3].PhasePos=1200   # or for motor 2: Motor[2].PhasePos=300
      #  #3j/
      #  &1p


      #folgende parameter setzen: Motor[3].HomePos
      meta['HomePos']=-223.8   #Motor[3].HomePos
      meta['PhasePos']=1200  #Motor[3].PhasePos


if __name__=='__main__':
  from optparse import OptionParser, IndentedHelpFormatter
  class MyFormatter(IndentedHelpFormatter):
    'helper class for formating the OptionParser'

    def __init__(self):
      IndentedHelpFormatter.__init__(self)

    def format_epilog(self, epilog):
      if epilog:
        return epilog
      else:
        return ""

  def parse_args():
    'main command line interpreter function'
    #usage: gpasciiCommunicator.py --host=PPMACZT84 myPowerBRICK.cfg
    (h, t)=os.path.split(sys.argv[0]);cmd='\n  '+(t if len(h)>3 else sys.argv[0])+' '
    exampleCmd=('--host=PPMAC1391 -m 63 --cfg gather.cfg',
                'samplePowerBrick.cfg',
                '-n stackCheck1.cfg',
                '--host=PPMACZT84 stackCheck1.cfg',
                '--host=PPMACZT84 stackCheck1.cfg -v15',
               )
    epilog=__doc__+'''
Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '

    fmt=MyFormatter()
    parser=OptionParser(epilog=epilog, formatter=fmt)

    parser.add_option('-v', '--verbose', type="int", dest='verbose', help='verbosity bits (see below)', default=0)
    (args, other)=parser.parse_args()
    args.other=other

    mr=MoveRecord(args)
    mr.run()
#------------------ Main Code ----------------------------------
  #ssh_test()
  ret=parse_args()
  exit(ret)
