#!/usr/bin/perl -w

# Chiller PID Servo
# Adapted by Danny Atkinson 2009-05-20 from:
# PID Servo for PSL-FSS (Slow)
# Tobin Fricke 2007-01-09
#
# Current SVN version
# $Id: PIDChillerServo.pl 937 2010-05-04 16:42:01Z daniel.atkinson@LIGO.ORG $
#
# THEORY OF OPERATION
#
# In the "ideal parallel form", a PID controller is defined as:
#
# u(t) = Ki Integral[ e(tau) dtau ] + Kp e(t) + Kd e'(t)
#
# where
# u(t) is the actuation at time t
# e(t) is the error signal at time t
# e'(t) is the first derivative of the error signal
# (Ki, Kp, Kd) are the integral, proportional, and derivative gains
#
# In order to implement this in a computer program, we have to
# discretize it.  In general we'll know the error signals at time
# intervals Delta t.  We'll also know what actuations we produced at
# these earlier times.  From the prior values of u and e, and the
# current value of e, we want to compute what the current value of the
# actuation u should be.
#
# To compute the current actuation, we use a first-order Taylor
# expansion of u:
#
# u(t) = u(t - Delta t) + (Delta t)u'(t)
#
# where the derivative u'(t) comes from the definition of the PID controller:
#
# u'(t) = Ki e(t) + Kp e'(t) + Kd e''(t)
#
# Now we replace the derivatives e'(t) and e''(t) with their finite
# (backwards) difference approximations:
#
# e'(t) =  (e(t) - e(t - Delta t))/(Delta t)
# e''(t) = (e(t) - 2e(t - Delta t) + e(t - 2 Delta t))/(Delta t)^2
#
# To implement this in Perl we use some last-in-first-out buffers to
# remember the current and previous two error signals and actuations:
#
# u[0] := u(t)
# u[1] := u(t - Delta t)
# u[2] := u(t - 2 Delta t)
#
# So the update equation becomes:
#
# u[0] = u[1] +
#       dt(Ki e[0] + Kp (e[0] - e[1]) / dt + Kd (e[0] - 2 e[1] + e[2]) / (dt^2))
#
# or, multiplying through by dt:
#
# u[0] = u[1]
#       + Ki e[0] dt
#       + Kp (e[0] - e[1])
#       + Kd (e[0] - 2 e[1] + e[2]) / dt
#
# where dt = Delta t is the timestep.
#
# We can then absorb the timestep into the parameters Ki and Kd provided
# we scale them appropriately when we change the timestep.

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

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

# Parameters
my $arm = $ARGV[0];
my $process  = 'H1:TCS-ITM'.$arm.'_LSR_HDTEMPDEG';
my $actuator = 'H1:TCS-CHILLER_'.$arm.'_CHLSETPT';
my $setpoint = 'H1:TCS-CHILLER_'.$arm.'_SRVSETPT';
my $blinkystatus = 0;
my $p = get_value($process);

my ($KpParam, $KiParam, $KdParam) = 
	('H1:TCS-CHILLER_'.$arm.'_PGAIN', 'H1:TCS-CHILLER_'.$arm.'_IGAIN', 'H1:TCS-CHILLER_'.$arm.'_DGAIN');
#($KpParam, $KiParam, $KdParam) = (-0.130, -0.00278, -0.025);
if (looks_like_number($KpParam) != 1) { `ezcawrite $KpParam -0.130`; }
if (looks_like_number($KiParam) != 1) { `ezcawrite $KiParam -0.00278`; }
if (looks_like_number($KdParam) != 1) { `ezcawrite $KdParam -0.025`; }
my $timestepParam = 100;
my $loopTimer = 100;

#($KpParam, $KiParam, $KdParam) = (-0.001, -0.0025, -0.005);

# Verify that CHLSETPT as read is legitimate (close to where the chiller really is).
# If not, copy the initial CHLSETPT from the chiller's current readback setpoint.
my $chiller = get_value('H1:TCS-CHILLER_'.$arm.'_SETPT1');
if (abs($chiller - get_value($actuator)) > 0.1) {
    `ezcawrite $actuator $chiller`;
}

# Limiting parameters
my @hard_stops = (get_value($actuator) - 3.0 , get_value($actuator) + 3.0);
my @absolute_stops = (10.0, 30.0);
my $increment_limit = 0.2;

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

my $debug = 0;
if ($#ARGV > 0) {
    if ($ARGV[1] eq "-d") {
	$debug = 1;
	print "Warning: Debug mode is active! Servo will not actually write temperatures!\n";
	print "Hard stops are ".$hard_stops[0]." and ".$hard_stops[1].", increment limit is ".$increment_limit."\n";
    }
}

print "Starting TCS Chiller 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");
}

my @months = qw( Jan Feb Mar Apr May Jun Jul Aug Sep Oct Nov Dec );

# Start our controller

while (1) {
    # Set up a logfile for our records.
    my @fulltime = localtime(time);
    my $mon = $fulltime[4];
    my $year = $fulltime[5] + 1900;
    open LOGFILE, ">>PID".$arm.$months[$mon].$year."Log.txt" or die "Couldn't open file";

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

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

    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 Setpoint
    my $s = get_value($setpoint);

    if ($debug) {
	print "P: ".$p."\n";
	print "S: ".$s."\n";
    }

    # Verify that CHLSETPT as read is legitimate (close to where the chiller really is).
    # If not, copy the CHLSETPT from the chiller's current readback setpoint.
    my $chiller = get_value('H1:TCS-CHILLER_'.$arm.'_SETPT1');
    my $chillerset = get_value($actuator);
    if ((abs($chiller - $chillerset) > 1.0)&(get_value("H1:TCS-CHILLER_".$arm."_SRVENSW") == 0)) {
	`ezcawrite $actuator $chiller`;
    }

    # Check for laser head temp channel errors. If so, disable PID
    if ($p eq "accessible") {
	`ezcawrite "H1:TCS-CHILLER_"$arm"_SRVENSW" 1`;
	$p = $s;
	print LOGFILE "Laser head temperature channel unavailable, disabling PID.\n";
    }

    # The basic finite-difference PID approximation
    $e[0] = $p - $s;
    # Check for newly restarted h0epics0. Don't try to servo on zero!
    if ($s == 0.00) {
	print LOGFILE "Setpoint invalid, holding last actuation.\n";
	$e[0] = 0;
    }
    # $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]);
    print $u[0]." and ".$u[1]."\n";
   
    # Enforce hard stops and maximum |increment|
    $u[0] = $u[1] + rail($u[0] - $u[1],$increment_limit);
    $u[0] = rail($u[0], @hard_stops);
    
    print LOGFILE "Time: ".localtime(time)."  Temperature ".$p."  Setpoint ".$s."  Error ".$e[0]."  Actuation:  ".$u[0]."  Gains:  ".$Kp." ".$Ki." ".$Kd."\n";

    # Rounding to nearest hundredth.
    my $rounded = int($u[0]*100 + 0.5)/100;

    # Perform the actuation
    if ($debug) {
	print "Actuator <-- $rounded\n";
    }

    # If the PID servo is enabled, write to the chiller set point.
    if (get_value("H1:TCS-CHILLER_".$arm."_SRVENSW") == 0) {
	`ezcawrite $actuator $rounded`;
    } else {
	$rounded = get_value($actuator);
	$u[0] = $rounded;
	print "PID control overriden. Using unmodified chiller setpoint.\n";
	print LOGFILE "PID control is disabled. Using Actuation: ".$rounded."\n";
    }

    $rounded = rail($rounded, @absolute_stops);

    # We now servo onto the chiller setpoint.
    my $loopset = get_value($actuator);
    my $interval = int(10 - (int((get_value($loopset) * 100) + 0.5) % 10));
    my $lowpt = int($loopset * 10) / 10;
    for(my $timer = 0; $timer < ($timestep / $loopTimer); $timer++) {
	for(my $i = 0; $i < 10; $i++) {
	    my $setpt = 0;
	    if($i >= $interval) {
		$setpt = $lowpt + 0.1;
	    } else {
		$setpt = $lowpt;
	    }
	    if ($debug) {
		print $i." ".$setpt."\n";
	    }
	    if ($debug == 0) {
		`ezcawrite "H1:TCS-CHILLER_"$arm"_REQSETPT1" $setpt`;
	    }	    
	    
	    for(my $j = 0; $j < ($loopTimer / 10); $j++) {
		sleep(1);
		# Blink the blinky light
		if ($blinkystatus) {
		    $blinkystatus = 0;
		} else {
		    $blinkystatus = 1;
		}
		`ezcawrite "H1:TCS-CHILLER_"$arm"_SRVKPALV" $blinkystatus`;
		
		# Average sensor readings to squelch sensor noise (@ LLO).
		my $newp = get_value($process);
		if ($newp ne "accessible") {
		    $p = ($p + ($newp * 0.01))/1.01; #Update our moving average.		   
		}
	    }	   
	}
    }

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

    #Close the file, in case we're moving to a new month.
    close LOGFILE;
}

# 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 = `ezcaread $_`;
	    chomp $epResult;
	    my @read = split(/ /, $epResult);
	    push(@results, $read[$#read]);
        }
    }
    if($#results == 0) { #This is necessary so perl doesn't auto-convert the name of the returned array
	return $results[0];  #to its scalar length if only one scalar is expected.
    }
    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));
}