#!/usr/bin/env python

import transform
from numpy import matrix, sin, cos, degrees, radians

class myTransform(transform.transform):
  ### The transform functions should be overloaded for each application
  
  #
  # Note: transform functions must gracefully handle transform over 
  #       the position ranges available to the real motors. 
  #

  # Transform real coordinates into soft coordinates
  def fwdTrans(self, m_positions, t_positions):
    """ """
    m_matrix = matrix(m_positions)
    m_matrix.shape=3,1
    th, phi, chi = [radians(x) for x in t_positions]
    Rth = matrix([[cos(th), -sin(th), 0], [sin(th), cos(th), 0], [0, 0, 1]])
    Rphi = matrix([[1, 0, 0],[0, cos(phi), -sin(phi)],[0, sin(phi), cos(phi)]])
    Rchi = matrix([[sin(chi), 0, -cos(chi)], [0, 1, 0], [cos(chi), 0, sin(chi)]])
    
    R = Rth * Rchi * Rphi

    retval = R * m_matrix

    #!print retval
    retval.shape=1,3
	
    return retval.tolist()[0]
  
  # Transform soft coordinates into real coordinates
  def invTrans(self, tm_positions, t_positions):
    """ """
    tm_matrix = matrix(tm_positions)
    tm_matrix.shape=3,1
    th, phi, chi = [radians(x) for x in t_positions]
    Rth = matrix([[cos(th), -sin(th), 0], [sin(th), cos(th), 0], [0, 0, 1]])
    Rphi = matrix([[1, 0, 0],[0, cos(phi), -sin(phi)],[0, sin(phi), cos(phi)]])
    Rchi = matrix([[sin(chi), 0, -cos(chi)], [0, 1, 0], [cos(chi), 0, sin(chi)]])

    R = Rth * Rchi * Rphi

    # Inverse (R.I) is the same as the transpose (R.T) for rotations

    retval = R.T * tm_matrix
    retval.shape=1,3
    print "retval",retval.tolist()

    return retval.tolist()[0]

if __name__ == "__main__":
  #!prefix = 'kmp2:'
  #!prefix = 'kmp3:'
  prefix = 'kmp:'
  labStr = 'lab'
  realMotors = ["%s%s" % (prefix, x) for x in ('sm1', 'sm2', 'sm3') ]
  transformMotors = ["%s%s" % (prefix, x) for x in ('m1', 'm2', 'm3') ]
  softMotors = ["%s%s:%s" % (prefix, labStr, x) for x in ('m1', 'm2', 'm3') ]
  enablePv = "%s%s:enable" % (prefix, labStr)
  print realMotors, transformMotors, softMotors, enablePv
  MyTrans = myTransform(realMotors, transformMotors, softMotors, enablePv)
  MyTrans.main()