######################################################################
#              |
#              V  beam from upstream (ob,ib,ds) denotes (outboard,inboard,downstream) motors
#         ob       ob
#              |  distance to axis of rotation from ob, ib motors is 152 mm
#           --------- axis of rotation to pitch
#              |  distance to axis of rotation from ds motor is 62 mm
#              ds
#  Motor resolutions for inboard, outboard and downstream motors are the same
#  tilt the capillary, Θ mrad, around 152 mm radius (R_ib_ob) from the pivot point to inboard (ib) and outboard (ob) motors
#  For inboard and outboard motors, let MRES_ib_ob is defined as  motor resolution in unit mm/step, Convert to Θ mrad/step is given by MRES_ib_ob/R_ib_ob * 1000
#  tilt in opposite direction 62 mm radius (R_ds) from the pivot point to the down stream motor
#  For downstream motor, let MRES_ds is defined as  motor resolution in unit mm/step, Convert to Θ mrad/step is given by MRES_ib_ob/R_ds * 1000
#
# 
#
######################################################################

# virtual motor
record(motor,"$(P){MD2:Pitch}Mtr"){
	field(DTYP,"Soft Channel")
	field(DESC,"VIRTUAL MOTOR, mrad tilt")

	# "Use Readback Interface"
	field(URIP, "Yes")

        # Where to read the readback from
	field(RDBL, "$(P){readback} NPP NMS")

        # How to stop this motion
        field(STOO, "$(P){STOP} PP MS")

        # Where to get "done moving" status from
        field(DINP, "$(P){DMOV} NPP NMS") 

	# Where to forward link to process, after this record is processed
    	field(OUT,"$(P){Tilt} PP MS")

	# MRES here only determines the smallest
    	# allowed movement. Decrease/increase as
    	# appropriate.
	field(MRES, "0")  #will be reinitialize by $(P){mradian}MRES at startup
    	field(RRES, "1.0")
    	field(PREC, "3")
    	field(EGU,  "mrad") 
   	
	# Dial Limits
    	field(DHLM, "10.0")
    	field(DLLM, "-10.0")
}

#initialize vitual motor MRES at startup, intially at zero
record(calcout,"$(P){mradian}MRES")
{	
        field(DESC,"mm to mrad motor resolution")
	field(PINI,"YES")
	field(INPA, "$(P){MD2:1-Ax:Y:us:ib}Mtr.MRES NPP NMS")
	#convert mm to mrad, radius is 152 mm
	field(CALC, "a/152*1000")
	field(OUT,"$(P){MD2:Pitch}Mtr.MRES PP MS")
}

# Distribute Setpoint, tild mrad angle to uptream and down stream motors
#record(fanout,"$(P){Fanout}")
#{
#	field(DESC,"mrad to us:ib & us:ob and ds motors")
#	field(LNK1,"$(P){downstream:tilt} ")
#	field(LNK2,"$(P){upstream:tilt} ")
#}

# Stop both motions
record(dfanout, "$(P){STOP}") {
    field(DESC, "Stop Pitch")
    field(OUTA, "$(P){MD2:1-Ax:Y:us:ib}.STOP PP MS")
    field(OUTB, "$(P){MD2:1-Ax:Y:us:ob}.STOP PP MS")
    field(OUTC, ":$(P){MD2:1-Ax:Y:ds}.STOP PP MS")
}

# Done Moving
record(calc, "$(P){DMOV}") {
    field(DESC, "Pitch Done Moving")
    field(INPA, "$(P){MD2:1-Ax:Y:us:ib}.DMOV CP MS")
    field(INPB, "$(P){MD2:1-Ax:Y:us:ob}.DMOV CP MS")
    field(INPC, "$(P){MD2:1-Ax:Y:ds}.DMOV CP MS")
    field(CALC, "A&&B&&C")
}

# inboard, outboard, downstream motor resolutions in mm/step
record(calc, "$(P){motorMRES}"){
	field(PINI, "YES")
        field(DESC, "mm/step")
        field(INPA, "$(P){MD2:1-Ax:Y:us:ib}Mtr.VAL NPP NMS")       
        field(CALC, "A")
}

#distance from pitch rotation axes to upstream inboard and outboard motors
record(calc, "$(P){upstream}RADIUS"){
        field(DESC, "ib,ob mtr radius")
	field(PINI, "YES")
        field(CALC, "152")
}

#distance from pitch rotation axes to downstream motor
record(calc, "$(P){downstream}RADIUS"){
        field(DESC, "ds radius mtr")
	field(PINI, "YES")
        #field(INPA, "62")       #▲s
        field(CALC, "62")
}

# The calcout record performs the calculation to tilt upstream inboard and outboard motors by ▲Θ 
record(transform, "$(P){Tilt}") {
    field(DESC, "tilt (tild ▲Θ mrad, us & ds mtrs, ")
    #field(INPA, "$(P){MD2:1-Ax:Pitch}Mtr.VAL PP MS") #input ▲Θ, PREC is current value of a record
    field(INPB, "$(P){upstream}RADIUS.VAL NPP NMS")
    field(CLCC, "a*b/1000") # r * ▲Θ/1000 =▲s
    field(CLCF, "c") # r * ▲Θ/1000 =▲s
    field(OUTC, "$(P){MD2:1-Ax:Y:us:ib}Mtr.VAL PP MS ")  # Output to upstream inboard and outboard, default is CA, for clarity for different IOC
    field(OUTF, "$(P){MD2:1-Ax:Y:us:ob}Mtr.VAL PP MS")  # Output to upstream inboard and outboard, default is CA, for clarity for different IOC
    #field(OUTC, "$(P){MoveUpstream} PP ")  # Output to upstream inboard and outboard, default is CA, for clarity for different IOC
    #get downstream radius, Tilt in -▲Θ direction 
    field(INPD, "$(P){downstream}RADIUS.VAL NPP MS")
    field(CLCE, "-a*d/1000") # r * (-▲Θ)/1000 =▲s
    field(OUTE, "$(P){MD2:1-Ax:Y:ds}Mtr.VAL PP MS")  # Output to upstream inboard and outboard, default is CA, for clarity for different IOC
}

record(calc,"$(P){readback}')
{
	field(INPA,"$(P){MD2:1-Ax:Y:us:ib}Mtr.RBV CP MS")
	field(INPB,"$(P){upstream}RADIUS NPP MS")
	#convert ▲s to ▲Θ, ▲Θ = ▲s/r*1000
	field(CALC,"a/b*1000")
	field(PREC, "3")
    	field(PINI, "YES") 
}