#!/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
  2 plot sorting steps
  4 list program
  4 upload progress
  8 plot gather path

#config file example:
{
  "points": [
    [100,523],[635,632],[756,213],
  "sequencer":[
    'gen_grid_points(w=10,h=10,pitch=100,rnd=.2)',
    'sort_points()',
    'gen_prog(file="'+fn+'.prg")',
    'plot_gather("'+fn+'.npz")']
}

Sequencer functions are:
 - generate points (if not in the 'points' configuration)
   gen_rand_points(self,n=107,scale=1000)
   gen_grid_points(w=10,h=10,pitch=100,rnd=.2)
 - sorting points:
    sort_points(self)
 - generate/download/execute motion progran, upload trace of motors (gather data)
    gen_prog(self,prgId=2,file=None,host=None)
      if host=None nothing will be downloaded/executed and trace of motors will not be uploaded
      if file=None the program will not be saved and nothing will be executed
      gen_prog modes:
      -1 jog a 10mm square
       0 linear motion
       1 pvt motion
       2 spline motion


 - plot gathered data
     plot_gather("'+fn+'.npz")
       this makes only sence, if motion has been executed and data can be gathered from the powerbrick

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
from utilities import *

class ShapePath:
    def __init__(self,args):
      if args.cfg:
        fh=open(args.cfg,'r')
        s=fh.read()
        cfg=json.loads(s, object_hook=ConvUtf8)
        s=json.dumps(cfg, indent=2, separators=(',', ': '));print(s)
      else:
        fn='/tmp/shapepath4'
        #fn='/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/data/'+time.strftime('%y-%m-%d-%H_%M_%S')
        #cfg={"points": [[100,523],[635,632],[756,213]],"sequencer":['sort_points()','gen_prog(file="'+fn+'.prg",host="SAR-CPPM-EXPMX1")','plot_gather("'+fn+'.npz")']}
        #cfg={"sequencer":['gen_rand_points(n=107, scale=1000)','sort_points()','gen_prog(file="'+fn+'.prg",host="SAR-CPPM-EXPMX1")','plot_gather("'+fn+'.npz")']}
        #cfg={"sequencer":['gen_grid_points(w=10,h=10,pitch=100,rnd=.2)','sort_points()','gen_prog(file="'+fn+'.prg",host="SAR-CPPM-EXPMX1")','plot_gather("'+fn+'.npz")']}
        #cfg={"sequencer":['gen_grid_points(w=10,h=10,pitch=100,rnd=0.2)','sort_points()','gen_prog(file="'+fn+'.prg")','plot_gather("'+fn+'.npz")']}
        #cfg = {"sequencer": ['gen_rand_points(n=107, scale=1000)', 'sort_points()','plot_gather("'+fn+'.npz")']}
        #cfg={"sequencer":['gen_grid_points(w=20,h=20,pitch=50,rnd=.2)','sort_points()','gen_prog(file="'+fn+'.prg",host="SAR-CPPM-EXPMX1")','plot_gather("'+fn+'.npz")']}
        #cfg={"sequencer":['gen_grid_points(w=20,h=20,pitch=50,rnd=.2)','sort_points()','gen_prog(file="'+fn+'.prg")','plot_gather("'+fn+'.npz")']}
        #cfg={"sequencer":['gen_rand_points(n=400, scale=1000)','sort_points()','gen_prog(file="'+fn+'.prg",host="SAR-CPPM-EXPMX1")','plot_gather("'+fn+'.npz")']}
        #cfg = {"sequencer": ['gen_grid_points(w=20,h=20,pitch=50,rnd=.2)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAR-CPPM-EXPMX1",mode=0)', 'plot_gather("'+fn+'.npz")']}
        #cfg = {"sequencer": ['gen_grid_points(w=5,h=5,pitch=50,rnd=.2)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAR-CPPM-EXPMX1",mode=1)', 'plot_gather("'+fn+'.npz")']}
        #cfg = {"sequencer": ['gen_grid_points(w=2,h=2,pitch=100,rnd=0)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAR-CPPM-EXPMX1",mode=0)', 'plot_gather("'+fn+'.npz")']}
        #cfg = {"sequencer": ['gen_grid_points(w=5,h=5,pitch=100,rnd=0.2)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAR-CPPM-EXPMX1",mode=0)', 'plot_gather("'+fn+'.npz")']}
        #cfg = {"points": [[0, 0],[100, 0],[200, 0],[300, 0],[400, 0],[400, 100],[300, 100],[200, 100],[100, 100],[0, 100],[10, 200],[100, 200],[200, 200],[300, 200],[400, 200],[410, 300],[300, 300],[200, 300],[100, 300],[0, 300],[0, 400],[100, 400],[200, 400],[300, 400],[400, 400]],"sequencer": ['gen_prog(file="'+fn+'.prg",host="SAR-CPPM-EXPMX1",mode=0)', 'plot_gather("'+fn+'.npz")']}
        #cfg = {"sequencer": ['gen_grid_points(w=2,h=2,pitch=10000,rnd=0)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAR-CPPM-EXPMX1",mode=-1)', 'plot_gather("'+fn+'.npz")']}
        #cfg = {"sequencer": ['gen_grid_points(w=2,h=2,pitch=10000,rnd=0)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAR-CPPM-EXPMX1",mode=1,pt2pt_time=1000)', 'plot_gather("'+fn+'.npz")']}
        #cfg = {"sequencer": ['gen_grid_points(w=20,h=20,pitch=50,rnd=0.2)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAR-CPPM-EXPMX1",mode=1,pt2pt_time=10,acq_per=10)', 'plot_gather("'+fn+'.npz")']}
        #cfg = {"sequencer": ['gen_rand_points(n=400, scale=1000)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAR-CPPM-EXPMX1",mode=1,pt2pt_time=10)', 'plot_gather("'+fn+'.npz")']}
        cfg = {"sequencer": ['gen_grid_points(w=5,h=5,pitch=100,rnd=0.4)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAR-CPPM-EXPMX1",mode=1,pt2pt_time=10,cnt=1)', 'plot_gather("'+fn+'.npz")']}
        #cfg = {"sequencer":['gen_rand_points(n=400, scale=1000)','sort_points()','gen_prog(file="'+fn+'.prg",host="SAR-CPPM-EXPMX1",mode=1,pt2pt_time=20,acq_per=10)','plot_gather("'+fn+'.npz")']}
        #cfg = {"sequencer": ['gen_swissfel_points(scale=300)','gen_prog(file="'+fn+'.prg",host="SAR-CPPM-EXPMX1",mode=1,pt2pt_time=100,acq_per=10)', 'plot_gather("'+fn+'.npz")']}
        #cfg = {"sequencer": ['opt_pts("'+fn+'.npz")','gen_prog(file="'+fn+'_corr.prg",host="SAR-CPPM-EXPMX1",mode=1,pt2pt_time=10,cnt=1)', 'plot_gather("'+fn+'_corr.npz")']}
        #cfg = {"sequencer": ['opt_pts("'+fn+'.npz")','gen_prog(file="'+fn+'.prg",mode=1,pt2pt_time=10,cnt=1)']}

        #cfg = {"sequencer": ['gen_grid_points(w=5,h=5,pitch=100,rnd=0.4)', 'sort_points()','gen_prog(file="'+fn+'.prg",mode=1,pt2pt_time=10,cnt=1)']}

      self.cfg=dotdict(cfg)
      self.args=args

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

    def gen_swissfel_points(self,scale=10):
      #string from inkscape path of the drawing
      d="m 15.801613,951.54022 -1.655274,-0.17578 -1.809082,-0.52002 0,-1.52344 1.765137,0.76172 1.699219,0.25635 1.955566,-0.49805 0.688477,-1.4209 -0.498047,-1.25976 -1.618652,-0.68115 -0.900879,-0.17578 -1.426392,-0.42298 -0.968628,-0.60974 -0.739746,-1.95557 0.254516,-1.29638 0.76355,-0.98877 1.217652,-0.62622 1.602173,-0.20874 1.567383,0.13916 1.6333,0.41748 0,1.44287 -1.589355,-0.60059 -1.442871,-0.19775 -1.071167,0.11719 -0.796509,0.35156 -0.651856,1.33301 0.432129,1.09863 1.655274,0.59326 0.893555,0.18311 1.437377,0.43579 1.001587,0.67749 0.593262,0.92651 0.197754,1.19751 -0.267334,1.3971 -0.802002,1.01257 -1.311035,0.61523 2.995605,-0.004 2.995606,-0.004 -0.536499,-2.05078 -0.536499,-2.05078 -0.536499,-2.05079 -0.536499,-2.05078 1.347656,0 0.421142,1.60034 0.421143,1.60034 0.421143,1.60035 0.421142,1.60034 0.419312,-1.60034 0.419311,-1.60035 0.419312,-1.60034 0.419311,-1.60034 1.589356,0 0.421142,1.60034 0.421143,1.60034 0.421143,1.60035 0.421142,1.60034 0.419312,-1.60034 0.419311,-1.60035 0.419312,-1.60034 0.419311,-1.60034 1.347657,0 -0.536499,2.05078 -0.5365,2.05079 -0.536499,2.05078 -0.536499,2.05078 -1.589355,0 -0.441284,-1.68091 -0.441285,-1.68091 -0.441284,-1.6809 -0.441284,-1.68091 -0.443115,1.68091 -0.443116,1.6809 -0.443115,1.68091 -0.443115,1.68091 2.330933,-8e-5 2.330933,-8e-5 2.330932,-8e-5 2.330933,-8e-5 0,-2.05078 0,-2.05078 0,-2.05079 0,-2.05078 0.673828,-1.48681 0.673828,0 0,-0.85327 0,-0.85327 -0.673828,0 -0.673828,0 0,0.85327 0,0.85327 0.673828,1.48681 0.673828,0 0,2.05078 0,2.05079 0,2.05078 0,2.05078 2.545166,0.1062 2.545166,0.1062 -1.376953,-0.13183 -1.501465,-0.38086 0,-1.3916 1.472168,0.58593 1.435547,0.19043 1.464844,-0.32226 0.512695,-0.92285 -0.373535,-0.84229 -1.038789,-0.41935 -1.048614,-0.25448 -1.078491,-0.33325 -0.730591,-0.47241 -0.55664,-1.50147 0.205078,-1.02539 0.615234,-0.76172 0.99243,-0.47241 1.336672,-0.15747 1.40625,0.10986 1.21582,0.32959 0,1.27442 -1.186523,-0.43946 -1.274414,-0.14648 -1.508789,0.30762 -0.498047,0.92285 0.358886,0.73975 0.763094,0.30189 0.76857,0.22893 0.785513,0.20842 0.813927,0.24037 0.809326,0.49988 0.455932,0.66833 0.151978,0.89173 -0.227051,1.02355 -0.681152,0.78553 -1.071167,0.49988 3.205262,0.0833 3.205261,0.0833 -1.376953,-0.13183 -1.501465,-0.38086 0,-1.3916 1.472168,0.58593 1.435547,0.19043 1.464843,-0.32226 0.512696,-0.92285 -0.373536,-0.84229 -1.038789,-0.41935 -1.048613,-0.25448 -1.078491,-0.33325 -0.730591,-0.47241 -0.556641,-1.50147 0.205078,-1.02539 0.615235,-0.76172 0.99243,-0.47241 1.336672,-0.15747 1.40625,0.10986 1.21582,0.32959 0,1.27442 -1.186524,-0.43946 -1.274414,-0.14648 -1.508789,0.30762 -0.498047,0.92285 0.358887,0.73975 0.805073,0.3095 0.76935,0.22988 0.764915,0.20509 0.791765,0.23514 0.809327,0.49988 0.455932,0.66833 0.151978,0.89173 -0.227051,1.02355 -0.681152,0.78553 -1.071167,0.49988 2.095642,-0.0229 2.095642,-0.0229 0,-1.36688 0,-1.36688 0,-1.36689 0,-1.36688 0,-1.36688 0,-1.36688 0,-1.36689 0,-1.36688 1.571045,0 1.571045,0 1.571045,0 1.571045,0 0,1.24512 -1.201172,0 -1.201172,0 -1.201172,0 -1.201172,0 0,0.80566 0,0.80567 0,0.80566 0,0.80566 1.083984,0 1.083985,0 1.083984,0 1.083985,0 0,1.24512 -1.083985,0 -1.083984,0 -1.083985,0 -1.083984,0 0,1.30554 0,1.30555 0,1.30554 0,1.30554 3.581543,0 3.581543,0 0,-1.36688 0,-1.36688 0,-1.36689 0,-1.36688 0,-1.36688 0,-1.36688 0,-1.36689 0,-1.36688 1.728516,0 1.728516,0 1.728515,0 1.728516,0 0,1.24512 -1.358643,0 -1.358642,0 -1.358643,0 -1.358643,0 0,0.80932 0,0.80933 0,0.80933 0,0.80932 1.30188,0 1.30188,0 1.30188,0 1.30188,0 0,1.24512 -1.30188,0 -1.30188,0 -1.30188,0 -1.30188,0 0,0.9906 0,0.9906 0,0.9906 0,0.9906 1.391602,0 1.391601,0 1.391602,0 1.391602,0 0,1.24512 -3.587581,3.8e-4 -1.964972,0 3.702844,0 4.295998,0 0,-1.36733 0,-1.36688 0,-1.36689 0,-1.36688 0,-1.36688 0,-1.36689 0,-1.36688 0,-1.36688 1.479492,0 0,1.21124 0,1.21124 0,1.21125 0,1.21124 0,1.21124 0,1.21124 0,1.21125 0,1.21124 1.331177,0 1.331177,0 1.331176,0 1.331177,0 0,1.24512 -1.70105,0 -1.701049,0 -1.70105,0"
      d=d.split()
      pts=np.ndarray((len(d)-1,2),dtype=np.float)
      for i in xrange(pts.shape[0]):
        pts[i,:]=map(float,d[i+1].split(','))

      pts[0,:]=(5,0)
      pts=pts.cumsum(0)
      pts[:,1]=-pts[:,1]
      pts*=scale
      self.points=pts

      verb=self.args.verbose
      if verb&2:
        self.plot_points(pts)
      plt.show()

    def gen_rand_points(self,n=107,scale=1000):
        np.random.seed(0)
        #data=np.random.randint(0,1000,(30,2))
        pts=np.random.rand(n,2)*scale
        self.points=pts

    def gen_grid_points(self,w=10,h=10,pitch=100,rnd=.2):
        np.random.seed(0)
        xx,yy=np.meshgrid(range(w), range(h))
        pts=np.array([xx.reshape(-1),yy.reshape(-1)],dtype=np.float).transpose()*pitch
        if rnd != 0:
          pts+=(np.random.rand(pts.shape[0],2)*(rnd*pitch))
        pts[:,0]+=500
        self.points=pts

    def opt_pts(self,fn):
      fh=np.load(fn)
      #res=rot.ActPos,x.ActPos,y.ActPos,rot.DesPos,x.DesPos,y.DesPos
      #idx     0        1        2          3        4        5
      rec=fh['rec']
      pts=fh['pts']
      desPos=rec[:,4:6]

      idx=np.ndarray(shape=len(pts),dtype=np.int32)
      for i in range(len(pts)):
        l=desPos-pts[i,:]
        l2=l[:,0]**2+l[:,1]**2
        idx[i]=np.argmin(l2)

      recPts=rec[idx,:]
      ptsCorr=(pts-recPts[:,1:3]+recPts[:,4:6])
      self.points=pts
      self.ptsCorr=ptsCorr
      print(ptsCorr)

    def gen_prog(self,prgId=2,file=None,host=None,mode=0,**kwargs):
      '''
      kwargs:
        acq_per    : acquire period: acquire data all acq_per servo loops (default=1)
        pt2pt_time : time to move from one point to the next point
      '''
      prg=[]
      acq_per=kwargs.get('acq_per',1)
      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].ActPos","Motor[2].ActPos","Motor[3].ActPos"]
      channels=["Motor[1].ActPos","Motor[2].ActPos","Motor[3].ActPos","Motor[1].DesPos","Motor[2].DesPos","Motor[3].DesPos"]
      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))
      # this uses Coord[1].Tm and limits with MaxSpeed
      if mode==-1: #### jog a 10mm square
        pos=self.points
        prg.append('  linear abs')
        prg.append('X(%g) Y(%g)' % tuple(pos[0, :]))
        prg.append('dwell 10')
        prg.append('Gather.Enable=2')
        prg.append('jog2:10000')
        prg.append('dwell 100')
        prg.append('jog3:10000')
        prg.append('dwell 100')
        prg.append('jog2:-10000')
        prg.append('dwell 100')
        prg.append('jog3:-10000')
        prg.append('dwell 100')
        prg.append('Gather.Enable=0')
      elif mode==0: #### linear motion
        pos=self.points
        prg.append('  linear abs')
        prg.append('X(%g) Y(%g)' % tuple(pos[0, :]))
        prg.append('dwell 10')
        prg.append('Gather.Enable=2')
        prg.append('  linear abs')
        for idx in range(pos.shape[0]):
          prg.append('X%g Y%g'%tuple(pos[idx,:]))
        prg.append('dwell 100')
        prg.append('Gather.Enable=0')
      elif mode==1: #### pvt motion
        try:
          pt2pt_time=kwargs['pt2pt_time'] #how many ms to move to next point (pt2pt_time)
        except KeyError:
          print('missing pt2pt_time, use default=100ms')
          pt2pt_time=100.
        try:
          cnt=kwargs['cnt'] #move path multiple times
        except KeyError:
          cnt=1

        try:
          pt=self.ptsCorr
        except AttributeError:
          pt=self.points
        vel=pt[2:,:]-pt[:-2,:]
        #pv is an array of posx posy velx vely
        pv=np.ndarray(shape=(pt.shape[0]+2,4),dtype=pt.dtype)
        pv[:]=np.NaN
        #pv[     0,(0,1)]=2*pt[0,:]-pt[1,:]
        pv[     0,(0,1)]=pt[0,:]
        pv[  1:-1,(0,1)]=pt
        #pv[    -1,(0,1)]=2*pt[-1,:]-pt[-2,:]
        pv[    -1,(0,1)]=pt[-1,:]
        pv[(0,0,-1,-1),(2,3,2,3)]=0
        dist=pv[2:,(0,1)] - pv[:-2,(0,1)]
        pv[  1:-1,(2,3)] = 1000.*dist/(2.*pt2pt_time)

        prg.append('  linear abs')
        prg.append('X%g Y%g' % tuple(pv[0, (0,1)]))
        prg.append('dwell 10')
        prg.append('Gather.Enable=2')
        if cnt>1:
          prg.append('P100=%d'%cnt)
          prg.append('N100:')
        prg.append('  pvt%g abs'%pt2pt_time) #100ms to next position
        for idx in range(1,pv.shape[0]):
          prg.append('X%g:%g Y%g:%g'%tuple(pv[idx,(0,2,1,3)]))
        prg.append('X%g Y%g' % tuple(pv[-1, (0,1)]))
        if cnt>1:
          prg.append('dwell 10')
          prg.append('P100=P100-1')
          prg.append('if(P100>0)')
          prg.append('{')
          prg.append('  linear abs')
          prg.append('X%g Y%g' % tuple(pv[0, (0,1)]))
          prg.append('dwell 100')
          prg.append('goto 100')
          prg.append('}')
        else:
          prg.append('dwell 1000')
        prg.append('Gather.Enable=0')
      elif mode==2: #### spline motion
        try:
          pt2pt_time=kwargs['pt2pt_time'] #how many ms to move to next point (pt2pt_time)
        except KeyError:
          print('missing pt2pt_time, use default=100ms')
          pt2pt_time=100.
        pos=self.points
        pcor=np.ndarray(pos.shape,dtype=pos.dtype);pcor[:]=np.NaN
        pcor[(0,-1),:]=pos[(0,-1),:]
        pcor[1:-1,:]=(-pos[0:-2,:]+8*pos[1:-1,:]-pos[2:,:])/6.
        #pcor=pos
        prg.append('  linear abs')
        prg.append('X(%g) Y(%g)' % tuple(pcor[0, :]))
        prg.append('dwell 10')
        prg.append('Gather.Enable=2')
        prg.append('  spline%g abs'%pt2pt_time) #100ms to next position
        for idx in range(pcor.shape[0]):
          prg.append('X%g Y%g'%tuple(pcor[idx,:]))
        prg.append('dwell 100')
        prg.append('Gather.Enable=0')

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

      if file is not None:
        fh=open(file,'w')
        fh.write('\n'.join(prg))
        fh.close()
        if host is not None:
          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()
          #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()
      self.prg=prg


    def sort_points(self):
      pts=self.points
      verb=self.args.verbose
      #if verb&2:
      #  self.plot_points(pts)

      cnt=pts.shape[0]
      idx=np.ndarray(cnt,dtype=np.int32)
      grp_cnt=int(np.sqrt(cnt))
      grp_sz=int(np.ceil(float(cnt)/grp_cnt))
      if self.args.xy==True:
        idxA=1;idxB=0
      else:
        idxA=0;idxB=1

      #sort points along idxA
      pts=pts[pts[:,idxA].argsort()]
      #group sorting along idxB
      for i in range(grp_cnt):
        a=i*grp_sz
        #print a,a+grp_sz
        if i%2:
          idx[a:a+grp_sz]=a+pts[a:a+grp_sz,idxB].argsort()[::-1]
        else:
          idx[a:a+grp_sz]=a+pts[a:a+grp_sz,idxB].argsort()
      #print(idx)
      pts=pts[idx]

      if verb&2:
        self.plot_points(pts)
      plt.show()
      self.points=pts

    @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_points(self,pts):
      fig=plt.figure()
      ax = fig.add_subplot(1,1,1)
      #hl=ax[0].plot(x, y, color=col)
      hl=ax.plot(pts[:,0],pts[:,1],'r.')
      hl=ax.plot(pts[:,0],pts[:,1],'y--')
      cid = fig.canvas.mpl_connect('button_press_event', self.onclick)
      fig.obj=self
      plt.axis('equal')
      self.ax=ax
      self.hl=hl

    def plot_gather(self,fnOut='/tmp/shapepath.npz',fnLoc='/tmp/gather.txt'):
      meta=self.meta
      pts=self.points    # X,Y array
      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,x.ActPos,y.ActPos,rot.DesPos,x.DesPos,y.DesPos
      #idx     0        1        2          3        4        5
      ofsy=-rec[0,4]+pts[0,0]
      ofsx=-rec[0,5]+pts[0,1]
      rec[:,(1,4)]+=ofsy
      rec[:,(2,5)]+=ofsx
      if fnOut:
        # time base:
        # Gather.Period=10
        # Sys.ServoPeriod=0.2 -> 5kHz
        # -> aquired data all 10*0.2ms= 2 ms -> 30000 points = 60 sec

        np.savez_compressed(fnOut, rec=rec, pts=pts, meta=meta)
      fig=plt.figure()
      ax = fig.add_subplot(1,1,1)
      #hl=ax[0].plot(x, y, color=col)
      hl=ax.plot(pts[:,0],pts[:,1],'r.')
      hl=ax.plot(pts[:,0],pts[:,1],'y--')
      hl = ax.plot(rec[:, 4], rec[:, 5], 'b-') # desired path
      hl=ax.plot(rec[:,1],rec[:,2],'g-') # actual path
      ax.xaxis.set_label_text('x-pos um')
      ax.yaxis.set_label_text('y-pos um')
      cid = fig.canvas.mpl_connect('button_press_event', self.onclick)
      fig.obj=self
      self.ax=ax
      self.hl=hl

      fig = plt.figure()
      ax = fig.add_subplot(1, 1, 1)
      errx=rec[:,1]-rec[:,4]
      erry=rec[:,2]-rec[:,5]
      err=np.sqrt(errx**2+erry**2)

      hl = []
      hl += ax.plot(errx, 'b-',label='x-error')
      hl += ax.plot(erry, 'g-',label='y-error')
      hl += ax.plot(err, 'r-',label='error')
      ax.xaxis.set_label_text('datapoint (timebase: %g ms per data point)'%meta['timebase'])
      ax.yaxis.set_label_text('pos-error um')
      legend = ax.legend(loc='upper right', shadow=True)
      print('average error x %g um, y %g um, %g um'%(np.abs(errx).mean(),np.abs(erry).mean(),err.mean()))
      plt.show()

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=('-n',
                '-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)
    parser.add_option('-n', '--dryrun', action='store_true', help='dryrun to stdout')
    parser.add_option('--xy', action='store_true', help='sort x,y instead y,x')
    parser.add_option('--cfg', help='config file containing json configuration structure')

    (args, other)=parser.parse_args()
    args.other=other

    sp=ShapePath(args)
    sp.run()
#------------------ Main Code ----------------------------------
  #ssh_test()
  ret=parse_args()
  exit(ret)
