#!/usr/bin/perl -w

# PID Servo for PSL-FSS (Slow)
# Tobin Fricke 2007-01-09

use strict;
#use Scalar::Util qw(looks_like_number);

sub looks_like_number {
    return ($_[0] =~ /^-?\d+\.?\d*$/);  #FIXME
}

use EpicsTools;

# Parameters
my $process  = 'C1:PSL-FSS_FAST';
my $actuator = 'C1:PSL-FSS_SLOWDC';
my $setpoint = 0;
my $blinkystatus = 0;

my ($KpParam, $KiParam, $KdParam) = 
	('C1:PSL-FSS_SLOWKP', 'C1:PSL-FSS_SLOWKI', 'C1:PSL-FSS_SLOWKD');
my $timestepParam = 'C1:PSL-FSS_TIMEOUT';

#($KpParam, $KiParam, $KdParam) = (-0.001, -0.0025, -0.005);
# Old values were Kp=-.15, Ki=0,Dk=-0.005
# More parameters -- these ones actually have to be numbers
my @hard_stops = (-5.0, 5.0);  # Perl5 apparently doesn't have an Inf value
my $increment_limit = 0.01;

# Variables
my @u;	# outputs to the actuator
my @e;  # error signal

my $debug = 1;

print "Starting FSS Slow Servo\n";

# Shift some initial values into our registers

while ($#u < 2) {
    unshift(@u, get_value($actuator));
    unshift(@e, 0);
    print("Current value of actuator = $u[0]\n");
}

# Start our controller

while (1) {
    # Get the current time step
    my $timestep = get_value($timestepParam);

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

    # Blink the blinky light
    if ($blinkystatus) {
        $blinkystatus = 0;
    } else {
        $blinkystatus = 1;
    }
 
    if ($debug) {
	print "\nSLOW_BEAT --> $blinkystatus\n";
    }
    epWrite('C1:PSL-FSS_SLOWBEAT', $blinkystatus);


    # Make sure the "ENABLE" button is checked
    if (get_value("C1:PSL-FSS_SLOWLOOP") == 0) {
        printf("FSS_SLOWLOOP disabled -- control loop disabled.\n");
	next;
    }

    # Make sure the loop is supposed to be active
    if (get_value("C1:PSL-FSS_RCTRANSPD") < get_value("C1:PSL-FSS_LOCKEDLEVEL")) {
	print("Reference Cavity not locked -- control loop disabled.\n");
	next;
    }

    # Instead of using the actuation we requested in the previous step as the 
    # previous value of the actuation, we'll read the current actuation from
    # EPICS.  This prevents us from monopolizing the slider.
    $u[0] = get_value($actuator);

    if ($debug) {
	print "\nActuator --> $u[0]\n";
   }
    
    # Make room for the present time
    unshift(@e, undef);
    unshift(@u, undef);
    
    # Read the PID parameters in case they have changed
    my ($Kp, $Ki, $Kd) = get_value($KpParam, $KiParam, $KdParam);
    $Ki *= $timestep;
    $Kd /= $timestep;

    if ($debug) {
         print("Kp = $Kp\tKi = $Ki\tKd = $Kd\n");
    }

    # Read the current value of the Process Variable and the Setpoint
    my $p = &get_value($process);
    my $s = &get_value($setpoint);
 
    # The basic finite-difference PID approximation
    $e[0] = $p - $s;
    # $u[0] = $u[1] + ($Kp + $Ki + $Kd)*$e[0] - ($Kp + 2*$Kd)*$e[1] + $Kd * $e[2];
    $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);
    $u[0] = rail($u[0], @hard_stops);

    # Bullshit rounding to prevent epWrite from complaining about the
    # number of digits in the $u[0] variable
    my $tempn = int($u[0]*10000)/10000;
    $u[0] = $tempn;

    # Perform the actuation
    if ($debug) {
	print "Actuator <-- $u[0]\n";
    }
    epWrite($actuator, $u[0]);

    # Discard samples sufficiently far in the past
    pop(@e);
    pop(@u);
}

# We need to distinguish between literal numbers and EPICS
# process variables.  If we're given the name of a process
# variable, we'll use ezcaread to dereference it.

sub get_value {
    my @results = ();
    foreach (@_) {
        if (looks_like_number($_)) {
	    push(@results, $_);
        } else {
            my @epResult = epRead($_);
            pop @epResult or die "Could not read EPICS process variable.";
            push(@results, pop(@epResult)); 
        }
    }
    if ($#results == 0) {  #FIXME: Is this necessary?
	return $results[0];
    } else {
	return @results;
    }
}

sub rail {
    my $value = shift(@_);
    my @limits = @_;

    # If we're only given one value for the limits, we'll
    # assume the limits are symmetric, i.e. lim ==> (-lim,lim)
    if ($#limits == 0) { 
        unshift(@limits, -$limits[0]);
    }

    if ($limits[1] < $limits[0]) {
	die "Upper limit is lower than lower limit!";
    }
    
    ($value < $limits[0] ? $limits[0] :
    ($value > $limits[1] ? $limits[1] : $value));
}