/* vxdemo \- A demo for StethoScope. */

/* 
   modification history
   ------------ -------
   5.4a,25sep00,ss  Modified ln. 165 to properly support random numbers
                    in VxWorks versions < 5.1
   5.4a,18sep00,gah added support for protection domain testing
   5.4a,17jun00,vwc Better cleanup of memory by doing semDelete().
                    Also use flag to cause main task to terminate rather than
                    taskDelete()
   5.4a,12jun00,vwc Add hook routine to be run in sample loop. Install
                    Scope's debug signals by default.
   5.3c,30mar00,vwc Added param to ScopeDemo to set data-buf sz
   5.3a,09jun99,laf Changed buffer sizes - we have 17 variables, not 16,
		    don't need quite as much memory for either buffer.
		    If ScopeInitServer fails (usually due to lack of
		    target memory), don't spawm the other tasks.
   5.1f,17may99,laf Double data buffer size
   5.1e,15apr99,sda Changed ordering of statements in Shutdown to safer.
   5.1e,02apr99,laf Provide Shutdown functionality to clean up.
   5.1a,26oct98,nm  ScopeInitServer() now takes two buffer sizes.
   5.1a,16oct98,nm  Set WtxOverride to 1, not to true.
   5.1a,12aug98,nm  Updated for scope 5.1 release.
   RTI,20dec92,sas  Added ScopeIndex.  Removed support for VxWorks 4.x
   RTI,25nov92,sas  Ported to VxWorks 5.1
   RTI,06may92,sas  Added rebootHookAdd to insure reboot success.
		    Made VxWorks 5.0 the default.
   RTI,03apr92,sas  Converted to mangen format.
   RTI,07nov91,sas  Added plant simulation.
   RTI,01sep89,sas  written.
*/

/*
   DESCRIPTION:
      This file contains code to start a simple synchronous sampler, and
      install and de-install a few demo signals.  It also contains a simple
      double-integrator plant simulator.

      To run the demo (this assumes you have already loaded StethoScope):
	rlogin <target>
	cd  "<scopedir>/lib/m68kVx5.1"
	ld 1 < vxdemo.lo
	ScopeDemo

      To recompile:
	cc68k -I/local/VxWorks/h -I<scopedir>/include -c vxdemo.c

      Useful things to play with:
	Omega = (float) 80;
	ScopeRemoveMultipleSignals("sin");
	InstallFakeSignals(40)
	ScopeDemoSetRate(20.0)
	sis("phase[3]")
	sis("Time","none",&Time,"float");

      A makefile is provided; it can be used to compile this code.  You
      should first edit it to reflect your system's file structure.

      This is a complete VxWorks application.  To utilize StethoScope in
      your system, all you need is a call to ScopeInitServer in your
      startup script, a call to ScopeCollectSignals somewhere in your
      regular processing cycle, and a few calls to ScopeInstallSignal.

   WARNING: 
      With 64 signals installed, the calculation of the sin functions below
      (FakeSignals()) can only be done at about 200 Hz (on a 16MHz 68020).
      Thus, setting the sample rate higher than this will starve both tasks
      tScopeProbeDaemon and tScopeLinkDaemon; Scope will not be able to
      display any data.
      This is intentional---Scope always strives for minimal impact on the
      real-time system.

   NOTES:
      This code normally works using taskDelay.  It can also work by
      attaching a semaphore (sampleSemaphore) to the Aux clock interrupt,
      the recommended means of executing periodic functions in VxWorks.  If
      your processor does have an aux-clock, set the "useAuxClock"
      parameter to 1.

   SEE ALSO:
      ScopeProbe(2), scope(1), vxdemo2(3)
 ------------------------------------------------------------------------- */
/* $Id: vxdemo.c,v 1.42 2002/07/18 22:24:57 sreenath Exp $ */

/* (c) Copyright Real-Time Innovations, Inc., 1999.  All rights reserved. */

#define  RTI_VXWORKS

#include <stdio.h>	/* Change to stdioLib.h for 5.0 */
#include <math.h>

#include "vxWorks.h"
#include "taskLib.h"
#include "sysLib.h"
#include "semLib.h"
#include "rtilib/vxVersions.h"
#include "rtilib/rti_types.h"
#include "rtilib/rti_endian.h"
#include "scope/scope.h"

#define MAXPHASES	(128)
#define PHASESHIFT	(0.1)
#define SQUARECOUNT	(20)

float square= 1.0;
struct ExampleStruct {
  float sint;
  float sin2t;
  float sin3t;
  float cost;
} *singroup;
float phases[MAXPHASES];
float Omega = 1.0;

int NumberOfPhases = 0;

float Pos = 0.0;
float Vel = 0.0;
float Acc = 0.0;

float Posdes = 1.0;
float Veldes = 0.0;
float Kp = 10;
float Kv = 4;

float NoiseMag = 0.1;

float Time = 0.0;
float Dt = 0.005;

SEM_ID SampleSemaphore = NULL;
int ScopeIndex = -1;
int ScopeHandle = -1;

int tid = 0;
static int ScopeDemoShutdownRequest = 0;
static int ScopeNoServer = 0;

void (*SampleHookFunc)(void *) = NULL;
void *SampleHookParam = NULL;

void SampleHookSet( void (*func)(void *), void *param )
{
    SampleHookFunc = func;
    SampleHookParam = param;
}

static
/* Calculate a set of simple wave signals, paying absolutely no attention to 
   efficiency. */
void SineWaves(float t)
{
    register int i;
    static int squareCount = SQUARECOUNT;
    register float wt = t * Omega;

    float sint, cost;

    if(--squareCount <= 0) {
	squareCount = SQUARECOUNT;
        square = 1.0 - square;
    }
    singroup->sint = sin(wt);
    singroup->cost = cos(wt);
    singroup->sin2t = sin(2 * wt);
    singroup->sin3t = sin(3 * wt);
    for(i=0; i<NumberOfPhases; i++) {
	phases[i] = sin(wt + PHASESHIFT*i);
    }

    sint = singroup->sint;
    cost = singroup->cost;

    /* Throw an event for when Sin and Cos are calculated. */
    if((singroup->sint > 0.1) && (singroup->sint <= 0.2)) {
	ScopeEventsCollect(ScopeHandle, 1, "Sine-0.1-0.2", (void *) &sint,
			   RTI_FLOAT32ID);
    }

    if((singroup->cost > 0.5) && (singroup->cost <= 0.6)) {
	ScopeEventsCollect(ScopeHandle, 1, "Cosine-0.5-0.6", (void *) &cost,
			   RTI_FLOAT32ID);
    }
}


static
/* Return a uniform random variable between a and b. */
float Noise(float a, float b)
{
    float result;

    result = (((float) rand() / RAND_MAX) - 0.5);
    return (result * (b - a) + (a + b) / 2.);
}

static
/* This "plant" is an Euler double integrator. */
void Plant(float t)
{
    Vel += Acc * Dt;		
    Vel += Noise(-NoiseMag, NoiseMag);
    Pos += Vel * Dt;
}

static
void Control(float t)
{
    static int stepCount = 300;
    if(stepCount-- <= 0) {
	stepCount = 300;
	Posdes = -Posdes;
	
	/* Throw an event whenever position desired changes. */
	ScopeEventsCollect(ScopeHandle, 2, "PosChangeEvent", (void *) &Posdes,
			   RTI_FLOAT32ID);
    }

    Acc = Kp*(Posdes - Pos) + Kv*(Veldes - Vel);
}

static
void Sample(float t, int noCollection)
{
    SineWaves(t);
    Plant(t);
    Control(t);
    if (noCollection == 0) { ScopeCollectSignals(ScopeIndex); }
    if (SampleHookFunc != NULL) {
	(*SampleHookFunc)(SampleHookParam);
    }
}

static
void InstallFakeSignals(int num)
{
    register int i;
    char str[132];

    if(num > MAXPHASES) {num = MAXPHASES;}
    NumberOfPhases = num;

    /* Install signals for debugging */

    ScopeInstallSignal("Square", "volts", &square, "float", ScopeIndex);
    singroup = (struct ExampleStruct *) calloc(1, sizeof(struct ExampleStruct));
    singroup->sint = 0.0;
    singroup->sin2t = 0.0;
    singroup->sin3t = 0.0;
    singroup->cost = 1.0;
    ScopeInstallSignal("Sine", "volts", &(singroup->sint), "float", ScopeIndex);
    ScopeInstallSignal("Cosine", "volts", &(singroup->cost), "float",
		       ScopeIndex);
    ScopeInstallSignal("Sine2T", "volts", &(singroup->sin2t), "float",
		       ScopeIndex);
    ScopeInstallSignal("Sine3T", "volts", &(singroup->sin3t), "float",
		       ScopeIndex);

    ScopeInstallSignal("Pos", "meters", &Pos, "float", ScopeIndex);
    ScopeInstallSignal("PosDesired", "meters", &Posdes, "float", ScopeIndex);
    ScopeInstallSignal("Vel", "m/s", &Vel, "float", ScopeIndex);
    ScopeInstallSignal("Acc", "m/s/s", &Acc, "float", ScopeIndex);

    for(i=0; i<num; i++) {
	sprintf(str,"sin/sin(t+%.1f)", PHASESHIFT*i);
	ScopeInstallSignal(str, "volts", &phases[i], "float", ScopeIndex);
    }
}

static
/* This routine simply lets the sampler run.  An alternative, asychronous
   sampling strategy is to simply call ScopeCollectSignals() from this routine
   (at interrupt level).  However, this scheme is probably more indicative of
   the way most users will implement Scope. */
int ScopeDemoTimerInterrupt(void)
{
    semGive(SampleSemaphore);
    return(0);		/*  sysAuxClkConnect should take VOIDFUNCPTR */
}

static
/* Called by main loop to clean up mem usage */
void ScopeDemoShutdownInternal(void)
{
    if (tid != 0) {
	if (SampleSemaphore != NULL) {
	    /* Must be aux clk, so clean up. */
	    sysAuxClkDisable();
	    sysAuxClkConnect(NULL, 0);
	    semDelete(SampleSemaphore);
	    SampleSemaphore = NULL;
	}
	if (ScopeNoServer == 0) {
            ScopeShutdown(ScopeIndex);
	} else {
            #if VXWORKS_AE_VERSION_1_0_OR_BETTER
	      ScopeRemoveMultipleSignals("", ScopeIndex);
	    #else
	      ScopeRemoveMultipleSignals("*", ScopeIndex);
	    #endif
	    ScopeNoServer = 0;
	}
        ScopeIndex = -1;
        free(singroup);
        tid = 0;
    }
    /* Reset flag */
    ScopeDemoShutdownRequest = 0;
}

static
/* This routine generates the signals, and does the sampling.
 * It attaches a semaphore to the Aux clock iff useAuxClock is non-zero.
 */
int ScopeDemoSampler(int requestedSR, int useAuxClock, int noCollection)
{
    int nTicksBwSamples = 0;
    float actualSR;

    if (useAuxClock) {
	SampleSemaphore = semBCreate(SEM_Q_PRIORITY, SEM_EMPTY);
    }

    /* Connect the timer to the auxillary clock interrupt. */
    if (useAuxClock &&
	(sysAuxClkConnect(ScopeDemoTimerInterrupt, 0) == OK)) {
	sysAuxClkEnable();
        sysAuxClkRateSet(requestedSR);
        /* hardware may not be exact */
	actualSR = sysAuxClkRateGet();
    } else {
	actualSR = sysClkRateGet();
	if (actualSR < requestedSR) {
            /* Requested rate (requestedSR) is higher than
             * maximum achievable rate.
             */
            nTicksBwSamples = 1;
        } else {
            nTicksBwSamples = actualSR / requestedSR;
            actualSR = requestedSR;
	}
    }

    /* Tell scope the actual sample rate (hardware may not be exact). */
    ScopeChangeSampleRateInt((int) actualSR, ScopeIndex);

    if((actualSR/requestedSR > 1.1) || (actualSR/requestedSR < 0.9)) {
        printf("ScopeDemo: requested rate (%f Hz) not \n"
	       "achievable, actual sampling rate is %f Hz\n",
	       (float) requestedSR, actualSR);
    }

    Dt = 1./actualSR;
    while(!ScopeDemoShutdownRequest) {
	if (useAuxClock) {
	    semTake(SampleSemaphore, WAIT_FOREVER);
	} else {
	    taskDelay(nTicksBwSamples);
	}
	Time += Dt;
	Sample(Time, noCollection);
    }
    ScopeDemoShutdownInternal();
    return(0);
}

/* 
ARGUMENTS
   If useAuxClock is TRUE, use the Aux clock. Otherwise (or if there is no
      Aux clock, use taskDelay.
   scopeIndex is the StethoScope index to use. It must be coordinated with
      the GUI.
   verbosity controls the amount of warning and debug messages.  0 means
      only errors will be printed.  Higher values causes more output.
   If reqDataBufSize is non-zero, use that value rather then the default (51k)
   If noServer is true, then this demo will not start a Scope server.  Assumes
     one is already started.
   If noCollection is non-zero then this demo will not call ScopeCollectSignals
     since it is assumed that another process is collecting.
*/
void ScopeDemo( int useAuxClock, int scopeIndex, int verbosity,
		int reqDataBufSize, int noServer, int noCollection )
{
    int samplingRate = 60;     /* Hz */
    int dataBufSize, signalBufSize, eventBufSize;

    /* Initialize */

    if (ScopeIndex > 0) 
	{
		return;
    }

    if (reqDataBufSize == 0) 
	{
		dataBufSize = 17*8*384; /* 51k data buffer (ints/floats) */;
    } else 
	{
		dataBufSize = reqDataBufSize;
    }
    
	signalBufSize = 40960;
    eventBufSize = -1;
    if (noServer == 0) 
	{
		/* Initialize an index. */
		ScopeIndex = ScopeInitServer(dataBufSize, signalBufSize, verbosity,
				     scopeIndex);
		if (ScopeIndex < 0) 
		{
			printf("ScopeInitServer failed, return code = %d\n", ScopeIndex);
			return;
		}

		/* Attach an event buffer to that index. */
		ScopeHandle = ScopeEventsAttach(eventBufSize, ScopeIndex);
		if(ScopeHandle == 0) 
		{
			printf("ScopeEventsAttach failed, exiting!\n");
			return;
		}
    } 
	else 
	{
		ScopeIndex = scopeIndex;
		ScopeNoServer = 1;
    }
    
    ScopeIndex = scopeIndex;
    InstallFakeSignals(8);

    tid = taskSpawn("ScopeDemo", 100, VX_FP_TASK|VX_STDIO, 0x4000, 
	      (int (*)()) ScopeDemoSampler,
	      samplingRate, useAuxClock, noCollection,0,0,0,0,0,0,0);
}


void ScopeDemoShutdown(void)
{
    if (tid != 0) {
	ScopeDemoShutdownRequest = 1;
    }
}