/*
 | circmain.c - Drive a Car in a Circle
 |
 | This is a minimal program to operate the fuzzy inference engine
 | using run-time information generated by CubiCalc RTC.
 |
 | Use it with CIRCLE.C, which contains the rule base. Compile this file
 | and CIRCLE.C, then link the two together with the fuzzy run-time library.
 |
 | NOTE TO DEMO USERS: This file is provided to illustrate the run-time
 | capability of CubiCalc RTC. You cannot build it without the headers and
 | libraries that are included in the RTC product.
 |
 | Although this program uses floating point in the calling program to do
 | the simulation, the pre-built executable provided on the demo disk is
 | using 8-bit integer arithmetic for the fuzzy computations.
 |
 | Copyright (c) 1992-1995 by HyperLogic Corporation.  All rights reserved.
*/


# include <stdio.h>
# include <stdlib.h>
# include <math.h>

# include <fzopti.h>      /* RTC options include file for integer runtime */
# include <fz.h>          /* RTC standard include file */
# include <circle.h>      /* RTC-generated data include file */




			/*
			 | The target radius and how close
			 | we must be before stopping
			*/

# define RANGE ((double)20.0)
# define TOLERANCE ((double)0.01)



			/*
			 | Macros to implement degree trig functions
			 |
			 | The standard C library uses radians only,
			 | but the CubiCalc simulation is in terms of degrees.
			*/

# define PI ((double)3.1415926535)

# define sind(x) sin(((x) / 180.0) * PI)
# define asind(x) ((asin(x) / PI) * (double)180.0)
# define cosd(x) cos(((x) / 180.0) * PI)
# define acosd(x) ((acos(x) / PI) * (double)180.0)




			/*
			 | Variables needed to operate fuzzy engine
			*/

static FZ_NUMERIC Inputs[Circ_INPUT_COUNT];   /* input array (RangeError) */
static FZ_NUMERIC Outputs[Circ_OUTPUT_COUNT]; /* output array (SteeringAngle) */
static FZ_NUMERIC Scratch[Circ_SCRATCH_SIZE]; /* scratch area */
static struct fz_ramparm Circ_RAM_Parms;      /* RAM parameter structure */



			/*
			 | Local variables, used for simulation
			*/

static double CarSpeed = (double)1.0;           /* car speed */
static double CarRadius = (double)0.0;          /* car-to-center distance */
static double CarAzimuth = (double)0.0;         /* azimuth angle of car */
static double RangeError = (double)0.0;         /* input to system */

static int Iteration = 1;                       /* iteration counter */

static double SteeringAngle = (double)0.0;      /* steering control */




			/*
			 | Prototypes
			*/

void main(void);
static void scale(void);
static void descale(void);
static void simulate(void);




			/*
			 | main - Fuzzy system control loop
			*/

void main(void) {

	int fzres;


			/*
			 | Initialize fuzzy engine:  Pass the addresses
			 | of the program's RAM-based structures.
			*/

	fzres = fz_init(&Circ_RAM_Parms, &Circ_ROM_Parms,
	                                          Scratch, Inputs, Outputs);
	if (fzres != FZ_ERROR_OK) {
		printf("Error initializing system (%d)!\n", fzres);
		exit(1);
	}


			/*
			 | Main loop
			*/

	Iteration = 0;

	for ( ; ; ) {


			/*
			 | Prepare inputs
			*/

		RangeError = CarRadius - RANGE;
		scale();


			/*
			 | Invoke the fuzzy engine
			*/

		fz_eval(&Circ_RAM_Parms);


			/*
			 | Recover the outputs
			*/

		descale();



			/*
			 | Print current values
			*/

		printf(
        "[%3d] CarRadius: %6.2f  CarAzimuth: %6.2f  SteeringAngle: %6.2f\n",
		             Iteration, CarRadius, CarAzimuth, SteeringAngle);



			/*
			 | Check for termination conditions
			*/

		if (++Iteration > 500
		|| fabs(RangeError) < TOLERANCE
		|| CarRadius > (double)100.0) break;



			/*
			 | Simulate the plant
			*/

		simulate();
	}


			/*
			 | Print final status
			*/

	printf(
	"\nFinal CarRadius: %6.2f  CarAzimuth: %6.2f  RangeError: %6.2f\n",
                                       CarRadius, CarAzimuth, RangeError);

	exit(0);
}



			/*
			 | fz_panic - User error handling routine
			 |
			 | Used only if the program is linked with 
			 | a "paranoid" debugging library
			*/

void fz_panic(int errcode) {

	printf("Error in executing fuzzy system: %d\n", errcode);
	exit(1);
}



			/*
			 | scale - Prepare input to fuzzy engine
			 |
			 |    Convert floating point to FZ_NUMERIC in the
			 |    0 - 1 range. The typecasts are set up so that
			 |    it works for both floating point and integer
			 |    FZ_NUMERIC types.
			*/

static void scale(void) {

	double ftmp;

			/*
			 | Scale the input to the 0.0 - 1.0 range
			*/

	ftmp = (RangeError + 70.0) / 140.0;

			/*
			 | Convert to the appropriate data type (integer
			 | or floating point, depending on the definition
			 | for FZ_NUMERIC). FZ_NUMERIC_ONE is 1.0 for
			 | floating point types or (for example) 255 for
			 | 8-bit integer.
			 | Put the converted value into the input array.
			*/

	Inputs[Circ_RangeError] = (FZ_NUMERIC)(ftmp * (double)FZ_NUMERIC_ONE);
}



			/*
			 | descale - Prepare output from fuzzy engine
			 |
			 |    Convert normalized FZ_NUMERIC output to floating
			 |    point value in degrees.
			 |    Works for both double and integer FZ_NUMERIC.
			*/

static void descale(void) {

	double ftmp;

	ftmp = ((double)Outputs[Circ_SteeringAngle] * 120.0);
	SteeringAngle = ftmp / (double)FZ_NUMERIC_ONE - 60.0;
}



			/*
			 | simulate - Simulate car motion
			 |
			 |    Calculate new CarRadius and new CarAzimuth
			*/

static void simulate(void) {


	/*
	 | Calculate car's new current radius using Law of Cosines.  Assume
	 | a step of size v in the direction car_az.  Computations are based
	 | on (SteeringAngle + 90) since the car must drive tangent to the
	 | circle (at right angles to the error input vector).  Note that
	 | cos(angle + 90) = - sin(angle).
	*/

	CarRadius = sqrt((CarRadius * CarRadius) + (CarSpeed * CarSpeed)
	                  + (2 * CarRadius * CarSpeed * sind(SteeringAngle)));

	/*
	 | Calculate car's new current azimuth using Law of Sines,
	 | correcting for angle modularity.
	*/

	CarAzimuth += ((fabs(CarRadius) < 0.0001) ? 0.0 :
	                  asind((CarSpeed * cosd(SteeringAngle)) / CarRadius));
	CarAzimuth = fmod(CarAzimuth, 360.0);
	if (CarAzimuth < 0.0) CarAzimuth += 360.0;
}

