/* -*- Mode: Text -*-* */

/********************************************************************\
* File: circular.hs                                                  *
* Date: 07/14/1997                                                   *
* Author: Cem Unsal                                                  *
*         Robotics Institute, Carnegie-Mellon University             *
*         unsal@ri.cmu.edu                                           *
*                                                                    *
* Description:                                                       *
* Circular highway                                                   *
*                                                                    *
* Description of the highway is in file                              *
* http://www.cs.cmu.edu/~unsal/research/shift/circular.html or the   *
* document sensor.{pdf,ps}                                           *
*                                                                    *
* External C functions: -                                            *
*                                                                    *
* This file is distributed under the conditions described in the     *
* file 'CONDITIONS' which should accompany this file.                *
\********************************************************************/

#include <smart-ahs/roadways/highway.hs>
#include <smart-ahs/roadways/source.hs>
#include <smart-ahs/roadways/weather.hs

#define lw 4           // lane width
#define sl 75         // segment length
#define pi 3.1415927
#define initiallanedev 0

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 Source source := create(Source,
                               gxp := 2.5*sl, 
                               gyp := 4*sl-1.5*lw+initiallanedev, 
                               gzp :=0,
                               section := secRL, 
                               lane := lnRL2,
                               segment := segRL1,
                               rxp := 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 := 15,
                               duration := 0,
                               period := 1.5);

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,
                                previousYOffsets := [0]);
        Section tsecRL := create(Section,
                                width := 3*lw,
                                length := sl+ (sl-3*lw)*pi/2 + sl,
                                previousYOffsets := [0]);
        Section tsecRU := create(Section,
                                width := 3*lw,
                                length := sl + (sl-3*lw)*pi/2 + sl,
                                previousYOffsets := [0]);
        Section tsecLU := create(Section,
                                width := 3*lw,
                                length := sl + (sl-3*lw)*pi/2 + sl,
                                previousYOffsets := [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 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/dwon section definitions */
        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 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;
      };
}