#!/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,re
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 *
import telnetlib

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):
      param = self.param
      cx, cz, w, fy,  = (0.2,0.3,0.1,0.4)
      #cx, cz, w, fy,  = (10.,20,3.,40)
      print 'input  : cx:%.6g cz:%.6g w:%.6g fy:%.6g' % (cx,cz,w/d2r*1000.,fy)
      (dx,dz,w,y) = self.fwd_transform(cx,cz,w,fy)
      print 'fwd_trf: dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx,dz,w/d2r*1000.,y)
      (cx,cz,w,fy) = self.inv_transform(dx,dz,w,y)
      print 'inv_trf: cx:%.6g cz:%.6g w:%.6g fy:%.6g' % (cx,cz,w/d2r*1000.,fy)

      dx, dz, w, y,  = (0.2,0.3,0.1,0.4)
      #dx, dz, w, y,  = (10.,20,3.,40)
      print 'input  : dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx,dz,w/d2r*1000.,y)
      (cx,cz,w,fy) = self.inv_transform(dx,dz,w,y)
      print 'inv_trf: cx:%.6g cz:%.6g w:%.6g fy:%.6g' % (cx,cz,w/d2r*1000.,fy)
      (dx,dz,w,y) = self.fwd_transform(cx,cz,w,fy)
      print 'fwd_trf: dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx,dz,w/d2r*1000.,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 mpl_test(self):
    plt.ion()
    fig = plt.figure()
    ax = fig.add_axes([.2, .05, .75, .90])
    x=np.arange(0,2*np.pi,.1)
    y=np.sin(x)
    h=ax.plot(x,y)
    # ax.set_position([.2,.05,.75,.90])
    #ax2 = ax.twinx()
    ax2 = fig.add_axes([.2, .05, .75, .90],sharex=ax)
    ax2.set_position([.1,.05,.75,.90])

    ax2.spines['top'].set_visible(False)
    ax2.spines['bottom'].set_visible(False)

    ax2.xaxis.set_visible(False)
    ax2.patch.set_visible(False)

    y2=y+.1*np.random.random(y.shape)
    h+=ax2.plot(x,y2,'g')
    ax2.set_position(ax.get_position())
    ax2.set_ylabel('mylbl', color='r')
    ax2.tick_params(axis='y', colors='b')
    ax2.spines['left'].set_position(('axes',-.1))
    plt.legend(handels=h)
    pass

  def show_vel(self):
    rec=self.rec
    fig = plt.figure()
    #y = np.diff(rec[:, 2])
    y = np.diff(rec,axis=0)
    mx=y.max(0)
    mn=y.min(0)
    scl=np.maximum(mx,-mn)
    scl=np.where(scl==0, 1, scl) # replace 0 by 1
    y/=scl
    x = np.arange(0, y.shape[0]);
    x= x.reshape(-1,1).dot(np.ones((1,y.shape[1])))
    lbl=('cx','cz','w','fy')
    #plt.plot(x, y,label=lbl)
    for i in range(y.shape[1]):
      plt.plot(x[:,i], y[:,i],label=lbl[i])

    plt.legend()

  def show_pos(self):
    rec=self.rec
    y = rec
    #plt.ion()
    fig = plt.figure(figsize=(20,6))
    c='bgrc'
    lbl=('cx','cz','w','fy')
    dx=.25/len(lbl)
    for i in range(rec.shape[1]):
      if i==0:
        ax = fig.add_axes([.2, .05, .75, .90])
        axl =[ax,]
      else:
        ax = fig.add_axes(axl[0].get_position(), sharex=ax)
        ax.spines['top'].set_visible(False)
        ax.spines['bottom'].set_visible(False)
        ax.xaxis.set_visible(False)
        ax.patch.set_visible(False)
        ax.spines['left'].set_position(('axes', -dx*i))
        axl.append(ax)
      ax.set_ylabel(lbl[i], color=c[i])
      ax.tick_params(axis='y', colors=c[i])

    x = np.arange(0, y.shape[0]);
    h=[]
    for i in range(rec.shape[1]):
      h+=axl[i].plot(x, y[:,i],c[i],label=lbl[i])
    plt.legend(handles=h)
    cid = fig.canvas.mpl_connect('motion_notify_event', self.onmove)
    fig.obj=self
    fig.data=y


  @staticmethod
  def onmove(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
    data = event.canvas.figure.data
    if(event.xdata):
      idx=round(event.xdata)
      msg='%d: cx:%.3f cz:%.3f w:%.1f fy:%.3f'%((idx,)+tuple(data[idx,:]))
      #print msg
      event.canvas.toolbar.set_message(msg)

  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)
    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,param[0,3]+param[1,3])

    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(cx,cz,w*d2r,fy,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)
    param=self.param

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

    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(-cx,-cz,w,-fy,self.hCrist)
    self.fig.canvas.draw_idle()

  def interactive_anim(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)
    param=self.param

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

    axFrm=plt.axes([0.1, 0.01, 0.8, 0.02])

    lx=[-1,1];ly=[0,1];lz=[-1,1]
    ly = param[:,1]
    self.sldFrm=sFrm=Slider(axFrm, 'frm', 0, self.rec.shape[0]-1, valinit=0)
    sFrm.on_changed(self.update_anim)
    m=Trf.trans(0,0,0); self.hOrig=self.pltOrig(m)
    self.hCrist=None
    self.fig=fig

    animCnt=100
    self.step=self.rec.shape[0]/animCnt
    #a = anim.FuncAnimation(fig, self.anim_gather_data, animCnt, fargs=(), interval=20, repeat=False, blit=False)
    self.update_anim(0)
    plt.show()
    pass


  def update_anim(self,frm):
    (cx, cz, w, fy)=self.rec[int(frm),:]
    #data/=. #scale from um to mm
    w*=d2r/1000 # scale from deg to rad
    #print (cx,cz,w,fy)
    self.hCrist,pt=self.pltCrist(-cx,-cz,w,-fy,self.hCrist)
    self.fig.canvas.draw_idle()


  def anim_gather_data(self,idx):
    (cx, cz, w, fy)=self.rec[int(idx*self.step),:]
    w*=d2r/1000 # scale from deg to rad
    self.hCrist,pt=self.pltCrist(-cx,-cz,w,-fy,self.hCrist)
    #data=self.rec[int(idx*self.step),:]
    #self.hCrist,pt=self.pltCrist(*data,h=self.hCrist)

  def save_rec(self,fn_npz='/tmp/helicalscan.npz'):
    param= self.param
    meta= self.meta
    points = self.points  # target points
    rec = self.rec
    np.savez_compressed(fn_npz, rec=rec, points=points, param=param, meta=meta)

  def load_rec(self,fn_npz='/tmp/helicalscan.npz'):
    try:
      fh=np.load(fn_npz)
    except IOError as e:
      sys.stderr.write('Unable to open File: '+fn_npz+'\n')
    else:
      pass
    s='content of numpy file: '+fn_npz+'\n'
    for k,v in fh.iteritems():
      s+='   '+k+': '+str(v.dtype)+' '+str(v.shape)+'\n'
      setattr(self,k,v)
    print s

  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
    idx=(2,0,1)
    r=m[idx,0] #1st
    g=m[idx,1] #2nd
    b=m[idx,2] #3rd
    o=m[idx,3] #origin
    lines = np.ndarray((3, 2, 3))  # numlines, points, xyz
    lines[:, 0, :] = o
    lines[0, 1, :] = o + r
    lines[1, 1, :] = o + g
    lines[2, 1, :] = o + b
    if h is None:
      lseg = tuple(lines)
      col=('r','g','b')
      hlc = plt3d.art3d.Line3DCollection(lseg, colors=col, linewidths=2)  # , *args[argi:], **kwargs)
      ax.add_collection(hlc)
      return hlc
    else:
      h.set_segments(lines)
      return h


  def pltCrist(self,cx=0,cz=0,w=0,fy=0,h=None):
    #h are the handles
    ax = self.ax
    param = self.param
    pt = np.ndarray((4, 3))
    if h is None:
      h=[] #handels
      for i in range(2):
        (z, y, x, r, phi) = param[i]
        x+=cx;y+=fy;z+=cz;phi+=w
        pt[i] = (z, x, y)
        pt[i + 2] = (z + r * np.cos(phi), x + r * np.sin(phi), y)
        obj = mpl.patches.Circle((z, x), r, facecolor=mpl.colors.colorConverter.to_rgba('y', alpha=0.2))
        h1=ax.add_patch(obj)
        h2=plt3d.art3d.pathpatch_2d_to_3d(obj, z=y, zdir="z")
        #print h1._segment3d
        h.append(obj)
      #hs=ax.scatter(pt[:, 2], pt[:, 0], pt[:, 1])
      hs=ax.plot(pt[:, 0], pt[:, 1], pt[:, 2],'.')
      hp=ax.plot(pt[2:, 0], pt[2:, 1], pt[2:, 2])
      h+=(hs[0],hp[0])
      if hasattr(self,'points'):
        lines = self.get_meas_lines(pt,cx,cz,fy,w)
        p=tuple(lines[:,0,:].T)
        hl =plt3d.art3d.Line3D(*p,color='r',marker='.')#, *args[argi:], **kwargs)
        ax.add_artist(hl);h.append(hl)

        lseg=tuple(lines)
        col=(mpl.colors.colorConverter.to_rgba('r'),)*len(lseg)
        hlc=plt3d.art3d.Line3DCollection(lseg,colors=col)#, *args[argi:], **kwargs)
        ax.add_collection(hlc);h.append(hlc)
    else:
      for i in range(2):
        (z, y, x, r, phi) = param[i]
        x+=cx;y+=fy;z+=cz;phi+=w
        pt[i] = (z, x, y)
        pt[i + 2] = (z + r * np.cos(phi), x + r * np.sin(phi), y)
        h[i].remove()
        obj = mpl.patches.Circle((z, x), r, facecolor=mpl.colors.colorConverter.to_rgba('y', alpha=0.2))
        ax.add_patch(obj)
        plt3d.art3d.pathpatch_2d_to_3d(obj, z=y, zdir="z")
        h[i]=obj
      h[2].set_data(pt[:, 0], pt[:, 1])#, pt[:, 1]))
      h[2].set_3d_properties(pt[:, 2])

      h[3].set_data(pt[2:, 0], pt[2:, 1])#, pt[:, 1]))
      h[3].set_3d_properties(pt[2:, 2])
      if hasattr(self,'points'):
        lines = self.get_meas_lines(pt,cx,cz,fy,w)
        hl=h[4]
        #hl.set_data(lines[:,0,0], lines[:,0,1])  # , pt[:, 1]))
        #hl.set_3d_properties(lines[:,0,2])
        p=tuple(lines[:,0,:].T)
        hl._verts3d=p;hl.stale=True

        hlc=h[5]
        hlc.set_segments(lines)


    return (h,pt)

  def get_meas_lines(self,pt,cx,cz,fy,w):
    param = self.param
    pts = self.points
    dx_ = pts[:, 0] # add 0.2 to test
    dz_ = pts[:, 1] # add 0.2 to test
    w_ = pts[:, 2] * (d2r / 1000.)
    y_ = pts[:, 3]

    # self.inv_transform(self, dx, dz, w, y):

    f = (pts[:, 3] - param[0, 1]) / (param[1, 1] - param[0, 1])
    lines = np.ndarray((pts.shape[0], 2, 3))
    ofx=dx_*-np.sin(w-w_)+dz_*np.cos(w-w_)  # 0.2=dx
    ofy=dx_*np.cos(w-w_)+dz_*np.sin(w-w_)
    lines[:, 0, 0] = pt[2, 0] + (pt[3, 0] - pt[2, 0]) * f +ofx # z data
    lines[:, 0, 1] = pt[2, 1] + (pt[3, 1] - pt[2, 1]) * f +ofy # x data
    lines[:, 0, 2] = pts[:, 3] + fy  # y data
    lines[:, 1, 0] = lines[:, 0, 0] + np.cos(w-w_)*.1
    lines[:, 1, 1] = lines[:, 0, 1] + np.sin(w-w_)*.1
    lines[:, 1, 2] = lines[:, 0, 2]
    return lines

  @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):
    param=self.param
    prg = []
    prg.append('''
// Set the motors as inverse kinematic axes in CS 1
//motors CX CZ RY FY
//        4  5  3  1

&1
a
#4->0
#5->0
#3->0
#1->0

#4->I
#5->I
#3->I
#1->I

open forward
  define(qCX='L4', qCZ='L5', qW='L3', qFY='L1')
  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"fwd_inp(%f)  %f %f %f %f\\n",D0,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"fwd_res     %f %f %f %f\\n",DX,DZ,W,Y
  //P1001+=1
  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
  //coord     X          Z         B         Y
  define( DX='C6' ,  DZ='C8' ,  W='C1',   Y='C7')
  define(vDX='C38', vDZ='C40', vW='C33', vY='C39')

  //motor q4         q5         q3        q1
  define( qCX='L4',  qCZ='L5',  qW='L3',  qFY='L1')
  define(vqCX='R4', vqCZ='R5', vqW='R3', vqFY='R1')

  define(p0_x='L10', p0_y='L11', p0_z='L12')
  define(p1_x='L13', p1_y='L14', p1_z='L15')
  define(p_x='L16', p_y='L17', p_z='L18')
  define(sclY='L19')
  define(scl='L20')
  //if(D0>0)
  //  send 1"inv_inp(%f) %f:%f %f:%f %f:%f %f:%f\\n",D0,DX,vDX,DZ,vDZ,W,vW,Y,vY
  //else
  //  send 1"inv_inp(%f) %f %f %f %f\\n",D0,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)

  sclY=(Y-(y_0))/(y_1-(y_0))
  p_x=p0_x+sclY*(p1_x-p0_x)
  p_y=p0_y+sclY*(p1_y-p0_y)
  p_z=p0_z+sclY*(p1_z-p0_z)
  qCX=DX+p_x
  qCZ=DZ+p_z
  qFY=Y
  if(D0>0)
  { // calculate velocities for PVT motion''')
    prg.append("    vW=vW*%g"%(d2r/1000.)) #scale from 1000*deg to rad
    prg.append('''    p_x=sclY*(p1_x-p0_x)
    p_z=sclY*(p1_z-p0_z)
    vqFY=vY
    vqCX=vDX + (p1_x-p0_x)/(p1_y-p0_y)*vY //+ vqW*p_z
    vqCZ=vDZ + (p1_z-p0_z)/(p1_y-p0_y)*vY //+ vqW*p_x
    //vqW=vqW+(vqCX+(p1_x-p0_x)/(p1_y-p0_y)*vY)*p_z+(vqCZ+(p1_z-p0_z)/(p1_y-p0_y)*vY)*p_x
    vqW=vW//+((p1_x-p0_x)/(p1_y-p0_y)*vY)*p_z+((p1_z-p0_z)/(p1_y-p0_y)*vY*p_x
    ''')
    prg.append("    vqW=vqW*%g"%(1000./d2r)) #scale from rad to 1000*deg
    prg.append('''//    send 1"inv_res    %f:%f %f:%f %f:%f %f:%f\\n",qCX,vqCX,qCZ,vqCZ,qW,vqW,qFY,vqFY
  }
  //else
  //  send 1"inv_res    %f %f %f %f\\n",qCX,qCZ,qW,qFY
  //P1002+=1
close
''')

#    vqCX=vDX + (p1_x-p0_x)/(p1_y-p0_y)*vY + vqW*p_x
#    vDX is in same direction, so add as it is
#    (p1_x-p0_x)/(p1_y-p0_y)*vY velocity part in vqCX direction, when moving in vY
#    vqW*p_x velocity part of the rotation (vqW is in rad)

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


  def gen_prog(self,prgId=2,mode=0,**kwargs):
    '''
    kwargs:
      acq_per    : acquire period: acquire data all acq_per servo loops (default=1)
      mode=-1: test motion
      mode=0: #linear motion with 100 ms break at each measurement
        cnt     : move path multiple times
        cntVert : number of vertical measurements
        cntHor  : number of horizontal measurements
        hRng    : min, max horizontal boundaries
        wRng    : starting and ending angle
        yRng    : starting and ending height
      mode=1: #PVT motion
        pt2pt_time : time to move from one point to the next point
        cnt     : move path multiple times
        cntVert : number of vertical measurements
        cntHor  : number of horizontal measurements
        hRng    : min, max horizontal boundaries
        wRng    : starting and ending angle
        yRng    : starting and ending height
    '''
    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].ActPos","Motor[2].ActPos","Motor[3].ActPos"]
    #          CX                CZ                W                 FY
    channels=["Motor[4].ActPos","Motor[5].ActPos","Motor[3].ActPos","Motor[1].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==-1: #### jog all motors 10000um (or 10000 mdeg)
      pos=np.array([[0,0,0,0],])
      prg.append('  linear abs')
      #prg.append('X(%g) Z(%g) B(%g) Y(%g)' % tuple(pos[0, :]))
      prg.append('  jog1,2,7,8=0')
      prg.append('  dwell 10')
      prg.append('  Gather.Enable=2')
      prg.append('  jog7:10000')
      prg.append('  dwell 100')
      prg.append('  jog8:10000')
      prg.append('  dwell 100')
      prg.append('  jog1:10000')
      prg.append('  dwell 100')
      prg.append('  jog2:10000')
      prg.append('  dwell 100')
      prg.append('  jog1,2,7,8=0')
      prg.append('  dwell 100')
      prg.append('  Gather.Enable=0')

    # ******** mode 0 ********
    elif mode==0: #### linear motion
      #y=2.3 6.2 dx=0, dz=0  w=0..3600000 # 10 rev
      cnt= kwargs.get('cnt', 1) #move path multiple times
      cntVert = kwargs.get('cntVert', 12)
      cntHor = kwargs.get('cntHor', 4)
      hRng = kwargs.get('hRng', (-.2,.2))
      wRng = kwargs.get('wRng', (0,360000))
      yRng = kwargs.get('yRng', self.param[:,1])

      numPt=cntVert*cntHor
      pt=np.zeros((numPt,4))
      if cntHor>1:
        a=np.linspace(hRng[0],hRng[1],cntHor)
        a=np.append(a,a[::-1])
        a=np.tile(a,(cntVert+1)//2)
        pt[:,0]= a[0:pt.shape[0]]
      #pt[:,1]= np.linspace(0,.2,numPt) #dz
      pt[:,2]= np.linspace(wRng[0],wRng[1],numPt) #w
      pt[:,3]= np.repeat(np.linspace(yRng[0],yRng[1],cntVert),cntHor) #y
      self.points=pt
      #prg.append('  Coord[1].SegMoveTime=.05') #to calculate every 0.05 ms the inverse kinematics
      prg.append('  nofrax') #needed to set all axis to use AltFeedRate
      prg.append('  Coord[1].AltFeedRate=0') # allow maximum speed
      prg.append('  Coord[1].SegMoveTime=0') #turn off segmented mode
      prg.append('  linear abs')
      prg.append('  X%g Z%g B%g Y%g' % tuple(pt[0, :]))
      prg.append('  dwell 100')
      prg.append('  Gather.Enable=2')
      prg.append('  Coord[1].AltFeedRate=0') # allow maximum speed
      prg.append('  Coord[1].SegMoveTime=0') #to calculate every 1 ms the inverse kinematics
      prg.append('  P2000=0')
      for i in range(pt.shape[0]):
        prg.append('  X%g Z%g B%g Y%g' % tuple(pt[i, :]))
        #prg.append('  P2000=%d'%(i,))
        prg.append('  dwell 10')
      prg.append('  Gather.Enable=0')

    # ******** mode 1 ********
    elif mode==1: #### pvt motion
      #y=2.3 6.2 dx=0, dz=0  w=0..3600000 # 10 rev
      cnt= kwargs.get('cnt', 1) #move path multiple times
      cntVert = kwargs.get('cntVert', 12)
      cntHor = kwargs.get('cntHor', 4)
      hRng = kwargs.get('hRng', (-.2,.2))
      wRng = kwargs.get('wRng', (0,360000))
      yRng = kwargs.get('yRng', self.param[:,1])
      pt2pt_time = kwargs.get('pt2pt_time', 100)
      smt  = kwargs.get('smt', 1) # SegMoveTime, default = 1ms -> velocity calc not yet 100% correct (smt=0 not 100% working)
      numPt=cntVert*cntHor
      pt=np.zeros((numPt,4))
      if cntHor>1:
        a=np.linspace(hRng[0],hRng[1],cntHor)
        a=np.append(a,a[::-1])
        a=np.tile(a,(cntVert+1)//2)
        pt[:,0]= a[0:pt.shape[0]]
      #pt[:,1]= np.linspace(0,.2,numPt) #dz
      pt[:,2]= np.linspace(wRng[0],wRng[1],numPt) #w
      pt[:,3]= np.repeat(np.linspace(yRng[0],yRng[1],cntVert),cntHor) #y
      self.points=pt
      #pv is an array of dx,dz,w,y,vel_dx,vel_dz,vel_w,vel_y
      pv=np.ndarray(shape=(pt.shape[0]+2,8),dtype=pt.dtype)
      pv[:]=np.NaN
      pv[     0,(0,1,2,3)]=pt[0,:]
      pv[  1:-1,(0,1,2,3)]=pt
      pv[    -1,(0,1,2,3)]=pt[-1,:]
      pv[(0,0,0,0,-1,-1,-1,-1),(4,5,6,7,4,5,6,7)]=0
      dist=pv[2:,(0,1,2,3)] - pv[:-2,(0,1,2,3)]
      pv[  1:-1,(4,5,6,7)] = 1000.*dist/(2.*pt2pt_time)

      prg.append('  nofrax') #needed to set all axis to use AltFeedRate
      prg.append('  Coord[1].AltFeedRate=0') # allow maximum speed
      prg.append('  Coord[1].SegMoveTime=0') #turn off segmented mode
      prg.append('  linear abs')
      prg.append('  X%g Z%g B%g Y%g' % tuple(pv[0, (0,1,2,3)]))
      prg.append('  dwell 10')
      prg.append('  Gather.Enable=2')
      if cnt>1:
        prg.append('  P100=%d'%cnt)
        prg.append('  N100:')
      prg.append('  Coord[1].AltFeedRate=0') # allow maximum speed
      prg.append('  Coord[1].SegMoveTime=%d'%smt) #to calculate every 1 ms the inverse kinematics
      prg.append('  pvt%g abs'%pt2pt_time) #100ms to next position
      for idx in range(1,pv.shape[0]):
        #prg.append('  P2000=%d'%idx)
        prg.append('  X%g:%g Z%g:%g B%g:%g Y%g:%g' % tuple(pv[idx, (0,4,1,5,2,6,3,7)]))
        #prg.append('Y%g:%g' % tuple(pv[idx, (5, 7)]))
        #prg.append('B%g:%g' %(idx*1000,0))
      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 Z%g B%g Y%g' % tuple(pv[0, (0, 1, 2, 3)]))
        prg.append('  dwell 100')
        prg.append('  goto 100')
        prg.append('  }')
      else:
        prg.append('  dwell 1000')
      prg.append('  Gather.Enable=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/helicalscan.cfg'):
    '''
      mode bits:
       bit 0 (1): wait execution finished (P1000==1)
       bit 1 (2): gather and save data
       bit 2 (4): ...
    '''
    host=self.args.host
    if prg is not None and self.args.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=' ')
        com=GpasciiCommunicator().connect(host,prompt='# ')
        ack=GpasciiCommunicator.gpascii_ack

        channels=["Motor[4].HomePos","Motor[5].HomePos","Motor[3].HomePos","Motor[1].HomePos"]
        ofs=np.ndarray(len(channels))
        for i,v in enumerate(channels):
          com.write(v+'\n')
          val=com.read_until(ack)
          #print val
          #regexp: https://docs.python.org/2/library/re.html -> %e, %E, %f, %g
          ofs[i]=float(re.search('=([-+]?(\d+(\.\d*)?|\.\d+)([eE][-+]?\d+)?)',val).group(1))
        self.rec-=ofs
        self.save_rec()


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__':
  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('--cfg', help='config file containing json configuration structure')
    parser.add_option('--host', help='hostname')

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

    hs=HelicalScan(args)
    hs.args.verbose = 255
    #hs.args.host='MOTTEST-CPPM-CRM0485'
    hs.args.host='SAR-CPPM-EXPMX1'
    #SAR-CPPM-EXPMX1  MOTTEST-CPPM-CRM0485
    #hs.sequencer()
    #hs.test_find_rot_ctr()
    #hs.test_find_rot_ctr(n=5. ,per=1.,bias=2.31,ampl=4.12,phi=24.6)

    hs.calcParam()
    #hs.param[0]=(15,2,0,3,0)#(z_i, y_i, x_i, r_i,phi_i)
    #hs.param[1]=(15,4,0,3,0)#(z_i, y_i, x_i, r_i,phi_i)
    hs.param[0]=(-100, 100,0,50,0)#(z_i, y_i, x_i, r_i,phi_i)
    hs.param[1]=(-100,-100,0,70,0)#(z_i, y_i, x_i, r_i,phi_i)

    hs.test_coord_trf()
    #hs.interactive_cx_cz_w_fy()
    #hs.interactive_dx_dz_w_y()

    #mode bits:
    #0:1 config simulated motors
    #1:2 config real motors
    #2:4 config coord trf
    mode=6#5#4#0
    os.chdir(os.path.join(os.path.dirname(__file__),'../cfg'))
    if mode&1:
      hs.download(file='sim_8_motors.cfg')
    if mode&2:
      hs.download(['$$$***','!common()','!SAR-EXPMX1()','#1..7j/','enable plc 1','Motor[1].MaxSpeed=25','Motor[2].MaxSpeed=25'])
      raw_input('press return when homed')
    if mode&4:
      hs.download(['disable plc 0',])
      time.sleep(.5)
      hs.gen_coord_trf_code()
      print ('gen_coord_trf_code')
      time.sleep(.5)
      hs.download(['enable plc 0',])
      time.sleep(.5)
      hs.download(['#1..7j/',])
    time.sleep(.5)

    #hs.gen_prog(mode=-1)
    #hs.gen_prog(mode=0,cntHor=1,cntVert=3,wRng=(120000,120000))
    #hs.gen_prog(mode=0,cntHor=1)
    #hs.gen_prog(mode=0)
    #hs.gen_prog(mode=0,cntHor=1,cntVert=5,wRng=(120000,120000))
    #hs.gen_prog(mode=1,cntHor=1,cntVert=5,wRng=(120000,120000),pt2pt_time=100)
    #hs.gen_prog(mode=1,cntHor=1,cntVert=5,wRng=(120000,120000),pt2pt_time=100,smt=0)
    #hs.gen_prog(mode=1,cntHor=1,cntVert=5,wRng=(0,360000))
    #hs.gen_prog(mode=1,cntHor=1,cntVert=5,hRng=(-.3,.3),wRng=(0,360000),smt=1)
    #hs.gen_prog(mode=1,cntHor=7,cntVert=2,hRng=(-3,3),wRng=(120000,120000),smt=0)
    #hs.gen_prog(mode=1,cntHor=3,cntVert=6,hRng=(-5,5),wRng=(00,120000),smt=0,pt2pt_time=10)
    #hs.gen_prog(mode=0,cntHor=3,cntVert=10,hRng=(-5,5),wRng=(0,120000))
    #hs.gen_prog(mode=0,cntHor=3,cntVert=25,hRng=(-5,5),wRng=(0,120000))
    hs.gen_prog(mode=1,cntHor=3,cntVert=25,hRng=(-5,5),wRng=(0,120000),smt=0,pt2pt_time=300)
    #hs.gen_prog(mode=1,cntHor=3,cntVert=20,hRng=(-5,5),wRng=(0,1200),smt=0,pt2pt_time=200)
    #hs.gen_prog(mode=1, cntHor=2, cntVert=2, wRng=(0, 360000), smt=0)
    #hs.gen_prog(mode=1)
    #hs.gen_prog(mode=1,pt2pt_time=100,cnt=1,cntVert=35,cntHor=7,hRng=(-.3,.3),wRng=(0,360000*3),yRng=(6.2,2.3))
    #hs.gen_prog(mode=1,pt2pt_time=100,cnt=1,cntVert=10,cntHor=3,hRng=(-30,30),wRng=(0,36000),yRng=(-50,-100))
    #hs.gen_prog(mode=1,cntHor=7,cntVert=2,hRng=(-100,50),wRng=(000,10000),smt=0)
    hs.load_rec()
    hs.show_pos();hs.show_vel()
    hs.interactive_anim()
    #hs.show_vel();    plt.show()
    #hs.show_pos();    plt.show()

    return

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