package RFC::PID;
#$Id: PID.pm,v 1.47 2023/11/06 07:02:20 sqko Exp $
#################################################################
#
#
#       RFC::PID
#
#
# This module defines a PID controller and can be used together with 
# the other RFCcontroll modules for process control systems.
# 
# To obtain an instance of this class call one of the constructors 
# like:
#
# $pid = RFC::PID->new($name,$filename,[opt lock_filename]);
#
# Notice that unlike most other RFC devices the PID device class
# does not require a RFC::Rig instance as an argument to the constructor.
# The name argument is the instance name (useful if operating  
# with more than one instance of the PID class).
#
# The filename is the name of the file in which to store process
# error and integration error information.
# This is used so that the PID device can maintain state across 
# processes (for instance if the control program is started once
# a minute from crontab).
#
# The RFC::PID class implements a standard IPD controller (refer
# wikipedia for a more detailed description of a PID controller)
#
# All RFC::PID objects has the following public member functions besides
# those derived from the Debug class.
#
# $pid->reset()          Resets the internal data including
#                        integrated error.
# $pid->name()           Returns the name given as first argument
#                        to the constructor.
# $pid->intwindup([$])   Sets or gets the maximum integration error
#                        possible (default is 0.2)
#                        To disable integrtion windup protection set
#                        the value to 0.
# $pid->data($[$])       Adds a new error value to the device.
#                        Arguments must be current error and optionally time.
#                        If no time argument is specified, the local system 
#                        time is used instead (utime).
# $pid->P([$])           Sets or gets the proportional gain (default is 0.9)
# $pid->I([$])           Sets or gets the integration gain (default is 0.2)
# $pid->D([$])           Sets or gets the differential gain (default is 0.1)
# $pid->get_P()          Returns the proportional error multiplied 
#                        with proportional gain
# $pid->get_I()          Returns the integrated error multiplied 
#                        with integration gain
# $pid->get_D()          Returns the differential error multiplied 
#                        with differential gain
# $pid->get_int()        Returns the integrated error.
# $pid->reset_int()      Resets the integrated error.
# $pid->set_int($)       Sets the integrated error to the specified value.
# $pid->deadband([$])    Sets ot gets the deadband (if the error is below this 
#                        calls to Out() returns undef);
#                        Remember to check for defined status of out if 
#                        deadband is used (default is no deadband).
# $pid->cutoff([$])      Sets or gets the cutoff vqlue. The cutoff behaves similar
#                        to deadband, but only for large error values making
#                        the PID more robust with respect to erroneous values
#                        of the measured process variable.
# $pid->min([$])         Sets or gets the minimum output value (default is -1)
# $pid->max([$])         Sets or gets the maximum output value (efault is 1)
# $pid->Out([$])         Returns the output to be set to the system.
#                        The output value will be between min and max
#                        If an argument is specified to Out() it is assumed
#                        to be a gain which is then multiplied to the raw
#                        output value and the result returned instead.
#                        Notice that if the error is within the deadband, Out()
#                        returns undef, so remember to check the return value
#                        of Out() before using it to set a device!
# $pid->remove()         This function works like DESTROY() except it does NOT
#                        run a store() command before removing the file object.
#
# Additionally the RFC::PID instances has the following private functions
# (although Perl does not protect private member functions from being
# called from outside an instance...)
#
# $pid->load()           Loads the data stored in the data file. 
#                        Called automatically by the constructor.
# $pid->store()          Saves the internal data to file. Called 
#                        automatically by the destructor. 
#                        Returns 1 on succes, 0 if the modification time
#                        of the file has changed since last read/write
#                        Note that store will only save state if it has
#                        changed, that is that $pid->out() has been run
#                        resulting in an output change. This means that if the 
#                        nescesarry change is below the deadband or above the 
#                        cutoff, changes will NOT be saved!
#
# The RFC::PID object also implements integration windup protection by the following 
# method (in addition to the posibility to set a maximum value for the integrated error):
# If the sum of the proportional gain and the previous integrated error is enough 
# for the output to reach the maximum or minimum value, the integrated error is NOT
# updated but is maintained at the previous value.
#
#################################################################
use strict;
use warnings;
use SemaforeFile;
use Class::Inspector;
use Debug;
use File::Touch;
use Time::HiRes qw(time);
our $AUTOLOAD;
our @ISA = qw(Debug);

BEGIN {
    use Exporter ();
    our ($VERSION, @ISA, @EXPORT, @EXPORT_OK, %EXPORT_TAGS);
    $VERSION = 1.0;
    %EXPORT_TAGS = (
        'standard'=>[],
                    );
    Exporter::export_ok_tags('standard');
    Exporter::export_tags('standard');
    @ISA = qw(Exporter);
}

return 1;

sub new
{
    my $class = shift;
    my $self = $class->SUPER::new();
    bless $self,$class;
    $self->{'_name'} = shift;
    $self->die("No name specified for new instance of RFC::PID!\n") unless ($self->{'_name'});
    my $file = shift;
    $self->die("No filename specified for storrage file for RFC::PID!\n") unless ($file);
    unless (-e $file)
    {
	touch($file);
	chmod 0666,$file;
    }
    if (@_)
    {
	$self->{'_file'} = SemaforeFile->new($file,shift);
    }
    else
    {
	$self->{'_file'} = SemaforeFile->new($file);
    }
    $self->{'_P'} = 0.9;
    $self->{'_I'} = 0.2;
    $self->{'_D'} = 0.1;
    $self->{'_dead'} = 0;
    $self->{'_cutoff'} = 0;
    $self->{'_old_error'} = 0;
    $self->{'_interror'} = 0;
    $self->{'_intprotect'} = 0;
    $self->{'_new_error'} = 0;
    $self->{'_old_time'} = 0;
    $self->{'_new_time'} = 0;
    $self->{'_ilimit'} = 0.2;
    $self->{'_min'} = -1;
    $self->{'_max'} = 1;
    $self->{'_modtime'} = $self->{'_file'}->mtime();
    $self->{'do_store'} = 0;
    $self->{'force_store'} = 0;
    $self->load();
    return $self;
}

sub name()
{
    my $self = shift;
    return $self->{'_name'};
}

sub intwindup
{
    my $self = shift;
    if (@_)
    {
	$self->{'_ilimit'} = shift;
	$self->{'_ilimit'} = 0 unless ($self->{'_ilimit'});
	$self->check_min_max; 
    }
    return $self->{'_ilimit'};
}

sub reset()
{
    my $self = shift;
    $self->{'_old_error'} = 0;
    $self->{'_interror'} = 0;
    $self->{'_new_error'} = 0;
    $self->{'_old_time'} = 0;
    $self->{'_new_time'} = 0;
    $self->{'do_store'} = 1;
    $self->{'force_store'} = 1;
}

sub reset_int()
{
    my $self = shift;
    $self->{'_interror'} = 0;
    $self->{'do_store'} = 1;
    $self->{'force_store'} = 1;
}

sub set_int()
{
    my $self = shift;
    my $int = shift;
    $self->writedebug("Interror set to: $int") if ($self->debug());
    $self->{'_interror'} = $int;
    $self->{'_intprotect'} = 0;
    $self->{'do_store'} = 1;
    $self->{'force_store'} = 1;
}

sub get_error()
{
    my $self = shift;
    return $self->{'_new_error'};
}

sub deadband
{
    my $self = shift;
    if (@_)
    {
	$self->{'_dead'} = shift;
	$self->{'_dead'} = 0 unless ($self->{'_dead'});
	$self->check_min_max;
    }
    return $self->{'_dead'};
}

sub cutoff
{
    my $self = shift;
    if (@_)
    {
	$self->{'_cutoff'} = shift;
	$self->{'_cutoff'} = 0 unless ($self->{'_cutoff'});
	$self->check_min_max;
    }
    return $self->{'_cutoff'};
}

sub check_min_max
{
    my $self = shift;
    $self->die("Fatal error: Maximum output is less than minimum output value!") unless ($self->max() > $self->min());
    $self->die("Fatal error: Negative proportional gain detected!") if ($self->P < 0);
    $self->die("Fatal error: Negative integration gain detected!") if ($self->I < 0);
    $self->die("Fatal error: Negative integration windup detected!") if ($self->intwindup < 0);
    $self->die("Fatal error: Negative deadband detected!") if ($self->deadband < 0);
    $self->die("Fatal error: Negative cutoff detected!") if ($self->cutoff < 0);
    $self->die("Fatal error: Negative differential gain detected!") if ($self->D < 0);
}

sub min
{
    my $self = shift;
    if (@_)
    {
	$self->{'_min'} = shift;
	$self->check_min_max;
    }
    return $self->{'_min'};
}

sub max
{
    my $self = shift;
    if (@_)
    {
	$self->{'_max'} = shift;
	$self->check_min_max;
    }
    return $self->{'_max'};
}

sub Out()
{
    my $self = shift;
    my $out = $self->get_P() + $self->get_I() + $self->get_D();
    $self->writedebug("Error: ".$self->get_error) if ($self->debug);
    return undef if ($self->{'_dead'} && abs($self->get_error()) < $self->{'_dead'});
    return undef if ($self->{'_cutoff'} && abs($self->get_error()) > $self->{'_cutoff'});
    $self->{'do_store'} = 1;
    $out = -1 if ($out < -1);
    $out = 1 if ($out > 1);
    ## Rerange to correct min - max range ###
    $out = ($out+1) * ($self->max - $self->min)/2 + $self->min;
    if (@_)
    {
	my $gain = shift;
	$out = $out * $gain; 
    }
    return $out;
}

sub get_P()
{
    my $self = shift;
    my $p = $self->get_error();
    $p = $p * $self->{'_P'};
    $self->writedebug("P: $p") if ($self->debug());
    return $p;
}

sub get_I()
{
    my $self = shift;
    return 0 unless ($self->{'_I'});
    my $i = $self->{'_interror'};
    my $int = $i * $self->{'_I'};
    $self->writedebug("I: $int") if ($self->debug());
    return $int;
}

sub get_int()
{
    my $self = shift;
    my $int = $self->{'_interror'};
    $self->writedebug("Interror: $int") if ($self->debug());
    return $int;
}

sub get_D()
{
    my $self = shift;
    return 0 unless ($self->{'_D'});
    my $err = $self->{'_old_error'} - $self->{'_new_error'};
    my $deltat = $self->{'_old_time'} - $self->{'_new_time'};
    return 0 unless ($deltat);
    my $diff = $err/$deltat;
    $diff = $diff * $self->{'_D'};
    $self->writedebug("D: $diff") if ($self->debug());
    return $diff;
}

sub data
{
    my $self = shift;
    $self->die('Wrong parameter count for '.$self->{'_name'}.'->data($$), expected (error,[opt time])') unless (@_ == 1 || @_ == 2);
    my $t = time;
    my $er = shift;
    $t = shift if (@_);
    if ($self->{'_file'})
    {
	$self->{'_file'}->lock_ex;
	if ($self->{'_modtime'} != $self->{'_file'}->mtime())
	{
	    my @l = ();
	    @l = $self->{'_file'}->readlines();
	    $self->{'_interror'} = $l[0];	
	    $self->{'_modtime'} = $self->{'_file'}->mtime();
	}
	$self->{'_file'}->lock_un;
    }
    $self->{'_old_error'} = $self->{'_new_error'};
    $self->{'_old_time'} = $self->{'_new_time'};
    $self->{'_new_error'} = $er;
    $self->{'_new_time'} = $t;
    $self->{'_old_interror'} = $self->{'_interror'};
    my $out = $self->get_P() + $self->get_I();
    $self->{'_intprotect'} = 1 if ($out > 1 || $out < -1);
    unless (($self->{'_dead'} && abs($self->get_error()) < $self->{'_dead'}) || ($self->{'_cutoff'} && abs($self->get_error()) > $self->{'_cutoff'}))
    {
	$self->{'_interror'} += ($self->{'_old_error'} + $self->{'_new_error'}) * ($self->{'_new_time'} - $self->{'_old_time'}) / 2  
    }
    return unless ($self->{'_ilimit'});
    $self->{'_interror'} = -$self->{'_ilimit'} if ($self->{'_interror'} < -$self->{'_ilimit'});
    $self->{'_interror'} = $self->{'_ilimit'} if ($self->{'_interror'} > $self->{'_ilimit'});
}

sub load()
{
    my $self = shift;
    my @l = ();
    if ($self->{'_file'})
    {
	$self->{'_file'}->lock_sh;
	@l = $self->{'_file'}->readlines();
	$self->{'_modtime'} = $self->{'_file'}->mtime() if ($self->{'_file'});
	$self->{'_file'}->lock_un;
	return unless (@l);
	$self->{'_interror'} = $l[0];
	$self->{'_new_error'} = $l[1];
	$self->{'_new_time'} = $l[2];
    }
    foreach ('_interror','_new_error','_new_time')
    {
	$self->{$_} = 0 unless ($self->{$_});
    }
    return undef;
}

sub store()
{
    my $self = shift;
    if ($self->{'_file'})
    {
	my $res = 0;
	$self->{'_file'}->lock_ex;
	if ($self->{'force_store'} || ($self->{'_modtime'} == $self->{'_file'}->mtime() && $self->{'do_store'}))
	{
	    if ($self->{'_intprotect'})
	    {
		$self->{'_file'}->writeline($self->{'_old_interror'},$self->{'_new_error'},$self->{'_new_time'}) if ($self->{'_file'});
		$self->writedebug(join("\n","Storing values:",$self->{'_old_interror'},$self->{'_new_error'},$self->{'_new_time'})) if ($self->debug > 3);
	    }
	    else
	    {
		$self->{'_file'}->writeline($self->{'_interror'},$self->{'_new_error'},$self->{'_new_time'}) if ($self->{'_file'});
		$self->writedebug(join("\n","Storing values:",$self->{'_interror'},$self->{'_new_error'},$self->{'_new_time'})) if ($self->debug > 3);
	    }
	    $self->{'_modtime'} = $self->{'_file'}->mtime();
	    $self->{'do_store'} = 0;
	    $self->{'force_store'} = 0;
	    $res = 1;
	}
	$self->{'_file'}->lock_un;
	return $res;
    }
    else
    {
	$self->writedebug("WARNING:  Attempting to write to Semafore object after it has been removed! Store() aborted".$self->name,5) 
    }
}

sub P
{
    my $self = shift;
    if (@_)
    {
	$self->{'_P'} = shift; 
	$self->{'_P'} = 0 unless ($self->{'_P'});
	$self->check_min_max;
    }
    return $self->{'_P'};
}

sub I
{
    my $self = shift;
    if (@_)
    {
	$self->{'_I'} = shift;
	$self->{'_I'} = 0 unless ($self->{'_I'});
	$self->check_min_max;
    }
    return $self->{'_I'};
}

sub D
{
    my $self = shift;
    if (@_)
    {
	$self->{'_D'} = shift;
	$self->{'_D'} = 0 unless ($self->{'_D'});
	$self->check_min_max;
    }
    return $self->{'_D'};
}



sub AUTOLOAD()
{
    ################################################################
    ### AUTOLOAD()                                               ###
    ### If caller ended here, undefiend subroutine call happened ###
    ################################################################
    my $self = shift;
    my $type = ref ($self) or die "$self is not an object!";
    die "Unknown function call $AUTOLOAD attempted!";
}

sub remove()
{
    my $self = shift;
    $self->writedebug("Removing SemaforeFile associated with PID ".$self->name) if ($self->debug());    
    $self->{'_file'}->DESTROY if ($self->{'_file'});
    $self->{'_file'} = undef; 
    return undef;
}

sub DESTROY()
{
    my $self = shift;
    $self->store() if ($self->{'_file'} && $self->{'do_store'});
    $self->{'_file'}->DESTROY if ($self->{'_file'});
    $self->{'_file'} = undef;
    return undef;
}

sub print_setup()
{
    #############################################
    ### print_setup()                         ###
    ### utility function for debugging        ###
    ### purposes, prints the contents of the  ###
    ### device instance data fields $self->{} ###
    #############################################
    my $self = shift;
    my @arr = ('*******************************************');
    push @arr,'Device ID: '.$self->{'_type'};
    push @arr,'Member functions:';
    my $class = $self;
    $class =~ s/=.*//;
    my @methods = Class::Inspector->methods( $class, 'full', 'public' );
    foreach my $m (@methods)
    {
        if (ref($m) =~ /ARRAY/)
        {
            foreach (@{$m})
            {
                push @arr,"\t$_";
            }
            next;
        }
        push @arr,"\t$m";
    }
    push @arr,'';
    push @arr,'Instance data (Field,  Value):';
    foreach my $key (keys %{$self})
    {
        if (defined ($self->{$key}))
        {
            if (ref($self->{$key}) =~ /ARRAY/)
            {
                push @arr,"$key, Array with values:";
                push @arr,"\tIndex\tValue";
                my $x = 0;
                foreach (@{$self->{$key}})
                {
                    push @arr,"\t$x\t'$_'";
                    $x++;
                }
                push @arr,'';
                next;
            }
            if (ref($self->{$key}) =~ /HASH/)
            {
                push @arr,"$key, Asociative array with values:";
                push @arr,"\tKey\tValue";
                foreach my $k (keys %{$self->{$key}})
                {
                    push @arr,"\t'$k'\t'$self->{$key}{$k}'";
                }
                push @arr,'';
                next;
            }
            push @arr,$key.",\t'$self->{$key}'";
        }
        else
        {
            push @arr,$key.",\t\\N";
        }
    }
    push @arr,'';
    return join("\n",@arr);
}


sub debug
{
    my $self = shift;
    if (@_)
    {
        $self->SUPER::debug(@_);
        $self->{'_file'}->debug(@_) if ($self->{'_file'});
    }
    return $self->{'_debug'};
}
