/* -*- Mode: C -*-* */

/********************************************************************\
* File: cemdemo2.hs                                                  *
* Date: 07/11/1997                                                   *
* Author: Cem Unsal                                                  *
*         Robotics Institute, Carnegie-Mellon University             *
*         unsal@ri.cmu.edu                                           *
*                                                                    *
* Description:                                                       *
* Description of the highway is in file                              *
* http://www.cs.cmu.edu/~unsal/research/circular.html                *
*                                                                    *
**********************************************************************
* This file is distributed under the following conditions:           *
* 								     *
* 1. The recipient shall refrain from disclosing the software,       *
*    in any form, to third parties without prior written	     *
*    authorization from Carnegie-Mellon University.  The	     *
*    recipient shall have the right to use and copy the	             *
*    software on, or in connection with the operation of, any        *
*    computer system owned or operated by it.  In addition,	     *
*    the recipient shall have the right to modify or merge           *
*    the software to form updated works.		             *
* 							             *
* 2. If the recipient receives a request from any third party        *
*    to furnish all or a portion of the software to any third        *
*    party,  it will refer such a request to Carnegie-Mellon         *
*    University.					             *
* 							             *
* 3. Carnegie-Mellon University shall not be held liable for any     *
*    damages resulting from the use or misuse of the software        *
*    provided by it.  Furthermore, Carnegie-Mellon University        *
*    remains without obligation to assist in its installation        *
*    or maintenance.						     *
* 								     *
* 4. The recipient agrees to acknowledge Carnegie-Mellon             *
*    University in appropriate citations appearing in public         *
*    literature when reference is made to the software provided      *
*    above.						             *
* 							             *
* 5. If the recipient develops any enhancements to the software      *
*    which materially improves its operation, the recipient	     *
*    agrees to make such enhancements available to Carnegie-	     *
*    Mellon University without charge, provided Carnegie-            *
*    Mellon University agrees in writing to receive such             *
*    enhancements in confidence, if requested to do so.	             *
* 								     *
* 6. This header comment must remain attached to the source	     *
*    code of the provided software.			             *
* 							             *
*  Bug reports and suggestions can be mailed to Cem Unsal by         *
*  electronic mail addressed to: "unsal@ri.cmu.edu". As	mentioned    *
*  in condition 3 above, the author is not obligated to fix any      *
*  such bugs, or even to acknowledge receipt of the bug report.      *
* 								     *
\********************************************************************/

#include "highway.hs"
#include "vrep.hs"
#include "ufo.hs"

#define lw 4        // lane width
#define sl 200      // segment length/radius
#define pi 3.1415927
#define initiallanedev 0.5
 
global Lane lnLL1, lnLL2, lnLL3, 
            lnRL1, lnRL2, lnRL3,
            lnRU1, lnRU2, lnRU3,
            lnLU1, lnLU2, lnLU3;

global Segment segLL1, segLL2, segLL3, segRL1, segRL2, segRL3,
               segRU1, segRU2, segRU3, segLU1, segLU2, segLU3;

global Section secLL, secRL, secRU, secLU;

global HighwayBuilder hwy := create(HighwayBuilder);

global Ufosource source := create(Ufosource,
				  gxp := 1.5*sl, gyp := 4*sl-1.5*lw+initiallanedev, gzp :=0,
				  section := secLL, 
				  lane := lnLL2,
				  segment := segLL3,
				  rxp := sl+(sl-3*lw)*pi/2+0.5*sl,
				  ryp := 1.5*lw+initiallanedev,
				  rzp := 0,
				  lyp := initiallanedev,
				  vgam11 := 1,
				  vgam12 := 0,
				  vgam13 := 0,
				  vgam21 := 0,
				  vgam22 := -1, // Note alignment of y axis!!!
				  vgam23 := 0,
				  vgam31 := 0,
				  vgam32 := 0,
				  vgam33 := 1,
			      	  vehiclesToCreate := 3,
				  duration := 0,
				  period := 1);

type Ufosource
{
  	output	number	gxp;		// vehicle's global x position
		number	gyp;		// vehicle's global y position
		number	gzp;		// vehicle's global z position
		Section	section;	// vehicle's section
		Segment	segment;	// vehicle's segment
		Lane	lane;		// vehicle's lane
		number	rxp;		// vehicle's road x position
		number	ryp;		// vehicle's road y position
		number	rzp;		// vehicle's road z position
		number	lyp;		// vehicle's lane y position

		number	vgam11;		// vehicle to global alignment matrix
		number	vgam12;
		number	vgam13;
		number	vgam21;
		number	vgam22;
		number	vgam23;
		number	vgam31;
		number	vgam32;
		number	vgam33;

	state	number period;
		continuous number duration;
		number vehiclesToCreate;
		AutomatedVehicle new_av;

	export	newVehicle;

	discrete start {duration' = 1;};

	transition
		start -> start {newVehicle}
		when duration >= period and vehiclesToCreate > 0
		do
		{
			duration := 0;
			vehiclesToCreate := vehiclesToCreate - 1;
			new_av := create(AutomatedVehicle, source := self);
		};
}

type HighwayBuilder
{
  setup
    define
      {
	/* segments */
	/* LL */
	Segment tsegLL1 := create(Segment,
				length := sl, xOffset := 0,
				gxa := 3*lw, gya := 2*sl, gza := 0,
				orientation := pi/2, curvature := 0,
				grade := 0, banking := 0, skid := 0);
	Segment tsegLL2 := create(Segment,
				length := (sl-3*lw)*pi/2, xOffset := sl,
				gxa := 3*lw, gya := 3*sl, gza := 0,
				orientation := pi/2,
                                curvature := 1/(sl-3*lw),
				grade := 0, banking := 0, skid := 0);
	Segment tsegLL3 := create(Segment,
				length := sl, xOffset := sl+(sl-3*lw)*pi/2,
				gxa := sl, gya := 4*sl-3*lw, gza := 0,
				orientation := 0, curvature := 0,
				grade := 0, banking := 0, skid := 0);
	/* RL */
	Segment tsegRL1 := create(Segment,
				length := sl, xOffset := 0,
				gxa := 2*sl, gya := 4*sl-3*lw, gza := 0,
				orientation := 0, curvature := 0,
				grade := 0, banking := 0, skid := 0);
	Segment tsegRL2 := create(Segment,
				length := (sl-3*lw)*pi/2, xOffset := sl,
				gxa := 3*sl, gya := 4*sl-3*lw, gza := 0, 
                                orientation := 0, 
                                curvature := 1/(sl-3*lw),
				grade := 0, banking := 0, skid := 0);
	Segment tsegRL3 := create(Segment,
				length := sl, xOffset := sl+(sl-3*lw)*pi/2,
				gxa := 4*sl-3*lw, gya := 3*sl, gza := 0,
				orientation := -pi/2, curvature := 0,
				grade := 0, banking := 0, skid := 0);
	/* RU */
	Segment tsegRU1 := create(Segment,
				length := sl, xOffset := 0,
				gxa := 4*sl-3*lw, gya := 2*sl, gza := 0,
				orientation := -pi/2, curvature := 0,
				grade := 0, banking := 0, skid := 0);
	Segment tsegRU2 := create(Segment,
				length := (sl-3*lw)*pi/2, xOffset := sl,
				gxa := 4*sl-3*lw, gya := sl, gza := 0,
				orientation := -pi/2, 
                                curvature := 1/(sl-3*lw),
				grade := 0, banking := 0, skid := 0);
	Segment tsegRU3 := create(Segment,
				length := sl, xOffset := sl+(sl-3*lw)*pi/2,
				gxa := 3*sl, gya := 3*lw, gza := 0,
				orientation := pi, curvature := 0,
				grade := 0, banking := 0, skid := 0);
	/* LU */
	Segment tsegLU1 := create(Segment,
				length := sl, xOffset := 0,
				gxa := 2*sl, gya := 3*lw, gza := 0,
				orientation := pi, curvature := 0,
				grade := 0, banking := 0, skid := 0);
	Segment tsegLU2 := create(Segment,
				length := (sl-3*lw)*pi/2, xOffset := sl,
				gxa := sl, gya := 3*lw, gza := 0,
				orientation := pi, 
                                curvature := 1/(sl-3*lw),
				grade := 0, banking := 0, skid := 0);
	Segment tsegLU3 := create(Segment,
				length := sl, xOffset := sl+(sl-3*lw)*pi/2,
				gxa := 3*lw, gya := sl, gza := 0,
				orientation := pi/2, curvature := 0,
				grade := 0, banking := 0, skid := 0);
	/* lanes */
	/* LL */
	Lane tlnLL1 := create(Lane, width := lw, yOffset := 0, laneLeft := nil);
	Lane tlnLL2 := create(Lane, width := lw, yOffset := lw);
        Lane tlnLL3 := create(Lane, width := lw, yOffset := 2*lw, laneRight := nil);
       
	Lane tlnRL1 := create(Lane, width := lw, yOffset := 0,	laneLeft := nil);
	Lane tlnRL2 := create(Lane, width := lw, yOffset := lw);
	Lane tlnRL3 := create(Lane, width := lw, yOffset := 2*lw, laneRight := nil);

	Lane tlnRU1 := create(Lane, width := lw, yOffset := 0,	laneLeft := nil);
	Lane tlnRU2 := create(Lane, width := lw, yOffset := lw);
	Lane tlnRU3 := create(Lane, width := lw, yOffset := 2*lw, laneRight := nil);

	Lane tlnLU1 := create(Lane, width := lw, yOffset := 0, laneLeft := nil);
	Lane tlnLU2 := create(Lane, width := lw, yOffset := lw);
	Lane tlnLU3 := create(Lane, width := lw, yOffset := 2*lw, laneRight := nil);

	/* sections */
	Section tsecLL := create(Section,
				width := 3*lw,
				length := sl+ (sl-3*lw)*pi/2+sl,
                                previousYOffset := 0); //must be an array!!!
	Section tsecRL := create(Section,
				width := 3*lw,
				length := sl+ (sl-3*lw)*pi/2 + sl,
				previousYOffset := 0);
	Section tsecRU := create(Section,
				width := 3*lw,
				length := sl + (sl-3*lw)*pi/2 + sl,
				previousYOffset := 0);
	Section tsecLU := create(Section,
				width := 3*lw,
				length := sl + (sl-3*lw)*pi/2 + sl,
				previousYOffset := 0);
	
	Weather tweather := create (Weather, 
				visibility := 100,
				precipitation := 0);
      }
    do
      {
	/* lanes */
	lnLL1 := tlnLL1; lnLL2 := tlnLL2; lnLL3 := tlnLL3;
	lnRL1 := tlnRL1; lnRL2 := tlnRL2; lnRL3 := tlnRL3;
	lnRU1 := tlnRU1; lnRU2 := tlnRU2; lnRU3 := tlnRU3;
	lnLU1 := tlnLU1; lnLU2 := tlnLU2; lnLU3 := tlnLU3;

	/* sections */
	secLL := tsecLL; secRL := tsecRL; 
	secRU := tsecRU; secLU := tsecLU;
	
	/* segments */
	segLL1 := tsegLL1; segLL2 := tsegLL2; segLL3 := tsegLL3;
	segRL1 := tsegRL1; segRL2 := tsegRL2; segRL3 := tsegRL3;
	segRU1 := tsegRU1; segRU2 := tsegRU2; segRU3 := tsegRU3;
	segLU1 := tsegLU1; segLU2 := tsegLU2; segLU3 := tsegLU3; 

	/* additional definitions - connections */
	
	/* lanes and segment arrays for sections */
	lanes(tsecLL) := [tlnLL1, tlnLL2, tlnLL3];
	lanes(tsecRL) := [tlnRL1, tlnRL2, tlnRL3];
	lanes(tsecRU) := [tlnRU1, tlnRU2, tlnRU3];
	lanes(tsecLU) := [tlnLU1, tlnLU2, tlnLU3];

	segments(tsecLL) := [tsegLL1, tsegLL2, tsegLL3];
	segments(tsecRL) := [tsegRL1, tsegRL2, tsegRL3];
	segments(tsecRU) := [tsegRU1, tsegRU2, tsegRU3];
	segments(tsecLU) := [tsegLU1, tsegLU2, tsegLU3];

	/* section arrays for lanes */
	section(tlnLL1) := tsecLL; section(tlnLL2) := tsecLL;
 	section(tlnLL3) := tsecLL;
 
	section(tlnRL1) := tsecRL; section(tlnRL2) := tsecRL;
 	section(tlnRL3) := tsecRL;
 
	section(tlnRU1) := tsecRU; section(tlnRU2) := tsecRU; 
 	section(tlnRU3) := tsecRU; 

	section(tlnLU1) := tsecLU; section(tlnLU2) := tsecLU;
 	section(tlnLU3) := tsecLU; 

 	/* up-down section connections for sections 
         * sectionUp(tsecLL) := [tsecLU] ; sectionDown(tsecLL) := [tsecRL];
         * sectionUp(tsecRL) := [tsecLL] ; sectionDown(tsecRL) := [tsecRU];
         * sectionUp(tsecRU) := [tsecRL] ; sectionDown(tsecRU) := [tsecLU];
         * sectionUp(tsecLU) := [tsecRU] ; sectionDown(tsecLU) := [tsecLL];
        */
	/* up-down lane connections for lanes */
	laneDown(tlnLL1) := [tlnRL1];
	laneDown(tlnLL2) := [tlnRL2]; 
	laneDown(tlnLL3) := [tlnRL3];

	laneDown(tlnRL1) := [tlnRU1]; 
	laneDown(tlnRL2) := [tlnRU2];
	laneDown(tlnRL3) := [tlnRU3];

	laneDown(tlnRU1) := [tlnLU1];
	laneDown(tlnRU2) := [tlnLU2]; 
	laneDown(tlnRU3) := [tlnLU3];

	laneDown(tlnLU1) := [tlnLL1]; 
	laneDown(tlnLU2) := [tlnLL2];
	laneDown(tlnLU3) := [tlnLL3]; 
	/* */
	laneUp(tlnLL1) := [tlnLU1];
	laneUp(tlnLL2) := [tlnLU2]; 
	laneUp(tlnLL3) := [tlnLU3]; 

	laneUp(tlnRL1) := [tlnLL1]; 
	laneUp(tlnRL2) := [tlnLL2];
	laneUp(tlnRL3) := [tlnLL3]; 

	laneUp(tlnRU1) := [tlnRL1]; 
	laneUp(tlnRU2) := [tlnRL2];
	laneUp(tlnRU3) := [tlnRL3]; 

	laneUp(tlnLU1) := [tlnRU1]; 
	laneUp(tlnLU2) := [tlnRU2];
	laneUp(tlnLU3) := [tlnRU3];

	/* left-right lane connections for lanes */
	laneRight(tlnLL1) := tlnLL2; laneRight(tlnLL2) := tlnLL3;
	 laneLeft(tlnLL2) := tlnLL1;  laneLeft(tlnLL3) := tlnLL2;

	laneRight(tlnRL1) := tlnRL2; laneRight(tlnRL2) := tlnRL3;
	 laneLeft(tlnRL2) := tlnRL1;  laneLeft(tlnRL3) := tlnRL2;

	laneRight(tlnRU1) := tlnRU2; laneRight(tlnRU2) := tlnRU3;
	 laneLeft(tlnRU2) := tlnRU1;  laneLeft(tlnRU3) := tlnRU2;

	laneRight(tlnLU1) := tlnLU2; laneRight(tlnLU2) := tlnLU3;
	 laneLeft(tlnLU2) := tlnLU1;  laneLeft(tlnLU3) := tlnLU2;

	/* section definitions for segment */
	section(tsegLL1) := tsecLL; section(tsegLL2) := tsecLL;	
	section(tsegLL3) := tsecLL;

	section(tsegRL1) := tsecRL; section(tsegRL2) := tsecRL;	
	section(tsegRL3) := tsecRL;

	section(tsegRU1) := tsecRU; section(tsegRU2) := tsecRU;	
	section(tsegRU3) := tsecRU;

	section(tsegLU1) := tsecLU; section(tsegLU2) := tsecLU;	
	section(tsegLU3) := tsecLU;

        /* up-down segment connections */
	upSegment(tsegLL1) := nil;     downSegment(tsegLL1) := tsegLL2;
	upSegment(tsegLL2) := tsegLL1; downSegment(tsegLL2) := tsegLL3;
	upSegment(tsegLL3) := tsegLL2; downSegment(tsegLL3) := nil;

	upSegment(tsegRL1) := nil;     downSegment(tsegRL1) := tsegRL2;
	upSegment(tsegRL2) := tsegRL1; downSegment(tsegRL2) := tsegRL3;
	upSegment(tsegRL3) := tsegRL2; downSegment(tsegRL3) := nil;

	upSegment(tsegRU1) := nil;     downSegment(tsegRU1) := tsegRU2;
	upSegment(tsegRU2) := tsegRU1; downSegment(tsegRU2) := tsegRU3;
	upSegment(tsegRU3) := tsegRU2; downSegment(tsegRU3) := nil;

	upSegment(tsegLU1) := nil;     downSegment(tsegLU1) := tsegLU2;
	upSegment(tsegLU2) := tsegLU1; downSegment(tsegLU2) := tsegLU3;
	upSegment(tsegLU3) := tsegLU2; downSegment(tsegLU3) := nil;
	
	/* weather definitions for segments */
	weather(tsegLL1) := tweather; weather(tsegLL2) := tweather;
	weather(tsegLL3) := tweather;
	weather(tsegRL1) := tweather; weather(tsegRL2) := tweather;
	weather(tsegRL3) := tweather;
	weather(tsegRU1) := tweather; weather(tsegRU2) := tweather;
	weather(tsegRU3) := tweather;
	weather(tsegLU1) := tweather; weather(tsegLU2) := tweather;
	weather(tsegLU3) := tweather;
      };
}

/* end of file -- cemdemo2.hs -- */