//***************************************************************
// Copyright 2008 Centre For Advanced Spatial Analysis, UCL
//
// Author: Joel Dearden, University College London
//
// Contact: j.dearden@ucl.ac.uk
//
// Joel Dearden, 
// Centre for Advanced Spatial Analysis,
// University College London,
// 1-19 Torrington Place,
// London,
// WC1E 7HB
//
//
// This file is part of SLPedEvac.
//
//  SLPedEvac is free software: you can redistribute it and/or modify
//  it under the terms of the GNU General Public License as published by
//  the Free Software Foundation, either version 3 of the License, or
//  (at your option) any later version.
//
//  SLPedEvac is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU General Public License for more details.
//
//  You should have received a copy of the GNU General Public License
//  along with SLPedEvac.  If not, see <http://www.gnu.org/licenses/>.
//
//***************************************************************


//CONSTANTS START=========================================
integer controlSendCh = -220211; 
integer controlRecCh = -293321;

integer talkToPed = -1; //set on rez
integer talkToPMV = -1; //set on rez

integer talkToStairEntry = -2929221;

//integer INVALID_DESTTYPE = -1;
integer EXIT_DESTTYPE = 0;
integer SIGN_DESTTYPE = 1;
integer STAIRENTRY_DESTTYPE = 2;
//integer STAIRTOP_DESTTYPE = 3;
integer START_DESTTYPE = 4;

integer scanRange = 96; //metres



float minDirMod;
float maxDirMod;

integer fsNEITHER = 0;
integer fsLEFT = 1;
integer fsRIGHT = 2;

float rgh_thrshld_flat_xy = 1.5; //metres
float rgh_thrshld_flat_z = 2.5; //metres
float rgh_thrshld_staircase = 0.5; //metres

float localSpcRng = 2; //metres 
float minLocalSpc = 4.6; //metres squared 
float maxLocalSpc = 6.8; //metres squared 
float pedArea = 0.1; //metres square

float minFlatVel = 0.4; //m/s 
float maxFlatVel = 1.5; //m/s 

float minStaircaseVel = 0.325; //m/s 
float maxStaircaseVel = 0.8; //m/s 

integer vertPMVOffset = 4; //metres

integer maxDestMoves = 10;

//CONSTANTS END=========================================
 





//VARIABLES START=========================================
vector dest;
integer destType = START_DESTTYPE;


integer currentDestMoves = 0;

integer flatmove = TRUE;

list visitedSigns = [];

integer destCheckStep = -1;

list sensorList = [];
list rotList = [];
integer crrntSnsrListIndx = -1;
integer maxSensorListIndex = -1;

float currentVelocity = 0;

float savedMoveBearing = 0;

integer favouredSide = 0;

float cmltveDirMod = 0;
integer firstMoveCheck = TRUE;



string lastLengthMessage = "";


float slopeAngle;
float downSlopeBearing;
float staircaseBottomZ;
vector flatSlopeDirectionVector;

integer blockedCounter = 0;

integer waitingForStaircaseEntry = FALSE;

//VARIABLES END=========================================
















//FUNCTIONS START=========================================

integer signFacingMe(vector signPos, rotation signRot)
{
 
    
    vector distVec = signPos - llGetPos();
 
    vector signForwardVec = <1,0,0> * signRot;
    
    float relativeBearing = llFabs(angleBetweenVectors(distVec, signForwardVec));
    //llOwnerSay("relativeBearing=" + (string)relativeBearing);
    
    if(relativeBearing > PI)
    {
     
        relativeBearing = (2*PI) - relativeBearing;   
    }
    
    if(relativeBearing < (135 * DEG_TO_RAD))
    {
        //llOwnerSay("signFacingMe=FALSE");
        return FALSE;   
    }
    else
    {
        //llOwnerSay("signFacingMe=TRUE");
        return TRUE;   
    }
}

// getRotToPointAxisAt()
// Gets the rotation to point the specified axis at the specified position.
// @param axis The axis to point. Easiest to just use an AXIS_* constant.
// @param target The target, in region-local coordinates, to point the axis at.
// @return The rotation necessary to point axis at target.
// Created by Ope Rand, modifyed by Christopher Omega
rotation getRotToPointAxisAt(vector axis, vector target) 
{
    
    return llGetRot() * llRotBetween(axis * llGetRot(), target - llGetPos());
}



CastLOSRay(vector target)
{
    //cancel previous timeout
    llSetTimerEvent(0);
    
    //set new timeout
    llSetTimerEvent(60);
    
    //we want to fire a LOS ray at the target
    
    //llOwnerSay("firing LOS ray at" + (string)target);
    
    vector direction = target - llGetPos();
    
    vector velocity = direction / llVecMag(direction);
    
    llRezObject(    "LOSray",
                    llGetPos(),
                    velocity,
                    ZERO_ROTATION,
                    talkToPed
                    );
}

CreatePMV(float bearing)
{
    
    

    
    //rotation PMVrotation = llGetRot() * llEuler2Rot(<0,0,bearing>);
    rotation facing = llEuler2Rot(<0,0,bearing>);
    
    
    
    
    //setup vector along x-axis...
    //length is (current velocity / 2) plus half depth of pedestrian 
    //rotated by facing...
    vector createOffsetVector = <(currentVelocity/2) + 0.25,0,0> * facing;
 
    //create the pmv 
    llRezObject("PMV", llGetPos() + createOffsetVector + <0,0,vertPMVOffset>, <0,0,0>, facing, talkToPed);
    
    //make sure pmv has time to start listening
    llSleep(1.0);
    
    //send length message
    lastLengthMessage = "ln:" + (string)currentVelocity;
    
    llSay(talkToPMV, lastLengthMessage);
}




CreateSPMV(float bearing)
{
    
        
    //calculate slope vector
    vector slopeVector = <(currentVelocity/2) + 0.25,0,0>;
    //llOwnerSay("slopeVector=" + (string)slopeVector);
    
    float relativeBearing = relativeBearing(downSlopeBearing, bearing);
    //llOwnerSay("downSlopeBearing=" + (string)downSlopeBearing);
    //llOwnerSay("bearing=" + (string)bearing);
    //llOwnerSay("relativeBearing(downSlopeBearing, bearing)=" + (string)relativeBearing);
    
    slopeVector *= llEuler2Rot(<0,0,relativeBearing>);
    //llOwnerSay("slopeVector after rotation by relativeBearing=" + (string)slopeVector);
    
    
    slopeVector *= llEuler2Rot(<0,slopeAngle,0>);
    //llOwnerSay("slopeVector after rotation by slopeAngle=" + (string)slopeVector);
    
    //rotate to face bearing
    
    slopeVector *= llEuler2Rot(<0,0,-relativeBearing>); //undo relative rotation
    //llOwnerSay("slopeVector after rotation by -relativeBearing=" + (string)slopeVector);
    slopeVector *= llEuler2Rot(<0,0,bearing>); 
    //llOwnerSay("slopeVector after rotation by bearing=" + (string)slopeVector);

    //calculate horizontal and vertical components
    vector h;
    h.x = slopeVector.x;
    h.y = slopeVector.y;
    h.z = 0;
    
    vector v;
    v.x = 0;
    v.y = 0;    
    v.z = slopeVector.z;
    

    
    
    //if more than 45* off staircase bearing
    if(llFabs(relativeBearing) > (PI/2))
    {
        
        //upslope from pedestrian
        
        //rez sPMV at pedPos + flatMovementVector – verticalMovementVector + verticalOffset
        llRezObject("sPMV", llGetPos() + h + v + <0,0,vertPMVOffset>, <0,0,0>, llEuler2Rot(<0,0,bearing>), talkToPed);
    }
    else
    {
        
        //downslope from pedestrian
        
        //rez sPMV at pedPos + flatMovementVector + verticalMovementVector + verticalOffset    
        llRezObject("sPMV", llGetPos() + h - v + <0,0,vertPMVOffset>, <0,0,0>, llEuler2Rot(<0,0,bearing>), talkToPed);
    }
    
            
    //make sure pmv has time to start listening
    llSleep(1.0);
    
    //send length + slope angle + down-slope-bearing message
    lastLengthMessage = "lns:" + (string)currentVelocity + ";" + (string)slopeAngle + "/" + (string)downSlopeBearing;
    
    llSay(talkToPMV, lastLengthMessage);
}

float relativeBearing(float base, float measure)
{
 
    float angle = measure - base;
    
    return angle;
}

ResendPMVLength()
{
 
    llSay(talkToPMV, lastLengthMessage);
}

ConfirmNewPMV()
{
    llSay(talkToPMV, "confirm");    
}

ClearOldPMV()
{
    llSay(talkToPMV, "clear");    
}



integer roughlyEqualFlat(vector a, vector b)
{
        
    //llOwnerSay("roughlyEqualFlat(" + (string)a + "," + (string)b + "=");
        
    if( (llFabs(a.x - b.x) < rgh_thrshld_flat_xy) &&
        ((llFabs(a.y - b.y) < rgh_thrshld_flat_xy) && 
        (llFabs(a.z - b.z) < rgh_thrshld_flat_z))
        )
    {
    
        //llOwnerSay("TRUE");
        ////llOwnerSay("roughlyEqual(" + (string)a + "," + (string)b + ") returns TRUE"); 
        return TRUE;   
    }
    else
    {
     
        //llOwnerSay("FALSE");
        //llOwnerSay("roughlyEqual(" + (string)a + "," + (string)b + ") returns FALSE"); 
        return FALSE;   
    }       
}

integer roughlyEqualStaircase(vector a, vector b)
{
    
    //llOwnerSay("roughlyEqualStaircase(" + (string)a + "," + (string)b + "=");
    
    if( (llFabs(a.x - b.x) < rgh_thrshld_staircase) &&
        ((llFabs(a.y - b.y) < rgh_thrshld_staircase) && 
        (llFabs(a.z - b.z) < rgh_thrshld_staircase))
        )
    {
    
        //llOwnerSay("TRUE");
        ////llOwnerSay("roughlyEqual(" + (string)a + "," + (string)b + ") returns TRUE"); 
        return TRUE;   
    }
    else
    {
     
        //llOwnerSay("FALSE");
        //llOwnerSay("roughlyEqual(" + (string)a + "," + (string)b + ") returns FALSE"); 
        return FALSE;   
    }        
}

integer roughVectorInList(list checkList, vector pos)
{
    integer i;
    
    //llOwnerSay("checkList=" + (string)checkList);
    
    for(i = 0; i < llGetListLength(checkList); i++)
    {
     
        if(roughlyEqualFlat(llList2Vector(checkList, i), pos))
        {
            
            //llOwnerSay("llList2Vector(checkList, i)=" + (string)llList2Vector(checkList, i));
            //llOwnerSay("pos=" + (string)pos);
            //llOwnerSay("are the same");
            
            return TRUE;
        }
    }
    
    return FALSE;
}

//converts any bearing into a 0 to 2PI bearing
float capBearingRange(float bearing)
{
    
    if(bearing < 0)
    {
     
        //max should be -2PI
        bearing = (2 * PI) + bearing;   
    }
    else
    {
     
        if(bearing > (2 * PI))
        {
            //max should be 4PI
            bearing = bearing  - (2 * PI);
        }   
    }
    
    return bearing;
}


float distanceBetweenPos(vector posA, vector posB) 
{
 
    vector distVector;
    distVector.x = posA.x - posB.x;
    distVector.y = posA.y - posB.y;
    distVector.z = 0;
    
    float dist;
    
    dist = llSqrt(llPow((distVector.x), 2) + llPow((distVector.y), 2));
    
    return dist;
}



//returns the bearing from this object to the specified target in the range 0 to 2PI
float bearingToPos(vector target)
{
    
    //find bearing of target from this pedestrian
    
    vector pedPos = llGetPos();
    
    vector directionVector;
    
    directionVector.x = target.x - pedPos.x;
    directionVector.y = target.y - pedPos.y;
    directionVector.z = 0;
        
    float targetBearing;
    
    targetBearing = angleBetweenVectors(<1, 0, 0>, directionVector);    
    
    
    if(targetBearing < 0)
    {
     
        targetBearing = (2 * PI) + targetBearing;   
    }
    
    return targetBearing;
}


//returns the angle of measure relative to base
float angleBetweenVectors(vector base, vector measure)
{
 
        
    
    float angle;
    
    //angle of 2 relative to 1= atan2(v2.y,v2.x) - atan2(v1.y,v1.x)
    angle = llAtan2(measure.y, measure.x) - llAtan2(base.y, base.x);    
    
    return angle;
}

//returns the acute angle of b relative to a
float acuteAbsoluteAngleBetweenVectors(vector base, vector measure)
{
 
        
    
    float angle;
    
    //angle of 2 relative to 1= atan2(v2.y,v2.x) - atan2(v1.y,v1.x)
    angle = llFabs(llAtan2(measure.y, measure.x) - llAtan2(base.y, base.x));  
    
    if(angle > PI)
    {
    
        angle = (2 * PI) - angle;    
    }  
    
    return angle;
}


init()
{
    
    //set textures
    llSetTexture("f961d62a-300f-fe6d-29df-fd0e04fd1f35", ALL_SIDES);
    llSetTexture("c172efd0-d6c2-4484-b288-aebe2cab4403", 2);
    llSetTexture("44eb7ca6-1f6a-4b9f-fe7d-f51688b3dc17", 4);
        
    
    //initially we have reached our dest - forces us to choose a new one at the start;
    dest = llGetPos();
    
    //maxNormalTurn = PI / 16; //radians
    minDirMod = PI / 16; //radians
    maxDirMod = PI / 8; //radians
}

//FUNCTIONS END=========================================






















//STATES START=========================================



default
{
    state_entry()
    {
        
    }
    
    on_rez(integer start_param)
    {
                
        //llOwnerSay("llGetFreeMemory( )=" + (string)llGetFreeMemory( ));
                
        init();
        
        llListen(controlSendCh, "ABMcontroller", NULL_KEY, "");
        
        llShout(controlRecCh, "pEdiN");
    }
    
    listen(integer channel, string name, key id, string message)
    {
     
        //llOwnerSay("pedestrian: got message:" + message);
     
        //split into UUID and rayChannel
        integer dividerIndex = llSubStringIndex(message, ":");
     
        if(dividerIndex != -1)
        {
     
            //is message for this pedestrian?...     
            if((key)llGetSubString(message, 0, dividerIndex - 1) == llGetKey())
            {
                
                //...yes
                integer base = (integer)llGetSubString(message, dividerIndex + 1, -1);
                
                talkToPed = base;
                talkToPMV = base + 1; 
                
                //llOwnerSay("pedestrian: got channel range from " + (string)base + " to " + (string)(base+1));
                
                                
                state wander;
            }
        }
    }
}


state wander
{
 
    state_entry()
    {
        llSetText("wander",<0,0,0>,1);
        llListen(controlSendCh, "ABMcontroller", NULL_KEY, "");
      
        //start wander script
        llMessageLinked(LINK_THIS, talkToPed, "wander", NULL_KEY);
    }
    
    listen(integer channel, string name, key id, string message)
    {
     
        if(channel == controlSendCh)
        {
        
            if(message == "alarm")
            {
                
                //tell wander script to stop
                llMessageLinked(LINK_THIS, 0, "stopwander", NULL_KEY);
                llSleep(0.5);
                llMessageLinked(LINK_THIS, 0, "stopwander", NULL_KEY);
                llSleep(0.5);
                llMessageLinked(LINK_THIS, 0, "stopwander", NULL_KEY);
            }    
        }   
    }
    
    link_message(integer sender_num, integer num, string list_argument, key id) {
        
        
        if(list_argument == "confirmstopwander")
        {
            
            
            vector pos = llGetPos();
        
            llHTTPRequest(  "<ADDRESS OF YOUR WEB LOGGING SERVICE>/input.aspx?f=pi&px=" + 
                            (string)pos.x + 
                            "&py=" 
                            + (string)pos.y + 
                            "&pz=" 
                            + (string)pos.z +
                            "&u=" 
                            + (string)llGetKey() +
                            "&t=" + 
                            (string)llGetGMTclock(), 
                            [], 
                            ""
                            );
         
            state wait_clock;   
        }
    }
}

state wait_clock
{
    state_entry()
    {
        llSetText("wait_clock",<0,0,0>,1);
        llListen(controlSendCh, "ABMcontroller", NULL_KEY, "TiCk");
    }
    
    listen(integer channel, string name, key id, string message)
    {
        
        if(message == "TiCk")
        {
            
            //llOwnerSay("pedstrian: got tickmsg: compare " + (string)llGetPos() + " and "+ (string)dest);
            
            if(waitingForStaircaseEntry)
            {
             
                state reach_flat_dest;
            }
            
            if(flatmove)
            {
                
                
                
                if(roughlyEqualFlat(dest, llGetPos()))
                {
                    state reach_flat_dest;
                }
                else
                {
                    
                    if(currentDestMoves > maxDestMoves)
                    {
                     
                        state choose_dest;   
                    }
                    
                    state flat_move;
                }
            }
            else
            {
             
                if(roughlyEqualStaircase(dest, llGetPos()))
                {
                    state reach_staircase_bottom;
                }
                else
                {
                    state staircase_move;
                }
            }
        }   
    }
}

state reach_flat_dest
{
    state_entry()
    {
        llSetText("reach_flat_dest",<0,0,0>,1);
        if(destType == START_DESTTYPE)
        {
         
            state choose_dest;
        }
        
        if(destType == EXIT_DESTTYPE)
        {
            state exit_model;
        }
        
        if(destType == SIGN_DESTTYPE)
        {
            //Add to visited list
            
            //llOwnerSay("added exit sign to visited list");
            
            //llOwnerSay("oldList=" + (string)visitedSigns);
            visitedSigns += [dest];
            //llOwnerSay("newList=" + (string)visitedSigns);
            
            state choose_dest;
        }
        
        if(destType == STAIRENTRY_DESTTYPE)
        {
            
            //ask staircase entry for staircase top and bottom positions (include our listen channel in msg)
            llListen(talkToPed, "", NULL_KEY, "");
            llWhisper(talkToStairEntry, "sr:" + (string)talkToPed);
            
            //set timeout - staircase entry won't reply if already occupied
            llSetTimerEvent(5.0);
            
            //llOwnerSay("asking for staircase details...");
        }
    }
    
    timer()
    {
     
        //staircase must be busy - wait until next update
        llSetTimerEvent(0.0);
        state register_move;
    }
    
    listen(integer channel, string name, key id, string message)
    {
     
        if(channel == talkToPed)
        {
        
            //staircase detail message
            //
            //  FORMAT: sd:<staircaseTopPos>;<staircaseBottomPos>
        
            if(llGetSubString(message, 0, 2) == "sd:")
            {
                
                //cancel retry timer
                llSetTimerEvent(0.0);
                
                
                
                
                //break msg into parts
                integer delimiterIndex = llSubStringIndex(message, ";");
                
                vector staircaseTop = (vector) llGetSubString(message, 3, delimiterIndex - 1);
                vector staircaseBottom = (vector) llGetSubString(message, delimiterIndex + 1, -1);
                
                
                
                
                //move to staircase top immediately (or in stages if more than 10m)
                
                vector directionVector = llVecNorm(staircaseTop - llGetPos());
                
                while(llVecDist(llGetPos(), staircaseTop) > 10.0)
                {
                    
                    //five times direction vector should ensure straight line distance doesn't exceed 10m
                    llSetPos(llGetPos() + (5*directionVector));
                }
                
                llSetPos(staircaseTop);
                
                
                
                //calculate slope direction vector
                flatSlopeDirectionVector = staircaseBottom - staircaseTop;
                flatSlopeDirectionVector.z = 0;
                
                
                //calculate staircase angle
                //from basic trigonometry: slope angle = invtan(x,y dist / z dist)
                slopeAngle = llAtan2( 
                                    (staircaseTop.z - staircaseBottom.z),
                                    llSqrt(llPow((staircaseTop.x - staircaseBottom.x),2) + llPow((staircaseTop.y - staircaseBottom.y),2))
                                    );
                    
                //llOwnerSay("slopeAngle = " + (string)slopeAngle);
                    
                //calculate staircase down slope bearing
                downSlopeBearing = bearingToPos(staircaseBottom);
                
                //Set relevant staircaseBottom as dest
                dest = staircaseBottom;
                
                //save staircase bottom z so we can set it when we reach the bottom
                staircaseBottomZ = staircaseBottom.z;
                
                
                
                
                
                flatmove = FALSE;
                waitingForStaircaseEntry = FALSE;
                
                state staircase_move;
            }    
        }   
    }
}

state reach_staircase_bottom
{
    state_entry()
    {
        llSetText("reach_staircase_bottom",<0,0,0>,1);
        
        flatmove = TRUE;

        //make sure we are at floor level now        
        vector pos = llGetPos();
        pos.z = staircaseBottomZ;
        llSetPos(pos);
        
        state choose_dest;
    }
}

state choose_dest
{
     
    //this has lots of sub-states
     
    state_entry()
    {
        llSetText("choose_dest",<0,0,0>,1);
        //llOwnerSay("pedestrian: choosing dest");
        
        currentDestMoves = 0;
            
        state choose_dest_SUBSTATE_check_exitdoors;
    }
}

state choose_dest_SUBSTATE_check_exitdoors
{
    state_entry()
    {
        llSetText("choose_dest_SUBSTATE_check_exitdoors",<0,0,0>,1);
        //llOwnerSay("pedestrian: choosing dest: looking for exit?");
        
        llSensor("exit", NULL_KEY, PASSIVE | SCRIPTED, scanRange, PI);
    }
    
    sensor(integer num_detected)
    {
        
        //llOwnerSay((string)num_detected + " exits found");
        
        //put detected objects into sensor list
        
        
        
        sensorList = [];
        
        
        integer i;
        
        for(i = 0; i < num_detected; i++)
        {
        

            sensorList += [llDetectedPos(i)];
            
        }            
        
        //listen for ray info
        llListen(talkToPed, "", NULL_KEY, "");
        
        crrntSnsrListIndx = 0;
        maxSensorListIndex = num_detected;
        
        
                
        CastLOSRay(llDetectedPos(crrntSnsrListIndx));
    }
    
    no_sensor()
    {
     
        //llOwnerSay("no exits found...moving onto next dest type check..");
     
        state choose_dest_SUBSTATE_check_staircase_entries;   
    }
    
    listen(integer channel, string name, key id, string message)
    {
                
        //get visibility state of current item in list
        
        //we are checking LOS
        
        //ray info sent back will be:
        //
        //  <name of object hit>:<position of object hit>
        
        //llOwnerSay("pedestrian: got message ?:" + message);
        
        if(message == "X")
        {
         
            //llOwnerSay("nothing hit by LOS ray cast: retrying");
            
            CastLOSRay(llList2Vector(sensorList, crrntSnsrListIndx));
        }
        else
        {
        
            //should be valid collision info
            
            integer dividerIndex = llSubStringIndex(message, ":");
            
            string collisionObjectName = (string) llGetSubString(message, 0, dividerIndex - 1);
            vector collisionObjectPos = (vector) llGetSubString(message, dividerIndex + 1, -1);
            
            if(roughlyEqualFlat(collisionObjectPos,llList2Vector(sensorList, crrntSnsrListIndx)))
            {
             
                //this exit is visible!
                
                
                
                    
                    
                //llOwnerSay("pedestrian: exit at" + (string)llList2Vector(sensorList, crrntSnsrListIndx) + " is visible");
         
                //set as dest 
                dest = llList2Vector(sensorList, crrntSnsrListIndx);
                destType = EXIT_DESTTYPE;
            
                //llOwnerSay("pedestrian: my dest is an exit");
                
                state flat_move;
                
                
             
                
            }
            else
            {
             
                //this exit is not visible
                
                //llOwnerSay("pedestrian: exit at" + (string)llList2Vector(sensorList, crrntSnsrListIndx) + " is NOT visible");
                
                //llOwnerSay("crrntSnsrListIndx:" + (string)crrntSnsrListIndx + "-vs-maxSensorListIndex:" + (string)maxSensorListIndex );
                
                
                //make progress
                crrntSnsrListIndx++;
                
                //any more to check?
                if(crrntSnsrListIndx < maxSensorListIndex)
                {
                    
                    //yes
                    CastLOSRay(llList2Vector(sensorList, crrntSnsrListIndx));
                }
                else
                {
                 
                    //no
                    state choose_dest_SUBSTATE_check_staircase_entries;
                }
            }
        }        
    }
    
    timer()
    {
     
        //something wrong with LOS ray - restart
        llSetTimerEvent(0);
        state choose_dest_SUBSTATE_check_exitdoors;
    }
}

state choose_dest_SUBSTATE_check_staircase_entries
{
    state_entry()
    {
        llSetText("choose_dest_SUBSTATE_check_staircase_entries",<0,0,0>,1);
        //llOwnerSay("pedestrian: choosing dest: looking for staircase entry?");
        
        llSensor("staircaseEntry", NULL_KEY, PASSIVE | SCRIPTED, scanRange, PI);
    }
    
    sensor(integer num_detected)
    {
        
        //put detected objects into sensor list
        
        sensorList = [];
        
        integer i;
        
        for(i = 0; i < num_detected; i++)
        {
        

            sensorList += [llDetectedPos(i)];
        }            
        
        //listen for ray info
        llListen(talkToPed, "", NULL_KEY, "");
        
        crrntSnsrListIndx = 0;
        maxSensorListIndex = num_detected;
        
        CastLOSRay(llDetectedPos(crrntSnsrListIndx));
    }
    
    no_sensor()
    {
     
        state choose_dest_SUBSTATE_check_new_exit_sign;   
    }
    
    listen(integer channel, string name, key id, string message)
    {
                
        //get visibility state of current item in list
        
        //we are checking LOS
        
        //ray info sent back will be:
        //
        //  <name of object hit>:<position of object hit>
        
        if(message == "X")
        {
         
            //llOwnerSay("nothing hit by LOS ray cast: retrying");
            
            CastLOSRay(llList2Vector(sensorList, crrntSnsrListIndx));
        }
        else
        {
        
            //should be valid collision info
            
            integer dividerIndex = llSubStringIndex(message, ":");
            
            string collisionObjectName = (string) llGetSubString(message, 0, dividerIndex - 1);
            vector collisionObjectPos = (vector) llGetSubString(message, dividerIndex + 1, -1);
            
            if(roughlyEqualFlat(collisionObjectPos,llList2Vector(sensorList, crrntSnsrListIndx)))
            {
             
                //this staircaseEntry is visible!
             
                //set as dest 
                dest = llList2Vector(sensorList, crrntSnsrListIndx);
                destType = STAIRENTRY_DESTTYPE;
            
                //llOwnerSay("pedestrian: my dest is a staircase entry");
                
                state flat_move;
            }
            else
            {
             
                //this staircase entry is not visible
                
                //make progress
                crrntSnsrListIndx++;
                
                //any more to check?
                if(crrntSnsrListIndx < maxSensorListIndex)
                {
                    
                    //yes
                    
                    
                    
                    CastLOSRay(llList2Vector(sensorList, crrntSnsrListIndx));
                }
                else
                {
                 
                    //no
                    state choose_dest_SUBSTATE_check_new_exit_sign;
                }
            }
        }        
    }
    
    
    
    timer()
    {
     
        //something wrong with LOS ray - restart
        llSetTimerEvent(0);
        state choose_dest_SUBSTATE_check_staircase_entries;
    }
}

state choose_dest_SUBSTATE_check_new_exit_sign
{
    state_entry()
    {
        llSetText("choose_dest_SUBSTATE_check_new_exit_sign",<0,0,0>,1);
        //llOwnerSay("pedestrian: choosing dest: looking for new exit sign?");
        
        llSensor("exitSign", NULL_KEY, PASSIVE | SCRIPTED, scanRange, PI);
    }
    
    sensor(integer num_detected)
    {
        
        //put detected objects into sensor list
        
        sensorList = [];
        
        //also save rotation to determine sign facing
        rotList = [];
        
        integer i;

        //llOwnerSay("found the following exit signs:");
                
        for(i = 0; i < num_detected; i++)
        {
        
            //llOwnerSay(llDetectedName(i) + ":" + (string)llDetectedPos(i));
            sensorList += [llDetectedPos(i)];
            rotList += [llDetectedRot(i)];
        }            
        
        //listen for ray info
        llListen(talkToPed, "", NULL_KEY, "");
        
        crrntSnsrListIndx = 0;
        maxSensorListIndex = num_detected;
        
        //llOwnerSay("so sensor list looks like this:" + (string)sensorList);
        //llOwnerSay("crrntSnsrListIndx=" + (string)crrntSnsrListIndx);
        //llOwnerSay("maxSensorListIndex=" + (string)maxSensorListIndex);

        
        CastLOSRay(llDetectedPos(crrntSnsrListIndx));
    }
    
    no_sensor()
    {
     
        state choose_dest_SUBSTATE_check_old_exit_sign;   
    }
    
    listen(integer channel, string name, key id, string message)
    {
                
        //get visibility state of current item in list
        
        //we are checking LOS
        
        //ray info sent back will be:
        //
        //  <name of object hit>:<position of object hit>
        
        if(message == "X")
        {
         
            //llOwnerSay("nothing hit by LOS ray cast: retrying");

                        
            //recast LOS ray for current exit sign
            CastLOSRay(llList2Vector(sensorList, crrntSnsrListIndx));
        }
        else
        {
        
            //should be valid collision info
            
            integer dividerIndex = llSubStringIndex(message, ":");
            
            string collisionObjectName = (string) llGetSubString(message, 0, dividerIndex - 1);
            vector collisionObjectPos = (vector) llGetSubString(message, dividerIndex + 1, -1);
            
            if(roughlyEqualFlat(collisionObjectPos,llList2Vector(sensorList, crrntSnsrListIndx)))
            {
             
                //this exitsign is visible!
                                
                
                //llOwnerSay("pedestrian: exit sign at" + (string)llList2Vector(sensorList, crrntSnsrListIndx) + " is visible");
                
                //have we been there before?
                if(roughVectorInList(visitedSigns, collisionObjectPos) == FALSE)
                {
                 
                    //no!  
                    
                    //BUT is it facing the right way?
                    if(signFacingMe(llList2Vector(sensorList, crrntSnsrListIndx), llList2Rot(rotList, crrntSnsrListIndx)))
                    {
                 
                        //yes
                        
                        //llOwnerSay("visitedSigns=" + (string)visitedSigns);
                    
                        //llOwnerSay("pedestrian: ...and we haven't been there before...so...");
                        
                        //set as dest 
                        dest = llList2Vector(sensorList, crrntSnsrListIndx);
                        destType = SIGN_DESTTYPE;
                    
                        //llOwnerSay("pedestrian: my dest is a new exit sign");
                        
                        state flat_move;
                    }
                    else
                    {
                     
                        //no
                        
                        //we've been there before
    
                        //llOwnerSay("pedestrian: ...but we have been there before...so will keep looking");
                        
        
                        //make progress
                        crrntSnsrListIndx++;
                        
                        //any more to check?
                        if(crrntSnsrListIndx < maxSensorListIndex)
                        {
                            
                            //yes
                            
                            CastLOSRay(llList2Vector(sensorList, crrntSnsrListIndx));
                        }
                        else
                        {
                         
                            //no
                            state choose_dest_SUBSTATE_check_old_exit_sign;
                        }
                    }
                    
                    
                }
                else
                {
                 
                    //yes  
                    
                    //we've been there before
    
                    //llOwnerSay("pedestrian: ...but we have been there before...so will keep looking");
                    
    
                    //make progress
                    crrntSnsrListIndx++;
                    
                    //any more to check?
                    if(crrntSnsrListIndx < maxSensorListIndex)
                    {
                        
                        //yes
                        
                        CastLOSRay(llList2Vector(sensorList, crrntSnsrListIndx));
                    }
                    else
                    {
                     
                        //no
                        state choose_dest_SUBSTATE_check_old_exit_sign;
                    }
                }
             
                
            }
            else
            {
             
                //this exit sign is not visible
                
                //llOwnerSay("pedestrian: exit sign at" + (string)llList2Vector(sensorList, crrntSnsrListIndx) + " is NOT visible");
                
                //make progress
                crrntSnsrListIndx++;
                
                //any more to check?
                if(crrntSnsrListIndx < maxSensorListIndex)
                {
                    
                    //yes
                    
                    
                    
                    CastLOSRay(llList2Vector(sensorList, crrntSnsrListIndx));
                }
                else
                {
                 
                    //no
                    state choose_dest_SUBSTATE_check_old_exit_sign;
                }
            }
        }        
    }
    
    
    
    timer()
    {
     
        //something wrong with LOS ray - restart
        llSetTimerEvent(0);
        state choose_dest_SUBSTATE_check_new_exit_sign;
    }
}

state choose_dest_SUBSTATE_check_old_exit_sign
{
    state_entry()
    {
        llSetText("choose_dest_SUBSTATE_check_old_exit_sign",<0,0,0>,1);
        //llOwnerSay("pedestrian: choosing dest: looking for old exit sign?");
        
        llSensor("exitSign", NULL_KEY, PASSIVE | SCRIPTED, scanRange, PI);
    }
    
    sensor(integer num_detected)
    {
        
        //put detected objects into sensor list
        
        sensorList = [];
        
        //also save rotation to determine sign facing
        rotList = [];
        
        integer i;
        
        for(i = 0; i < num_detected; i++)
        {
        

            sensorList += [llDetectedPos(i)];
            rotList += [llDetectedRot(i)];
        }            
        
        //listen for ray info
        llListen(talkToPed, "", NULL_KEY, "");
        
        crrntSnsrListIndx = 0;
        maxSensorListIndex = num_detected;
        
        CastLOSRay(llDetectedPos(crrntSnsrListIndx));
    }
    
    no_sensor()
    {
     
        state choose_dest_SUBSTATE_check_any_exit_sign;   
    }
    
    listen(integer channel, string name, key id, string message)
    {
                
        //get visibility state of current item in list
        
        //we are checking LOS
        
        //ray info sent back will be:
        //
        //  <name of object hit>:<position of object hit>
        
        if(message == "X")
        {
                
            //llOwnerSay("nothing hit by LOS ray cast: retrying");
            
            CastLOSRay(llList2Vector(sensorList, crrntSnsrListIndx));
        }
        else
        {
        
            //should be valid collision info
            
            integer dividerIndex = llSubStringIndex(message, ":");
            
            string collisionObjectName = (string) llGetSubString(message, 0, dividerIndex - 1);
            vector collisionObjectPos = (vector) llGetSubString(message, dividerIndex + 1, -1);
            
            if(roughlyEqualFlat(collisionObjectPos,llList2Vector(sensorList, crrntSnsrListIndx)))
            {
             
                //this exitsign is visible!
             
                //we don't care now if its an old one!
                
                //BUT is it facing the right way?
                if(signFacingMe(llList2Vector(sensorList, crrntSnsrListIndx), llList2Rot(rotList, crrntSnsrListIndx)))
                {
                    
                    //yes
                    
                    //set as dest 
                    dest = llList2Vector(sensorList, crrntSnsrListIndx);
                    destType = SIGN_DESTTYPE;
                
                    //llOwnerSay("pedestrian: my dest is an old exit sign");
                    
                    state flat_move;
                }
                else
                {
                    
                    //no
                    
                    //make progress
                    crrntSnsrListIndx++;
                    
                    //any more to check?
                    if(crrntSnsrListIndx < maxSensorListIndex)
                    {
                        
                        //yes
                        
                        
                        
                        CastLOSRay(llList2Vector(sensorList, crrntSnsrListIndx));
                    }
                    else
                    {
                     
                        //no
                        state choose_dest_SUBSTATE_check_any_exit_sign;
                    }
                }
                
            }
            else
            {
             
                //this exitsign is not visible
                
                //make progress
                crrntSnsrListIndx++;
                
                //any more to check?
                if(crrntSnsrListIndx < maxSensorListIndex)
                {
                    
                    //yes
                    
                    
                    
                    CastLOSRay(llList2Vector(sensorList, crrntSnsrListIndx));
                }
                else
                {
                 
                    //no
                    state choose_dest_SUBSTATE_check_any_exit_sign;
                }
            }
        }        
    }
    
    
    
    timer()
    {
     
        //something wrong with LOS ray - restart
        llSetTimerEvent(0);
        state choose_dest_SUBSTATE_check_old_exit_sign;
    }
}

state choose_dest_SUBSTATE_check_any_exit_sign
{
    state_entry()
    {
        llSetText("choose_dest_SUBSTATE_check_any_exit_sign",<0,0,0>,1);
        //llOwnerSay("pedestrian: choosing dest: looking for any exit sign?");
        
        llSensor("exitSign", NULL_KEY, PASSIVE | SCRIPTED, scanRange, PI);
    }
    
    sensor(integer num_detected)
    {
        
        //there is a exit sign nearby
        
        //just pick the closest one
             
        //we don't care now if visible or not 
        
        //we don't care if its an old one!
            
        //set as dest 
        dest = llDetectedPos(0);    
        destType = SIGN_DESTTYPE;
        
        //llOwnerSay("pedestrian: my dest is any exit sign");
        
        state flat_move;
    }
    
    no_sensor()
    {
     
        //llOwnerSay("ERROR: I can't find any exit signs!");
    }
}

state register_move
{
    state_entry()
    {
        llSetText("register_move",<0,0,0>,1);
        //llOwnerSay("pedestrian: registering move");
        
        currentDestMoves++;
        
        llListen(controlSendCh, "", NULL_KEY, "");
        llShout(controlRecCh, "RegMoV");
        
        //log move
        vector pos = llGetPos(); 
        
        llHTTPRequest(  "<ADDRESS OF YOUR WEB LOGGING SERVICE>/input.aspx?f=pu&px=" + 
                        (string)pos.x + 
                        "&py=" 
                        + (string)pos.y + 
                        "&pz=" 
                        + (string)pos.z +
                        "&u=" 
                        + (string)llGetKey() +
                        "&t=" + 
                        (string)llGetGMTclock() +
                        "&s=" + (string)currentVelocity, 
                        [], 
                        ""
                        );
        
        llSetTimerEvent(10);
    }
    
    listen(integer channel, string name, key id, string message)
    {
        
        if(llGetSubString(message, 0, 2) == "ACK")
        {
         
            key test = (key) llGetSubString(message, 3, -1);
            
            //llOwnerSay("checking keys:" + (string)test + " against " + (string)llGetKey());
            
            if(test == llGetKey())
            {
                //llOwnerSay("match");
                llSetTimerEvent(0.0);
                state wait_clock;
            }
            else
            {
                //llOwnerSay("no match");
            }
        }
    }
    
    timer()
    {
        //llOwnerSay("resending regmove");
        llShout(controlRecCh, "RegMoV");
    }
}


state flat_move
{
    state_entry()
    {
        llSetText("flat_move",<0,0,0>,1);
        //this just goes through three substates
        
        //1) look 
        //2) think
        //3) act
          
        state flat_move_SUBSTATE_look;
    }
}

state flat_move_SUBSTATE_look
{
    state_entry()
    {
        llSetText("flat_move_SUBSTATE_look",<0,0,0>,1);
        //DEBUG
        //llOwnerSay("look");
        
        
        //scan for pedestrians nearby to determine local space for velocity update
        llSensor("pedestrian", NULL_KEY, PASSIVE | SCRIPTED, localSpcRng, PI_BY_TWO);
    }
    
    no_sensor()
    {
     
        //no pedestrians nearby so move at maximum speed
        currentVelocity = maxFlatVel;
        
        state flat_move_SUBSTATE_raycast_think;
    }
    
    sensor(integer total_number) // total_number is the number of objects detected.
    {
        
        //this can be optimised but only when we know it works!
        
        //calculate total free space
        float currentSpace = maxLocalSpc - (total_number * pedArea);
                
        //calculate velocity multiplier
        float velMult = (currentSpace - minLocalSpc) / (maxLocalSpc - minLocalSpc);
        
        //calculate current velocity
        currentVelocity = minFlatVel + (velMult * (maxFlatVel - minFlatVel));
                        
        state flat_move_SUBSTATE_raycast_think;
    }
}


state flat_move_SUBSTATE_raycast_think
{
    
    
    
    
    state_entry()
    {
        llSetText("flat_move_SUBSTATE_raycast_think",<0,0,0>,1);
        //clear any existing PMV - we only ever want one
        ClearOldPMV();
        
        
        //enter state and set move direction as ATCF
        savedMoveBearing = bearingToPos(dest);
        //favouredSide = fsNEITHER;
        
        
        //listen for PMV replies
        llListen(talkToPed, "", NULL_KEY, "");
        
        //start looping move-test process...
        cmltveDirMod = 0;
        //llOwnerSay("cmltveDirMod=0");
        firstMoveCheck = TRUE;
                        
        CreatePMV(savedMoveBearing);        
    }
    
    listen(integer channel, string name, key id, string message)
    {
     
        
        //llOwnerSay("flat_move_ped:got msg:" + message);
        
        if(message == "pmvbroken")
        {
            
            //rez a replacement
            CreatePMV(capBearingRange(savedMoveBearing + cmltveDirMod));
        }
        
        
        if(message == "lostmsg")
        {
         
            //PMV hasn't received length message
            
            //resend previous message
            ResendPMVLength();
        }
        
        if(message == "safe")
        {
         
            //move OK - keep the new PMV                    
            savedMoveBearing += cmltveDirMod;
            
            if(firstMoveCheck == TRUE)
            {
             
                //ATCF move worked - reset favoured side
                favouredSide = fsNEITHER;   
            }
            
            //fix the new PMV
            ConfirmNewPMV();
            
            
            blockedCounter = 0;
            
            state flat_move_SUBSTATE_act;  
        }
        
        if(message == "unsafe")
        {
                    
            //the move is not safe 
            //(the PMV will delete itself)
                    
            // otherwise modify move and then repeat    
            
            //when a pedestrian modifies its move it has obviously reached an obstacle and will slow down
            //e.g you don't sprint round a table you go round it at half speed or less and then speed up afterwards
            //so...
            if(firstMoveCheck == TRUE)
            {
    
                firstMoveCheck = FALSE;
                currentVelocity = currentVelocity/2;
                //we only cut velocity in half once
            }
            
            
            //Other moves means testing each of the following possibilities until we find a match
            
            //Change move by random angle away from ATCF – try (randomly) ALL LEFT or ALL RIGHT versions
            
            
        
    
            if(llFabs(cmltveDirMod) < ((2*PI)/3))
            {
                
                
                                
                //if we have a favoured side keep trying that
                if(favouredSide == fsLEFT)
                {
                    
                    //keep left   
                                     
                    cmltveDirMod += minDirMod + llFrand(maxDirMod - minDirMod);
                    //llOwnerSay("cmltveDirMod=" + (string)cmltveDirMod);
                    
                    favouredSide = fsLEFT;
                    
                    CreatePMV(capBearingRange(savedMoveBearing + cmltveDirMod));  
                }
                else
                {
                 
                    if(favouredSide == fsRIGHT)
                    {
                    
                        //keep right                        
                        
                        cmltveDirMod -= minDirMod + llFrand(maxDirMod - minDirMod);
                        //llOwnerSay("cmltveDirMod=" + (string)cmltveDirMod);
                        
                        favouredSide = fsRIGHT;
                        
                        CreatePMV(capBearingRange(savedMoveBearing + cmltveDirMod));    
                    }   
                    else
                    {
                        
                        //random   
                        
                        if(llFrand(1.0) > 0.5)
                        {
                        
                            //try left
                        
                            cmltveDirMod += minDirMod + llFrand(maxDirMod - minDirMod);
                            //llOwnerSay("cmltveDirMod=" + (string)cmltveDirMod);
                            
                            favouredSide = fsLEFT;
                            
                            CreatePMV(capBearingRange(savedMoveBearing + cmltveDirMod));    
                        }
                        else
                        {
                         
                            //try right                        
                           
                            cmltveDirMod -= minDirMod + llFrand(maxDirMod - minDirMod);
                            //llOwnerSay("cmltveDirMod=" + (string)cmltveDirMod);
                        
                            favouredSide = fsRIGHT;
                            
                            CreatePMV(capBearingRange(savedMoveBearing + cmltveDirMod));    
                        }
                    }                    
                }
            }
            else
            {
                
                //llOwnerSay("cmltveDirMod>PI so agent not moving this update");
                
                //If we reach here then the agent can’t move this update
                
                
                //set the other side as favoured so we don't keep repeating the same mistake
                
                if(favouredSide == fsLEFT)
                {
                 
                    favouredSide = fsRIGHT;
                }
                else
                {
                    if(favouredSide == fsRIGHT)
                    {
                     
                        favouredSide = fsLEFT;
                    }
                }
                
                //check for agents that have been jammed for a long time
                blockedCounter++;
                
                if(blockedCounter > 3)
                {
                    state unjam_agent;
                }
                else
                {
                
                    //favouredSide = fsNEITHER;
                    state register_move;
                }
            }
        }     
    }
}

state unjam_agent
{
 
    state_entry()
    {
        llSetText("unjam_agent",<0,0,0>,1);
        //llOwnerSay("I'm blocked what do I do?");
        
        
        state register_move;
    }   
}

state flat_move_SUBSTATE_act
{
    
    state_entry()
    {
        llSetText("flat_move_SUBSTATE_act",<0,0,0>,1);
        //DEBUG
        //llOwnerSay("act");
        
        

        //Move the pedestrian a at current velocity in the chosen direction   
        
        vector eulerBearing = <0, 0, savedMoveBearing>;
        rotation quatBearing = llEuler2Rot(eulerBearing);
        
        //need to rotate to face direction we are moving
        llSetRot(quatBearing);
        
        vector moveVector;
        
        //moveVector = <0, 1, 0> * (halfMoveRange + llFrand(halfMoveRange));
        moveVector = <currentVelocity, 0, 0>;
        moveVector = moveVector * quatBearing;
        
        vector position = llGetPos();
        
        position += moveVector;
        
        llSetPos(position);
        
        //log move
        vector pos = llGetPos();
        
        //llHTTPRequest(  "<ADDRESS OF YOUR WEB LOGGING SERVICE>/input.aspx?f=pu&px=" + 
        //                (string)pos.x + 
          //              "&py=" 
            //            + (string)pos.y + 
              //          "&u=" 
                ///        + (string)llGetKey() +
                   ///     "&t=" + 
                      //  (string)llGetGMTclock(), 
                        //[], 
                        //""
                        //);
        
        state register_move;
    }
}

state staircase_move
{
    state_entry()
    {
        llSetText("staircase_move",<0,0,0>,1);
        //this just goes through three substates
        
        //1) look 
        //2) think
        //3) act
          
        state staircase_move_SUBSTATE_look;
    }
}

state staircase_move_SUBSTATE_look
{
    state_entry()
    {
        llSetText("staircase_move_SUBSTATE_look",<0,0,0>,1);
        //DEBUG
        //llOwnerSay("look");
        
        
        //scan for pedestrians nearby to determine local space for velocity update
        llSensor("pedestrian", NULL_KEY, PASSIVE | SCRIPTED, localSpcRng, PI_BY_TWO);
    }
    
    no_sensor()
    {
     
        //no pedestrians nearby so move at maximum speed
        currentVelocity = maxStaircaseVel;
        
        state staircase_move_SUBSTATE_raycast_think;
    }
    
    sensor(integer total_number) // total_number is the number of objects detected.
    {
        
        //this can be optimised but only when we know it works!
        
        //calculate total free space
        float currentSpace = maxLocalSpc - (total_number * pedArea);
                
        //calculate velocity multiplier
        float velMult = (currentSpace - minLocalSpc) / (maxLocalSpc - minLocalSpc);
        
        //calculate current velocity
        currentVelocity = minStaircaseVel + (velMult * (maxStaircaseVel - minStaircaseVel));
                        
        state staircase_move_SUBSTATE_raycast_think;
    }
}


state staircase_move_SUBSTATE_raycast_think
{
    
    
    
    
    state_entry()
    {
        llSetText("staircase_move_SUBSTATE_raycast_think",<0,0,0>,1);
        //clear any existing PMV - we only ever want one
        ClearOldPMV();
        
        //face parallel to slope decline
        //llSetRot(llEuler2Rot(<0,0,downSlopeBearing>));
        
        
        //enter state and set move direction as ATCF
        savedMoveBearing = bearingToPos(dest);
        //favouredSide = fsNEITHER;
        
        
        //listen for PMV replies
        llListen(talkToPed, "", NULL_KEY, "");
        
        //start looping move-test process...
        cmltveDirMod = 0;
        //llOwnerSay("cmltveDirMod=0");
        firstMoveCheck = TRUE;    
                        
        CreateSPMV(savedMoveBearing);        
    }
    
    listen(integer channel, string name, key id, string message)
    {
     
        
        if(message == "pmvbroken")
        {
            
            //rez a replacement
            CreateSPMV(capBearingRange(savedMoveBearing + cmltveDirMod));
        }
        
        
        if(message == "lostmsg")
        {
         
            //PMV hasn't received length message
            
            //resend previous message
            ResendPMVLength();
        }
        
        if(message == "safe")
        {
         
            //move OK - keep the new PMV                    
            savedMoveBearing += cmltveDirMod;
            
            if(firstMoveCheck == TRUE)
            {
             
                //ATCF move worked - reset favoured side
                favouredSide = fsNEITHER;   
            }
            
            //fix the new PMV
            ConfirmNewPMV();
            
            
            state staircase_move_SUBSTATE_act;  
        }
        
        if(message == "unsafe")
        {
                    
            //the move is not safe 
            //(the PMV will delete itself)
                    
            // otherwise modify move and then repeat    
            
            //when a pedestrian modifies its move it has obviously reached an obstacle and will slow down
            //e.g you don't sprint round a table you go round it at half speed or less and then speed up afterwards
            //so...
            if(firstMoveCheck == TRUE)
            {
    
                firstMoveCheck = FALSE;
                currentVelocity = currentVelocity/2;
                //we only cut velocity in half once
            }
            
            
            //Other moves means testing each of the following possibilities until we find a match
            
            //Change move by random angle away from ATCF – try (randomly) ALL LEFT or ALL RIGHT versions
            
            
        
    
            if(llFabs(cmltveDirMod) < (PI/2))
            {
                
                
                                
                //if we have a favoured side keep trying that
                if(favouredSide == fsLEFT)
                {
                    
                    //keep left   
                                     
                    cmltveDirMod += minDirMod + llFrand(maxDirMod - minDirMod);
                    //llOwnerSay("cmltveDirMod=" + (string)cmltveDirMod);
                    
                    favouredSide = fsLEFT;
                    
                    CreateSPMV(capBearingRange(savedMoveBearing + cmltveDirMod));  
                }
                else
                {
                 
                    if(favouredSide == fsRIGHT)
                    {
                    
                        //keep right                        
                        
                        cmltveDirMod -= minDirMod + llFrand(maxDirMod - minDirMod);
                        //llOwnerSay("cmltveDirMod=" + (string)cmltveDirMod);
                        
                        favouredSide = fsRIGHT;
                        
                        CreateSPMV(capBearingRange(savedMoveBearing + cmltveDirMod));    
                    }   
                    else
                    {
                        
                        //random   
                        
                        if(llFrand(1.0) > 0.5)
                        {
                        
                            //try left
                        
                            cmltveDirMod += minDirMod + llFrand(maxDirMod - minDirMod);
                            //llOwnerSay("cmltveDirMod=" + (string)cmltveDirMod);
                            
                            favouredSide = fsLEFT;
                            
                            CreateSPMV(capBearingRange(savedMoveBearing + cmltveDirMod));    
                        }
                        else
                        {
                         
                            //try right                        
                           
                            cmltveDirMod -= minDirMod + llFrand(maxDirMod - minDirMod);
                            //llOwnerSay("cmltveDirMod=" + (string)cmltveDirMod);
                        
                            favouredSide = fsRIGHT;
                            
                            CreateSPMV(capBearingRange(savedMoveBearing + cmltveDirMod));    
                        }
                    }                    
                }
            }
            else
            {
                
                //llOwnerSay("cmltveDirMod>PI so agent not moving this update");
                
                //If we reach here then the agent can’t move this update
                
                //set the other side as favoured so we don't keep repeating the same mistake
                
                if(favouredSide == fsLEFT)
                {
                 
                    favouredSide = fsRIGHT;
                }
                
                if(favouredSide == fsRIGHT)
                {
                 
                    favouredSide = fsLEFT;
                }
                
                //favouredSide = fsNEITHER;
                state register_move;
            }
        }     
    }
}

state staircase_move_SUBSTATE_act
{
    
    state_entry()
    {
        llSetText("staircase_move_SUBSTATE_act",<0,0,0>,1);
        //DEBUG
        //llOwnerSay("act");
        
        //rotate to face direction we are moving
        llSetRot(llEuler2Rot(<0,0,savedMoveBearing>));

        //Move the pedestrian at current velocity in the chosen direction on the staircase plane
        
        //calculate horizontal component of offset
        vector moveVector = <currentVelocity, 0, 0>;
        //llOwnerSay("moveVector=" + (string)moveVector);
        
        float relativeBearing = relativeBearing(downSlopeBearing, savedMoveBearing);
        //llOwnerSay("downSlopeBearing=" + (string)downSlopeBearing);
        //llOwnerSay("savedMoveBearing=" + (string)savedMoveBearing);
        //llOwnerSay("relativeBearing(downSlopeBearing, savedMoveBearing)=" + (string)relativeBearing);
        
        moveVector *= llEuler2Rot(<0,0,relativeBearing>);
        //llOwnerSay("moveVector after rotation by relativeBearing=" + (string)moveVector);
        
        
        moveVector *= llEuler2Rot(<0,slopeAngle,0>);
        //llOwnerSay("moveVector after rotation by slopeAngle=" + (string)moveVector);
        
        //rotate to face bearing
        
        moveVector *= llEuler2Rot(<0,0,-relativeBearing>); //undo relative rotation
        //llOwnerSay("moveVector after rotation by -relativeBearing=" + (string)moveVector);
        moveVector *= llEuler2Rot(<0,0,savedMoveBearing>); 
        //llOwnerSay("moveVector after rotation by savedMoveBearing=" + (string)moveVector);
    
        
        
        llSetPos(llGetPos() + moveVector);
                                
    
        
        
        state register_move;
    }
}

state exit_model
{
    state_entry()
    {
        llSetText("exit_model",<0,0,0>,1);
        llShout(controlRecCh, "pedOUt");
        
        //delete any remaining PMVs
        ClearOldPMV();
        
        
        //log exit with web server
        vector pos = llGetPos();
            
        llHTTPRequest(  "<ADDRESS OF YOUR WEB LOGGING SERVICE>/input.aspx?f=po&px=" + 
                        (string)pos.x + 
                        "&py=" 
                        + (string)pos.y + 
                        "&pz=" 
                        + (string)pos.z +
                        "&u=" 
                        + (string)llGetKey() +
                        "&t=" + 
                        (string)llGetGMTclock(), 
                        [], 
                        ""
                        );
    
        llDie();   
    }
    
}

//STATES END=========================================