#!/usr/bin/env python
#*-----------------------------------------------------------------------*
#|                                                                       |
#|  Copyright (c) 2014 by Paul Scherrer Institute (http://www.psi.ch)    |
#|                                                                       |
#|              Author Thierry Zamofing (thierry.zamofing@psi.ch)        |
#*-----------------------------------------------------------------------*
'''
Homing procedure for PPRM.

Long Doc'''
#for ID in 1 2 3 4 5 6
#do
#caput MYPREFIX:MOTOR_$ID.RLV 55
#caput MYPREFIX:MOTOR_$ID.SET Set
#caput MYPREFIX:MOTOR_$ID.VAL 0
#caput MYPREFIX:MOTOR_$ID.SET Use
#done

#for ID in 1 2 3 4 5 6
#do
#caput MYPREFIX:MOTOR_$ID.RLV -10
#done


import logging, sys, os, json
import CaChannel,time

class SwissMxSmarAct:

  def __init__(self, args):
    self.args=args
    prefix=self.args.var[0]
    if prefix[-1]==':':prefix[:-1]
    self.prefix=prefix
    self.pv=pvDict=dict()
    for i in range(1,16):
      #SARES30-ESBMX1:FRM_FORW.PROC
      #SARES30-ESBMX1:DRIVE
      s1=prefix+'%i'%i
      for j in ('.VAL','.RBV','.HOMF','.HOMR','.MSTA','_AUTOZERO'):
        s2=s1+j
        print(s2)
        pv = CaChannel.CaChannel(s2)
        pv.setTimeout(10.0)
        pv.search()
        pvDict[s2]=pv
      CaChannel.ca.pend_io(10.0)
      #print(pvDict)
      #pvDict['cryorbv'] = CaChannel.CaChannel("SAR-EXPMX:MOT_CRYO.RBV")
      #pvDict['cryorbv'].search()

  def home(self,idx,fwd):
    print('home motor %i fwd:%d...'%(idx,fwd))
    pv=self.pv[self.prefix+'%i'%idx+'_AUTOZERO']
    pv.putw(1) #set autozero on
    if fwd:
      pv=self.pv[self.prefix+'%i'%idx+'.HOMF']
    else:
      pv=self.pv[self.prefix+'%i'%idx+'.HOMR']
    #print('aaa')
    pv.putw(1) #set position and move
    #print('bbb')
    CaChannel.ca.pend_event(0.1)
    #print('ccc')

  def waitHomed(self,idx):
    #print('ddd')
    pv=self.pv[self.prefix+'%i'%idx+'.MSTA']
    #print('eee',pv,self.prefix+'%i'%idx+'.MSTA')
    while pv.getw()==0:
      print('wait motor %i homing done ...'%idx)
      CaChannel.ca.pend_event(0.2)
    #print('fff')

  def moveAbs(self,idx,val):
    pv=self.pv[self.prefix+'%i'%idx+'.VAL']
    pv.putw(val) #set position and move


  def check_pos(self, idx, val, tolerance=0.1):
    pvrbv = self.pv[self.prefix+'%i'%idx+'.RBV']
    pvst = self.pv[self.prefix+'%i'%idx+'.MSTA']

    CaChannel.ca.pend_event(0.2)
    while pvst.getw():
      CaChannel.ca.pend_event(0.2)
    rbv=pvrbv.getw()
    if abs(val - rbv) > tolerance:
      raise Exception("failed to reach target value %g,%g,%g"%(val,rbv,tolerance))

  def homing(self):
    #start homing in desired direction
    #cryorbv = self.pv['cryorbv']
    #if cryorbv.getw() < 4.9:
    #  raise Exception("Cryojet is too close, move it to 5.0 mm")

    # home Colli Y first and move it to the TOP
    ###self.home(2, True)
    ###CaChannel.ca.pend_event(0.5)
    ###self.waitHomed(2)
    ###CaChannel.ca.pend_event(0.5)
    ###self.moveAbs(2, 10.0)
    ###self.check_pos(2, 10.0)

    ###for idx,fwd in ((1,True),(4,True),(5,True),(6,True),(7,True),(8,True),(10,True),(11,True),(12,True),(13,True),(14,True),(15,True),):
    for idx,fwd in ((1,True),(2,True),(3,True),(4,True),(5,True),(6,True),(7,True),(8,True),(9,True),(10,True),(11,True),(12,True),(13,True),(14,True),(15,True),):
      self.home(idx,fwd)
    CaChannel.ca.pend_event(0.5)
    ####wait homing done
    ###for idx in (1,4,5,6,7,8,10,11,12,13,14,15,):
    ###  self.waitHomed(idx)

    ####move to desired position
    ###for idx,val in ((1,-10),(4,.4),(5,.5),(6,.6),(7,.7),(8,.8),(10,1.0),(11,1.1),(12,1.2),(13,1.3),(14,1.4),(15,1.5),):
    ###  self.moveAbs(idx,val)
    print('Homing done')

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=('SARES30-ESBMX',
               )
    epilog=__doc__+'''
Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '

    fmt=MyFormatter()
    parser=OptionParser(epilog=epilog, formatter=fmt)
    #parser.add_option('', '--host', dest='host', help='hostname (or none to use locally)', metavar='HOST')
    #parser.add_option('-v', '--verbose', type="int", dest='verbose', help='verbosity bits (see below)', default=0)
    (args, other)=parser.parse_args()
    print(args,other)
    args.var=other
    smrct=SwissMxSmarAct(args)
    smrct.homing()

  #os.environ['EPICS_CA_ADDR_LIST']='localhost'
  parse_args()