#!/usr/bin/env python

import re
import time
import os
from ezca import Ezca

#A python translation of the RCAV_thermalPID.pl/SLOW_PID.pl script

#translated from perl script
#checks if string looks like a number using regex
def looks_like_number(in_string):
	pattern = re.compile("/^-?\d+\.?\d*$/")

	if pattern.match(in_string) == None:
		return(False)
	else:
		return(True)


def rail(number, lower_limit, upper_limit):
	if number<lower_limit:
		number = lower_limit
	elif number>upper_limit:
		number = upper_limit

	return(number)

#####Actual Process Code Starts Here#####

RCPID = Ezca(ifo=None)

process  = 'C3:PSL-ACAV_FSS_FASTMON'  #current fast out
actuator = 'C3:PSL-ACAV_FSS_SLOWOUT'    # slowDC Voltage 
setpoint = 'C3:PSL-ACAV_SLOWFSS_SETPOINT' #fast out should be zeroed to this value with loop engauged
KpParam = 'C3:PSL-ACAV_FSS_SLOWPID_KP'        
KiParam = 'C3:PSL-ACAV_FSS_SLOWPID_KI'
KdParam = 'C3:PSL-ACAV_FSS_SLOWPID_KD'
timestepParam = 'C3:PSL-FSS_SLOWPID_TIMEOUT'  #time step in sec
EPICSBlinkChan = 'C3:PSL-ACAV_FSS_SLOWPID_BLINK' # Define channel used to blink on front pannel.
loopStateEnable = 'C3:PSL-ACAV_FSS_SLOWPID_EN' # Define channel used to define loop activation state.  1 = on , 0 = off.

blinkystatus = 0

# More parameters -- these ones actually have to be numbers
hard_stops = [-1.6, 1.6]

increment_limit = 0.03  # this is like a slew rate limit on the actuator

debug = 1;

print("Starting Reference Cavity Can Temperature Servo\n")


u_init = RCPID.read(actuator, log=False)
print("Current value of actuator = " + str(u_init) + "\n")
# Variables
u = [u_init]*4 # outputs to the actuator
e = [0]*4 # error signal


#start controller

while(1):
	timestep = RCPID.read(timestepParam, log=False)

	# Sleep for the rest of the time interval
	time.sleep(timestep)

	#blink the blinky light
	if (blinkystatus):
		blinkystatus = int(0)

	else:
		blinkystatus = int(1)

	if (debug):
		print("\nBlink --> " + str(blinkystatus) + "\n")

	RCPID.write(EPICSBlinkChan, blinkystatus, monitor=False)

	if (RCPID.read(loopStateEnable) == 0):
            print("ENABLE disabled -- control loop disabled.\n")

    #read actuator value
        u[0] = RCPID.read(actuator)
    
        if (debug):
            print("\nActuator --> " + str(u[0]) + "\n")

	#shift right
            u.insert(0,0)
            e.insert(0,0)

	# Read the PID parameters in case they have changed
            Kp = RCPID.read(KpParam)
            Ki = RCPID.read(KiParam)
            Kd = RCPID.read(KdParam)

            Ki = Ki*timestep
            Kd = Kd/timestep

	if (debug):
		print("Kp = " + str(Kp) + "\tKi = " + str(Ki) +"\tKd = " + str(Kd) + "\n")

	# Read the current value of the Process Variable and the Setpoint
        p = RCPID.read(process)
        s = RCPID.read(setpoint)

	# The basic finite-difference PID approximation
        e[0] = p - s

        u[0] = u[1]
        u[0] = u[0] + Ki * (e[0])
        u[0] = u[0] + Kp * (e[0] - e[1])
        u[0] = u[0] + Kd * (e[0] - 2*e[1] + e[2])

    # Enforce hard stops and maximum |increment|
        u[0] = u[1] + rail(u[0] - u[1], -increment_limit, increment_limit)
        u[0] = rail(u[0], hard_stops[0], hard_stops[1])

    #perform the actuation
        if (debug):
            print("Actuator <-- " + str(u[0]) + "\n")

        RCPID.write(actuator, u[0], monitor=False)

    # Discard samples sufficiently far in the past
        u.pop()
        e.pop()