def MX3_coordTrf(fileParser,kwargs):
  from math import sqrt
  '''
  --- mandatory ---
  exc   : excentricity of cam
  height: Height of the beam above moved plane
  width : x-distance of P2 to P3
  length: y-distance of P1 to P2,P3
  camSf : scale factor to convert cam encoder to rotation angle (360deg=2*pi)
  --- optional ---
  ...

  Documentation:
  'Cam Movers for SLS undulators' (21.5.2013, Calvi, Bruegger, Zimoch)
  home/zamofing_t/Documents/doc-ext/DeltaTau/Training/TrainingSlides/22-Power PMAC Cam Tables 2014-04.ppt
  Ref Manual: 96-106,488, 527-528, 655


  Coordinate Transformation
  -------------------------
   L1-5:   AA1-5 : Cam Angle = Motor position
   L11-15: S1-5  : Linear shift of the cam S.= E/sqrt(2)*sin(AA.)
   C3-7: X,Y,U,V,W : X,Y positions and rotation around axes X,Y,Z

open forward
L1-L5 are input motor positions, C3-C7 are output coordinate
X=D^-1*S



open inverse
L1-L5 are output motor positions, C3-C7 are input coordinate
S=D*X

  '''

  kw=kwargs
  kw['sqrt2']=sqrt(2.)
  fileParser.parse_txt('''
// ---------- Coordinate System/Transformation ----------
// Set the motors as inverse kinematic axes in CS 1
&1
#1->I
#2->I
#3->I
#4->I
#5->I

Coord[1].Ta=100
Coord[1].Td=100
Coord[1].AltFeedRate=1
Motor[1].MaxSpeed=Motor[1].JogSpeed
Motor[2].MaxSpeed=Motor[2].JogSpeed
Motor[3].MaxSpeed=Motor[3].JogSpeed
Motor[4].MaxSpeed=Motor[4].JogSpeed
Motor[5].MaxSpeed=Motor[5].JogSpeed

open forward
  //define(AA1='L1',AA2='L2',AA3='L3',AA4='L4',AA5='L5')
  define(AA1='L2',AA2='L1',AA3='L4',AA4='L5',AA5='L3')

  define(r='L6',scl='L7')
  define(S1='L11',S2='L12',S3='L13',S4='L14',S5='L15')
  define(X='C6',Y='C7',U='C3',V='C4',W='C5')
  r=$height/$width
  scl=$exc/$sqrt2
  S1=scl*cos(AA1*$camSf)
  S2=scl*cos(AA2*$camSf)
  S3=scl*cos(AA3*$camSf)
  S4=scl*cos(AA4*$camSf)
  S5=scl*cos(AA5*$camSf)

  //send 1"fwd_inp(%f)  %f %f %f %f %f\\n",D0,AA1,AA2,AA3,AA4,AA5
  //send 1"fwd_inp(%f)  %f %f %f %f %f\\n",D0,S1,S2,S3,S4,S5


  //X=D^-1*S
  X=+.5*S1   -.5*S2   +(r+.25)*S3   +(r-.75)*S4   +(.25-r)*$sqrt2*S5
  Y=+.5*S1   +.5*S2       +.25*S3       +.25*S4       +.25*$sqrt2*S5
  U=                       +1.*S3        +1.*S4        -1.*$sqrt2*S5
  V=-1.*S1   +1.*S2       +1.5*S3        -.5*S4        -.5*$sqrt2*S5
  W=-1.*S1   -1.*S2        +.5*S3        +.5*S4        +.5*$sqrt2*S5

  //send 1"fwd_res     %f %f %f %f %f\\n",X,Y,U,V,W

  D0=$$000000f8; //U=$$8 V=$$10 W=$$20 X=$$40 Y=$$80 hex(8+int('10',16)+int('20',16)+int('40',16)+int('80',16)) -> '0xf8'
close

open inverse
  //define(AA1='L1',AA2='L2',AA3='L3',AA4='L4',AA5='L5')
  define(AA1='L2',AA2='L1',AA3='L4',AA4='L5',AA5='L3')
  define(r='L6',scl='L7')
  define(S1='L11',S2='L12',S3='L13',S4='L14',S5='L15')
  define(X='C6',Y='C7',U='C3',V='C4',W='C5')

  //if(D0>0)
  //  send 1"Velocity calculation NOT SUPPORTED\\n"

  //send 1"inv_inp(%f)  %f %f %f %f %f\\n",D0,X,Y,U,V,W

  r=$height/$width

  S1=+.5*X        +.5*Y   +(.25-.5*r)*U   -.25*V          -.25*W
  S2=-.5*X        +.5*Y   +(.5*r-.25)*U   +.25*V          -.25*W
  S3=+.5*X        +.5*Y   +(.25-.5*r)*U   +.25*V          +.25*W
  S4=-.5*X        +.5*Y   +(.25+.5*r)*U   -.25*V          +.25*W
  S5=       .5*$sqrt2*Y   -.25*$sqrt2*U           +.25*$sqrt2*W

  scl=$sqrt2/$exc
  AA1=acos(S1*scl)/$camSf
  AA2=acos(S2*scl)/$camSf
  AA3=acos(S3*scl)/$camSf
  AA4=acos(S4*scl)/$camSf
  AA5=acos(S5*scl)/$camSf

  //send 1"inv_res     %f %f %f %f %f\\n",AA1,AA2,AA3,AA4,AA5

close

''',kw)
