#!/usr/bin/env python
# *-----------------------------------------------------------------------*
# |                                                                       |
# |  Copyright (c) 2017 by Paul Scherrer Institute (http://www.psi.ch)    |
# |                                                                       |
# |              Author Thierry Zamofing (thierry.zamofing@psi.ch)        |
# *-----------------------------------------------------------------------*
'''
tools to setup and execute a helical scan of a cristal

motors CX CZ RY FY
        7  8  1  2
Mot 1: Rotation stage LS Mecapion MDM-DC06DNC0H    32 poles = 1 rev = 16*2048=32768 phase_step
Mot 2: Stage Y Parker MX80L D11 25mm               one pole cycle = 13mm = 2048 phase_step
Mot 3: Stage X Parker MX80L D11 25mm               one pole cycle = 13mm = 2048 phase_step
Mot/Enc 4: camera base plate X
Mot/Enc 5: camera base plate Y
Mot 6: Backlight                                2.3A
Mot 7: Stada Stepper:                           670mA 200 poles 1 rev = 100*2048 phase_step (2 stepper motor)
Mot 8: Stada Stepper:                           670mA 200 poles 1 rev = 100*2048 phase_step (2 stepper motor)

verbose bits:
  #1 basic info
  #2 plot sorting steps
  4 list program
  #4 upload progress
  #8 plot gather path



'''

import os, sys, json
import numpy as np
import matplotlib as mpl
import matplotlib.pyplot as plt
import mpl_toolkits.mplot3d as plt3d
import matplotlib.animation as anim
from matplotlib.widgets import Slider
import subprocess as sprc
from utilities import *

d2r=2*np.pi/360


#ax.set_xlabel('Z');ax.set_ylabel('X');ax.set_zlabel('Y')
#plot coordinates: X Y Z
#data              Z X Y

class Trf:
  #https://stackoverflow.com/questions/6802577/python-rotation-of-3d-vector
  @staticmethod
  def rotZ(rad):
    """ Return matrix for rotating about the z-axis by 'radians' radians """
    c = np.cos(rad)
    s = np.sin(rad)
    m=np.identity(4)
    m[0:2,0:2]=((c, -s),(s, c))
    return m

  @staticmethod
  def rotY(rad):
    """ Return matrix for rotating about the z-axis by 'radians' radians """
    c = np.cos(rad)
    s = np.sin(rad)
    m=np.identity(4)
    m[np.ix_((0,2),(0,2))]=((c, s),(-s, c))
    return m

  @staticmethod
  def trans(x,y,z):
    m=np.identity(4)
    m[0:3, 3] =(x,y,z)
    return m

class HelicalScan:
  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 = {"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": ['test_find_rot_ctr()']}
      #cfg = {"sequencer": ['test_find_rot_ctr(n=5. ,per=1.,bias=2.31,ampl=4.12,phi=24.6)']}
      cfg = {"sequencer": ['test_coord_trf()']}

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

  def sequencer(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)


  @staticmethod
  def test_find_rot_ctr(n=3.,per=1.,bias=4.1,ampl=2.4,phi=37):
    # find the rotation center, amplitude out of n (niminum 3) measurements
    # n    number of equidistant measurements
    # per  number of periods (full rotation of all measurements nut be a interger value for precise measurements)
    # phi  phase
    # bias bias value
    # ampl amplitude

    t = np.arange(n)
    w=2*np.pi*per/n*t
    y=ampl*np.cos(w+phi*d2r)+bias
    plt.figure(1)
    plt.subplot(311)
    plt.plot(t,y,'b.-')

    plt.subplot(312)
    f = np.fft.fft(y)
    plt.step(t, f.real,'b.-', t, f.imag,'r.-', where='mid')

    (bias,ampl,phase)=HelicalScan.meas_rot_ctr(y, per)
    print('bias: '+str(bias))
    print('amplitude: '+str(ampl))
    print('phase: '+str(phase*360./2/np.pi))

    plt.subplot(313)
    t2 = np.linspace(0,2*np.pi,64)
    y2=ampl*np.cos(t2+phase)+bias
    plt.plot(t2,y2,'g-')
    plt.stem(w,y,'b-')


    plt.show()
    pass


  def test_coord_trf(self):
      self.calcParam()
      param = self.param
      dx, dz, w, y,  = (0.2,0.3,0.1,3.3)
      print 'input  : dx:%.3g dz:%.3g w:%.3g fy:%.3g' % (dx,dz,w/d2r,y)
      (cx,cz,w,fy) = self.inv_transform(dx,dz,w,y)
      print 'inv_trf: cx:%.3g cz:%.3g w:%.3g fy:%.3g' % (cx,cz,w/d2r,fy)
      (dx,dz,w,y) = self.fwd_transform(cx,cz,w,fy)
      print 'fwd_trf: dx:%.3g dz:%.3g w:%.3g fy:%.3g' % (dx,dz,w/d2r,y)

      # plt.ion()
      # fig = plt.figure()

      # a=anim.FuncAnimation(fig,self.my_anim_func3,100,fargs=(horig,pt),interval=20,blit=False)
      # plt.show()

  #  def my_anim_func3(self,idx):
  #    self.hCrist,pt=self.pltCrist(cx=0,ty=0,cz=0,w=10*idx*d2r,h=self.hCrist)

  def interactive_cx_cz_w_fy(self):
    fig = plt.figure()
    self.manip=False#True#False
    self.ax=ax=plt3d.Axes3D(fig,[0.02, 0.15, 0.96, 0.83])
    ax.set_xlabel('Z');ax.set_ylabel('X');ax.set_zlabel('Y')
    ax.view_init(elev=14., azim=10)
    self.calcParam()
    param=self.param
    # param[i]=(z_i, y_i, x_i, r_i,phi_i)
    ctr=param[:,0:3].mean(0)[::-1]
    self.axSetCenter(ctr,10)

    axCx=plt.axes([0.1, 0.01, 0.8, 0.02])
    axCz=plt.axes([0.1, 0.04, 0.8, 0.02])
    axW =plt.axes([0.1, 0.07, 0.8, 0.02])
    axFy=plt.axes([0.1, 0.10, 0.8, 0.02])
    if self.manip:
      lz=ax.get_xlim()
      lx=ax.get_ylim()
      ly=ax.get_zlim()
    else:
      lx = ly=lz=[-5,5]
    self.sldCx=sCx=Slider(axCx, 'cx', lx[0], lx[1], valinit=(lx[0]+lx[1])/2.)
    self.sldCz=sCz=Slider(axCz, 'cz', lz[0], lz[1], valinit=(lz[0]+lz[1])/2.)
    self.sldW =sW =Slider(axW, 'ang', -180., 180.0, valinit=0)
    self.sldFy=sFy=Slider(axFy, 'fy', ly[0], ly[1], valinit=(ly[0]+ly[1])/2.)
    sCx.on_changed(self.update_cx_cz_w_fy)
    sCz.on_changed(self.update_cx_cz_w_fy)
    sW.on_changed(self.update_cx_cz_w_fy)
    sFy.on_changed(self.update_cx_cz_w_fy)

    hCrist,pt=self.pltCrist()

    self.hCrist=hCrist;self.fig=fig
    # param[i]=(z_i, y_i, x_i, r_i,phi_i)
    p=np.ndarray((param.shape[0], 3))
    for i in range(2):
      (z_i, y_i, x_i, r_i, phi_i)=param[i]
      p[i,0]=x_i+r_i*np.sin(phi_i) # x= x_i+r_i*cos(phi_i+w)+cx
      p[i,1]=y_i # y= y_i
      p[i,2]=z_i+r_i*np.cos(phi_i)  # z= z_i+r_i*sin(phi_i*w)
    print p
    ofs=(p[1]+p[0])/2. # = center of the cristal

    m=Trf.trans(*ofs); self.hOrig=self.pltOrig(m)
    plt.show()


  def update_cx_cz_w_fy(self,val):
    cx = self.sldCx.val
    cz = self.sldCz.val
    w  = self.sldW.val
    fy = self.sldFy.val
    if self.manip:
      param = self.param
      # param[i]=(z_i, y_i, x_i, r_i,phi_i)
      p = np.ndarray((param.shape[0], 3))
      for i in range(2):
        (z_i, y_i, x_i, r_i, phi_i) = param[i]
        p[i, 0] = x_i + r_i * np.cos(phi_i)  # x= x_i+r_i*cos(phi_i+w)+cx
        p[i, 1] = y_i  # y= y_i
        p[i, 2] = z_i + r_i * np.sin(phi_i)  # z= z_i+r_i*sin(phi_i*w)
      print p
      ofs = (p[1] + p[0]) / 2.  # = center of the cristal

      m = Trf.trans(cx,fy,cz)
      m= m.dot(Trf.rotY(w*d2r))
      self.hOrig = self.pltOrig(m,self.hOrig)
    else:
      self.hCrist,pt=self.pltCrist(fy,cx,cz,w*d2r,self.hCrist)
    #l.set_ydata(amp * np.sin(2 * np.pi * freq * t))
    self.fig.canvas.draw_idle()


  def interactive_dx_dz_w_y(self):
    fig = plt.figure()
    self.ax=ax=plt3d.Axes3D(fig,[0.02, 0.15, 0.96, 0.83])
    ax.set_xlabel('Z');ax.set_ylabel('X');ax.set_zlabel('Y')
    ax.view_init(elev=14., azim=10)
    self.calcParam()
    param=self.param

    # param[i]=(z_i, y_i, x_i, r_i,phi_i)
    ctr=(0,0,0)
    self.axSetCenter(ctr,10)

    axDx=plt.axes([0.1, 0.01, 0.8, 0.02])
    axDz=plt.axes([0.1, 0.04, 0.8, 0.02])
    axW =plt.axes([0.1, 0.07, 0.8, 0.02])
    axY =plt.axes([0.1, 0.10, 0.8, 0.02])

    lx=[-1,1];ly=[0,1];lz=[-1,1]
    ly = param[:,1]
    self.sldDx=sDx=Slider(axDx, 'dx', lx[0], lx[1], valinit=(lx[0]+lx[1])/2.)
    self.sldDz=sDz=Slider(axDz, 'dz', lz[0], lz[1], valinit=(lz[0]+lz[1])/2.)
    self.sldW =sW =Slider(axW, 'ang', -180., 180.0, valinit=0)
    self.sldY =sY =Slider(axY, 'y', ly[0], ly[1], valinit=(ly[0]+ly[1])/2.)
    sDx.on_changed(self.update_dx_dz_w_y)
    sDz.on_changed(self.update_dx_dz_w_y)
    sW.on_changed(self.update_dx_dz_w_y)
    sY.on_changed(self.update_dx_dz_w_y)


    # param[i]=(z_i, y_i, x_i, r_i,phi_i)
    p=np.ndarray((param.shape[0], 3))
    for i in range(2):
      (z_i, y_i, x_i, r_i, phi_i)=param[i]
      p[i,0]=x_i+r_i*np.sin(phi_i) # x= x_i+r_i*cos(phi_i+w)+cx
      p[i,1]=y_i # y= y_i
      p[i,2]=z_i+r_i*np.cos(phi_i)  # z= z_i+r_i*sin(phi_i*w)
    ofs=(p[1]+p[0])/2. # = center of the cristal
    print 'p, ofs',p,ofs

    m=Trf.trans(0,0,0); self.hOrig=self.pltOrig(m)
    hCrist,pt=self.pltCrist(cx=-ofs[0],fy=-ofs[1],cz=-ofs[2])

    self.hCrist=hCrist;self.fig=fig
    plt.show()


  def update_dx_dz_w_y(self,val):
    dx = self.sldDx.val
    dz = self.sldDz.val
    w = self.sldW.val
    y = self.sldY.val
    w=w*d2r
    (cx,cz,w,fy)=self.inv_transform(dx,dz,w,y)
    #print (cx,cz,w,fy)
    self.hCrist,pt=self.pltCrist(-fy,-cx,-cz,w,self.hCrist)
    self.fig.canvas.draw_idle()


  def fwd_transform(self,cx,cz,w,fy):
    #cx,cy: coarse stage
    #input:  cx,cz,w,fy
    #output: dx,dz,w,y
    # param[i]=(z_i, y_i, x_i, r_i,phi_i)

    param=self.param
    p=np.ndarray((param.shape[0], 3))
    for i in range(2):
      (z_i, y_i, x_i, r_i, phi_i)=param[i]
      p[i,0]=x_i+r_i*np.sin(phi_i+w) # x= x_i+r_i*cos(phi_i+w)+cx
      p[i,1]=y_i # y= y_i
      p[i,2]=z_i+r_i*np.cos(phi_i+w)  # z= z_i+r_i*sin(phi_i*w)
    v=p[1]-p[0]
    #for y = 0..1:
    #v=v*y
    #for y = y_0..y_1:
    y_0=param[0,1];y_1=param[1,1];v=v*(fy-y_0)/(y_1-y_0)

    v=v+p[0]
    dx=cx-v[0]
    dz=cz-v[2]
    y=v[1]
    res=(dx,dz,w,y)
    return res


  def inv_transform(self,dx,dz,w,y):
    #input:  dx,dz,w,y
    #output: cx,cz,w,fy
    #dx,dy: deviation from cristal center line
    param=self.param
    p=np.ndarray((param.shape[0], 3))
    for i in range(2):
      (z_i, y_i, x_i, r_i, phi_i)=param[i]
      p[i,0]=x_i+r_i*np.sin(phi_i+w) # x= x_i+r_i*cos(phi_i+w)+cx
      p[i,1]=y_i # y= y_i
      p[i,2]=z_i+r_i*np.cos(phi_i+w)  # z= z_i+r_i*sin(phi_i*w)
    v=p[1]-p[0]
    #for y = 0..1:
    #v=v*y
    #for y = y_0..y_1:
    y_0=param[0,1];y_1=param[1,1];v=v*(y-y_0)/(y_1-y_0)

    v=v+p[0]
    cx=dx+v[0]
    cz=dz+v[2]
    fy=v[1]
    res=(cx,cz,w,fy)
    return res


  def calcParam(self):
    n = 3.;
    per = 1.;
    w = 2 * np.pi * per / n * np.arange(n)
    p = ((2.3, .71, 4.12, 10.6 * d2r),(6.2, .45, 3.2, 45.28 * d2r))  # (y, bias, ampl, phi)
    self.param = param = np.ndarray((len(p), 5))
    z = 14.5  # fix z position
    for i in range(2):
      (y, bias, ampl, phi) = p[i]
      x = ampl * np.cos(w + phi) + bias
      print('yMeas_%d=' % i + str(y) + ' xMeas_%d=' % i + str(x))
      # param[i]=(z_i, y_i, x_i, r_i,phi_i)
      param[i, 0] = z
      param[i, 1] = y
      param[i, 2:] = HelicalScan.meas_rot_ctr(x)  # (bias,ampl,phase)
      (bias, ampl, phase) = param[i][2:]
    print param


  def pltOrig(self,m,h=None):
    ax=self.ax
    # m is a 4x4 matrix. the transformed matrix
    r=m[:3,0] #1st
    g=m[:3,1] #2nd
    b=m[:3,2] #3rd
    o=m[:3,3] #origin
    if h is None:
      hr=ax.plot((o[2],o[2]+r[2]), (o[0],o[0]+r[0]), (o[1],o[1]+r[1]), 'r')
      hg=ax.plot((o[2],o[2]+g[2]), (o[0],o[0]+g[0]), (o[1],o[1]+g[1]), 'g')
      hb=ax.plot((o[2],o[2]+b[2]), (o[0],o[0]+b[0]), (o[1],o[1]+b[1]), 'b')
      return hr + hg + hb  # this is a list
    else:
      hr, hg, hb = h
      hr.set_data((o[2], o[2] + r[2]), (o[0], o[0] + r[0]))
      hr.set_3d_properties((o[1], o[1] + r[1]))
      hg.set_data((o[2], o[2] + g[2]), (o[0], o[0] + g[0]))
      hg.set_3d_properties((o[1], o[1] + g[1]))
      hb.set_data((o[2], o[2] + b[2]), (o[0], o[0] + b[0]))
      hb.set_3d_properties((o[1], o[1] + b[1]))
      return h


  def pltCrist(self,fy=0,cx=0,cz=0,w=0,h=None):
    #h are the handles
    if h is None:
      h=[] #handels
      ax=self.ax
      param=self.param
      pt = np.ndarray((4, 3))
      for i in range(2):
        (z, y, x, r, phi) = param[i]
        x+=cx;y+=fy;z+=cz;phi+=w
        pt[i] = (x, y, z)
        pt[i + 2] = (x + r * np.sin(phi), y, z + r * np.cos(phi))
        obj = mpl.patches.Circle((z, x), r, facecolor=mpl.colors.colorConverter.to_rgba('r', alpha=0.3))
        h1=ax.add_patch(obj)
        h2=plt3d.art3d.pathpatch_2d_to_3d(obj, z=y, zdir="z")
        h.append(obj)
      #hs=ax.scatter(pt[:, 2], pt[:, 0], pt[:, 1])
      hs=ax.plot(pt[:, 2], pt[:, 0], pt[:, 1],'.')
      hp=ax.plot(pt[2:, 2], pt[2:, 0], pt[2:, 1])
      h+=(hs[0],hp[0])
    else:
      ax=self.ax
      param=self.param
      pt = np.ndarray((4, 3))
      for i in range(2):
        (z, y, x, r, phi) = param[i]
        x+=cx;y+=fy;z+=cz;phi+=w
        pt[i] = (x, y, z)
        pt[i + 2] = (x + r * np.sin(phi), y, z + r * np.cos(phi))
        h[i].remove()
        obj = mpl.patches.Circle((z, x), r, facecolor=mpl.colors.colorConverter.to_rgba('r', alpha=0.3))
        ax.add_patch(obj)
        plt3d.art3d.pathpatch_2d_to_3d(obj, z=y, zdir="z")
        h[i]=obj
      h[2].set_data(pt[:, 2], pt[:, 0])#, pt[:, 1]))
      h[2].set_3d_properties(pt[:, 1])

      h[3].set_data(pt[2:, 2], pt[2:, 0])#, pt[:, 1]))
      h[3].set_3d_properties(pt[2:, 1])
      #hr.set_data((o[2], o[2] + r[2]), (o[0], o[0] + r[0]))
      #hr.set_3d_properties((o[1], o[1] + r[1]))

      #h[3].set_3d_properties(zs=pt[:, 1]))
      #h[4].set_data((pt[2:, 2], pt[2:, 0]))#, pt[2:, 1]))
      #hp=ax.plot(pt[2:, 2], pt[2:, 0], pt[2:, 1], label='zs=0, zdir=z')
      #h+=(hs,hp[0])
    return (h,pt)


  @staticmethod
  def meas_rot_ctr(y,per=1):
    # find the amplitude bias and phase of an equidistant sampled sinus
    # it needs at least 3 measurements e.g. at 0,120 240 deg or 0 90 180 270 deg
    # per is the number of persiods, default is 1 period =360 deg
    n=len(y)
    f = np.fft.fft(y)
    idx=int(per)
    bias=np.absolute(f[0]/n)
    phase=np.angle(f[idx])
    ampl=np.absolute(f[idx])*2/n
    return (bias,ampl,phase)


  def axSetCenter(self,v,l):
    ax=self.ax
    #v=center vector, l= length of each axis
    l2=l/2.
    ax.set_xlim(v[2]-l2, v[2]+l2);
    ax.set_ylim(v[0]-l2, v[0]+l2);
    ax.set_zlim(v[1]-l2, v[1]+l2)


  def gen_coord_trf_code(self,file=None,host=None):
    param=self.param
    prg = []
    prg.append('''
// Set the motors as inverse kinematic axes in CS 1
//motors CX CZ RY FY
//        7  8  1  2

&1
#7->I
#8->I
#1->I
#2->I

open forward
  define(qCX='L7', qCZ='L8', qW='L1', qFY='L2')
  define(DX='C6', DZ='C8', W='C1', Y='C7')
  //coord    X        Z       B       Y
  define(p0_x='L10', p0_y='L11', p0_z='L12')
  define(p1_x='L13', p1_y='L14', p1_z='L15')
  define(scale='L16')
  send 1"forward kinematic %f %f %f %f\\n",qCX,qCZ,qW,qFY''')
    for i in range(2):
      #https://stackoverflow.com/questions/3471999/how-do-i-merge-two-lists-into-a-single-list
      l=[j for i in zip((i,) * param.shape[1], list(param[i])) for j in i]
      prg.append("  define(z_%i=%g, y_%i=%g, x_%i=%g, r_%i=%g, phi_%i=%g)"%tuple(l))
    prg.append("  W=qW")
    prg.append("  qW=qW*%g"%(d2r/1000.)) #scale from 1000*deg to rad
    prg.append('''
  p0_x=x_0+r_0*sin(phi_0+qW)
  p1_x=x_1+r_1*sin(phi_1+qW)
  p0_y=y_0
  p1_y=y_1
  p0_z=z_0+r_0*cos(phi_0+qW)
  p1_z=z_1+r_1*cos(phi_1+qW)

  scale=(qFY-y_0)/(y_1-y_0)
  p0_x=p0_x+scale*(p1_x-p0_x)
  p0_y=p0_y+scale*(p1_y-p0_y)
  p0_z=p0_z+scale*(p1_z-p0_z)
  DX=qCX-p0_x
  DZ=qCZ-p0_z
  Y=qFY
  send 1"forward result    %f %f %f %f\\n",DX,DZ,W,Y

  D0=$000001c2; //B=$2 X=$40 Y=$80 Z=$100 hex(2+int('40',16)+int('80',16)+int('100',16)) -> 0x1c2
close
''')

    prg.append('''
open inverse
  define(DX='C6', DZ='C8', W='C1', Y='C7')
  //coord    X        Z       B       Y
  //D0 is set to $000001c2
  define(qCX='L7', qCZ='L8', qW='L1', qFY='L2')
  define(p0_x='L10', p0_y='L11', p0_z='L12')
  define(p1_x='L13', p1_y='L14', p1_z='L15')
  define(scale='L16')
  send 1"inverse kinematic %f %f %f %f\\n",DX,DZ,W,Y''')
    for i in range(2):
      # https://stackoverflow.com/questions/3471999/how-do-i-merge-two-lists-into-a-single-list
      l = [j for i in zip((i,) * param.shape[1], list(param[i])) for j in i]
      prg.append("  define(z_%i=%g, y_%i=%g, x_%i=%g, r_%i=%g, phi_%i=%g)" % tuple(l))
    prg.append("  qW=W")
    prg.append("  W=W*%g"%(d2r/1000.)) #scale from 1000*deg to rad
    prg.append('''
  p0_x=x_0+r_0*sin(phi_0+W)
  p1_x=x_1+r_1*sin(phi_1+W)
  p0_y=y_0
  p1_y=y_1
  p0_z=z_0+r_0*cos(phi_0+W)
  p1_z=z_1+r_1*cos(phi_1+W)

  scale=(qFY-y_0)/(y_1-y_0)
  p0_x=p0_x+scale*(p1_x-p0_x)
  p0_y=p0_y+scale*(p1_y-p0_y)
  p0_z=p0_z+scale*(p1_z-p0_z)
  qCX=DX+p0_x
  qCZ=DZ+p0_z
  qFY=Y
  send 1"inverse result    %f %f %f %f\\n",qCX,qCZ,qW,qFY

close
''')

    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 = '/home/zamofing_t/scripts/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

    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[7].ActPos","Motor[8].ActPos","Motor[1].ActPos","Motor[2].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))
      # 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


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

    hs=HelicalScan(args)
    #hs.sequencer()
    hs.args.verbose=255;hs.calcParam();hs.gen_coord_trf_code('/tmp/helicalscan.cfg','MOTTEST-CPPM-CRM0485')
    hs.test_coord_trf()

    return
    hs.test_find_rot_ctr()
    hs.test_find_rot_ctr(n=5. ,per=1.,bias=2.31,ampl=4.12,phi=24.6)
    hs.interactive_cx_cz_w_fy()
    hs.interactive_dx_dz_w_y()


  #------------------ Main Code ----------------------------------
  #ssh_test()
  ret=parse_args()
  exit(ret)
