/*--------------------------------------------------------------------
 * File:        emul.g
 *
 * Description: A Gamma program that simulates a PID-controlled loop.
 *
 * Functions:
 *                PID_Controller.init
 *                PID_Controller.calculate
 *                change_procvar
 *                auto_control
 *                change_values
 *------------------------------------------------------------------*/

/* Allow more than one emulator to run at a time, with the default being
   ("toolsdemo", "emul", "emulq") */

if (length(argv) > 1)
{
  demodomain = cadr(argv);
  demoname = string ("emul", demodomain);
  demoqueue = string ("emulq", demodomain);
}
else
{
  demodomain = "toolsdemo";
  demoname = "emul";
  demoqueue = "emulq";
}

/* Initialize interprocess communication for this process */
if (init_ipc(demoname, demoqueue, demodomain) == nil)
     error("Could not initialize IPC.");
     
/* See if a DataHub is running in the toolsdemo domain. If not, start it. */
if (locate_task (string ("/dh/", demodomain), t) == nil) 
     system(string ("datahub -d ", demodomain));

/* We need to try to register something or the auth is refused. */
SP_001 = register_point(#SP_001);     // The set point variable.
datahub = locate_task (string ("/dh/", demodomain), t);
result = send (datahub, #auth("writer", "writepass"));

/* Register the first points with the DataHub. */     
SP_001 = register_point(#SP_001);     // The set point variable.
MV_001 = register_point(#MV_001);     // The manipulated variable.
PV_001 = register_point(#PV_001);     // The process variable.
cycle_count = 0;                      // Counter for number of cycles

/* Ensure that the confidence values are set to 100, or the
 * logger will not log the values that we produce.  The computation
 * of PV and MV are relative to previous values, so they preserve
 * whatever confidence we initially set. */
SP_001 = set_conf (SP_001, 100);
MV_001 = set_conf (MV_001, 100);
PV_001 = set_conf (PV_001, 100);

/* Create a global variable for demonstrating gsend and lsend. */
demo_send = 1;             

function force_point(pointname, value)
{
	set (pointname, value);
	if (!datahub)
	{
		datahub = locate_task (string ("/dh/", demodomain), t);
	}
	if (datahub)
	{
		if (!send (datahub, `force(@pointname, @value)))
		{
			datahub = nil;
			force_point(pointname, value);
		}
	}
}

/********************************************************
 *                PID Controller Class                  *
 ********************************************************/

/*--------------------------------------------------------------------
 * Class:       PID_Controller
 * Description: Contains data and methods for creating and maintaining
 *              a simulated PID loop.
 *------------------------------------------------------------------*/
class PID_Controller
{
  name;

  errorPrev;
  errorInt = 0;
  errorDrv = 0;
  
  kProp;
  kInt;
  kDrv;
}

/*--------------------------------------------------------------------
 * Method:      PID_Controller.init
 * Returns:     a name
 * Description: Initializes the PID control loop by creating three
 *              variables representing proportion, integration, and
 *              derivative.
 *------------------------------------------------------------------*/
method PID_Controller.init (name)
{
  local val;
  
  .name = name;
  
  .kProp = symbol(string(name, "_Kp"));
  val = register_point(.kProp);
  if (number_p(val) && (conf(val) != 0))
    set(.kProp, val);
  else
    force_point(.kProp, 0.05);

  .kInt = symbol(string(name, "_Ki"));
  val = register_point(.kInt);
  if (number_p(val) && (conf(val) != 0))
    set(.kInt, val);
  else
    force_point(.kInt, 0.7);
  
  .kDrv = symbol(string(name, "_Kd"));
  val = register_point(.kDrv);
  if (number_p(val) && (conf(val) != 0))
    set(.kDrv, val);
  else
    force_point(.kDrv, 0.05);
  .name;
}

/*--------------------------------------------------------------------
 * Method:      PID_Controller.calculate
 * Returns:     an approximate value for MV, as a real number
 * Description: Sets and writes a new MV value.
 *------------------------------------------------------------------*/
method PID_Controller.calculate (sp, mv, pv)  
{                                             
  local error, res;
  
  error = eval(sp) - eval(pv);

  /* Appproximate the integral. */
  .errorInt += error;       

  /* Prevent error "wind-up" effect. */
  if (.errorInt < 0)        
    .errorInt = 0;

  /* Approximate the derivative. */
  if (.errorPrev)           
    .errorDrv = error - .errorPrev;
  .errorPrev = error;
  
  res = (eval(.kProp) * error
         + eval(.kInt) * .errorInt
         + eval(.kDrv) * .errorDrv);
  res = floor (res * 100) / 100;
  
  /* Set boundary conditions. */
  if (res > 200)            
    res = 200;
  if (res <= 0)
    res = 0;  
  force_point (mv, res);
  res;
}

/********************************************************
 *                   The Plant Model                    *
 ********************************************************/

/* The proportion and integral for PV. */
PROP_001 = register_point(#PROP_001); 
INT_001 = register_point(#INT_001);   

/* Check and correct out-of-range values,
 * by writing default "Poor" values.*/
if (PROP_001 < .01 || PROP_001 > 10)  
     force_point(#PROP_001, .7);      
if (INT_001 <= 0 || INT_001 > 10)
     force_point(#INT_001, 5);

/*--------------------------------------------------------------------
 * Function:    change_procvar
 * Returns:     t on success, nil on failure
 * Description: Sets and writes a new PV value.
 *------------------------------------------------------------------*/
function change_procvar (sptag, mvtag, pvtag) // 
{
  local diff = eval(mvtag) * PROP_001 - eval (pvtag),
        newval = eval(pvtag) + diff / INT_001;

  //  princ ((long_p(eval(pvtag)) ? "long " : "float "),
  //	  "pv: ", eval(pvtag), ":", conf(eval(pvtag)), "\n");
  
  newval = floor (newval * 100) / 100;
  newval = newval * demo_send;
  force_point (pvtag, newval);
}

/********************************************************
 *            Automatically Change SP Value             *
 ********************************************************/

/* Initialize a global variable for the automatic timer. */
AUTO_TIMER = nil;
AUTO_RESET_TIMER = nil;

/* Register points, exceptions, and echos with the DataHub.
 * The FREQ_001 variable holds the delay for automatic change of SP. */
AUTO_001 = register_point(#AUTO_001);
add_exception_function(#AUTO_001, #(AUTO_TIMER = auto_control(FREQ_001)));
add_echo_function(#AUTO_001, #(AUTO_TIMER = auto_control(FREQ_001)));

FREQ_001 = register_point(#FREQ_001); 
if (FREQ_001 < 1 || FREQ_001 > 20)
     force_point(#FREQ_001, 8);    
add_exception_function(#FREQ_001, #(AUTO_TIMER = auto_control(FREQ_001)));

function round2(x)
{
        floor (x * 100) / 100;
}

/*--------------------------------------------------------------------
 * Function:    auto_control
 * Returns:     a timer number
 * Description: Sets a timer to change the value of SP at a specified
 *              frequncy.
 *------------------------------------------------------------------*/
function auto_control(freq)
{
  if (AUTO_001 != 0)       
    {
      /* Start a timer to move SP to random settings. */
      if (AUTO_TIMER == nil)
        {
          if (45 < SP_001 && SP_001 < 55)
            force_point(#SP_001, 80);
          else
            force_point(#SP_001, 50);
          AUTO_TIMER = every(eval(freq),
                             `force_point(#SP_001, round2(random() * 100)));
        }
      else
        {
          /* Cancel an existing timer. */
          cancel(AUTO_TIMER);
          AUTO_TIMER = nil;
          auto_control(FREQ_001);
        }
    }
  else                          
    {
      /* Cancel the timer if the AUTO_001 button is not pressed. */
      if (AUTO_TIMER != nil)
        {
          cancel(AUTO_TIMER);
          AUTO_TIMER = nil;
        }
      if (AUTO_RESET_TIMER)
        cancel (AUTO_RESET_TIMER);
      AUTO_RESET_TIMER = after (60, #go_auto());
    }
  AUTO_TIMER;
}

/* Automatically revert to automatic setpoint change mode */
function go_auto ()
{
  if (AUTO_001 == 0)
    force_point (#AUTO_001, 1);
}

/* When the setpoint changes and the automatic setpoint mode is off,
   it means that a user has changed the setpoint manually, so reset
   the timer that throws us back into automatic setpoint mode for
   another minute. */
function sp_changed ()
{
  if (AUTO_001 == 0)
    {
      if (AUTO_RESET_TIMER)
        cancel (AUTO_RESET_TIMER);
      AUTO_RESET_TIMER = after (60, #go_auto());
    }
}

add_exception_function (#SP_001, #sp_changed());

/********************************************************
 *               Emulating the PID Loop                 *
 ********************************************************/

/*--------------------------------------------------------------------
 * Function:    change_values
 * Returns:     function
 * Description: Calls PID_Controller.calculate() and change_procvar()
 *------------------------------------------------------------------*/
function change_values (controller)
{
  controller.calculate(#SP_001, #MV_001, #PV_001);
  change_procvar (#SP_001, #MV_001, #PV_001);
  force_point (#CYCLE, ++cycle_count);
}

/* Create a controller. */
PID1 = new(PID_Controller);   
PID1.init ("PID1");

/* Start the controller running. */
every (0.1, #change_values(PID1));

force_point(#AUTO_001, 1);
AUTO_TIMER = auto_control(#FREQ_001);

/* Loop continuously. */
while (t)
{
  next_event();
}