//***************************************************************
// 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 viewRange = 0.5; //metres
//float halfMoveRange = 0.125; //metres

//NOTE: bearing taken from X-axis at zero rotation
//vector zeroDirectionVector = <1, 0, 0>; //arbitrary?
//NOTE: bearing taken from X-axis at zero rotation
//
//  right hand rule means:
//
//      - positive Z rotation is anti-clockwise when viewed from above
//

//float lineBarrierLength = 3.1; //metres (added 0.1 metres to prevent agents escaping from small gaps between barriers)

//float pointBarrierArcSize;
//float lineBarrierArcSize;
//float pedestrianArcSize;
//float exitArcSize;

float minDirMod;
float maxDirMod;

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

float rough_threshold_flat = 2; //metres
float rough_threshold_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 verticalPMVOffset = 4; //metres

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






//TODO START=========================================
//1-make rays temp on rez
//2-make new ray types los & collision
//3-update and shorten ray messages 
//4-make rays long and walls thick
//http://wiki.secondlife.com/wiki/Bullet
//TODO END=========================================







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

integer flatmove = TRUE;

list visitedSigns = [];

integer destCheckStep = -1;

list sensorList = [];
integer currentSensorListIndex = -1;
integer maxSensorListIndex = -1;

//list visiblePointBarrierList = [];
//list visibleLineBarrierPosList = [];
//list visibleLineBarrierRotList = [];
//list visiblePedestrianList = [];
//list visibleExitDoorList = [];

float currentVelocity = 0;

float savedMoveBearing = 0;

integer favouredSide = 0;

float cumulativeDirectionModifier = 0;
integer firstMoveCheck = TRUE;

integer usePMV_a = TRUE; //flip flop toggle switch (a, b, a, b, a, ...)


string lastLengthMessage = "";

integer atStaircaseTop = FALSE;
float slopeAngle;
float downSlopeBearing;
float staircaseBottomZ;
vector flatSlopeDirectionVector;

integer lastMoveSafe = FALSE;

integer stopWandering = FALSE;

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
















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

// 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)
//{
    
//    llOwnerSay("pedestrian: firing LOS ray at: " + (string)target);
            
        
    //rotation facing = llEuler2Rot(<0, 0, bearingToPos(target)>);
        
    //llSetRot(facing);
    
    //get rotation between local y-axis and direction vector to target
//    rotation rezRot = (llRotBetween(<0,1,0> * llGetRot(), (target - llGetPos())));
    
    
 
    //start pos is 0.05 metres towards target (ray is 0.1m long)
    //vector startPos = llGetPos() + ((<0, 0.05, 0> * llGetRot()) * rezRot);
    
//    llRezObject("LOSray", llGetPos(), <0, 0, 0>, llGetRot() * rezRot, rayChannel);   
//}

CastLOSRay(vector target)
{
 
    //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>);
    
    
    float PMVLength = currentVelocity + 0.125;
    
    //setup vector along x-axis...
    //scaled by current velocity...
    //rotated by facing...
    vector createOffsetVector = <PMVLength/2,0,0> * facing;
 
    //create the pmv 
    llRezObject("PMV", llGetPos() + createOffsetVector + <0,0,verticalPMVOffset>, <0,0,0>, facing, talkToPed);
    
    //make sure pmv has time to start listening
    llSleep(1.0);
    
    //send length message
    lastLengthMessage = "ln:" + (string)PMVLength;
    
    llSay(talkToPMV, lastLengthMessage);
}

//this is the old version
CreateStaircasePMV(float bearing)
{
    
        
    //calculate horizontal component of offset
    vector baseOffsetH = <currentVelocity/2, 0, 0>;
    llOwnerSay("baseOffsetH=" + (string)baseOffsetH);
    
    float relativeBearing = relativeBearing(downSlopeBearing, bearing);
    llOwnerSay("downSlopeBearing=" + (string)downSlopeBearing);
    llOwnerSay("bearing=" + (string)bearing);
    llOwnerSay("relativeBearing(downSlopeBearing, bearing)=" + (string)relativeBearing);
    
    baseOffsetH *= llEuler2Rot(<0,0,relativeBearing>);
    llOwnerSay("baseOffsetH after rotation by relativeBearing=" + (string)baseOffsetH);
    
    
    baseOffsetH *= llEuler2Rot(<0,slopeAngle,0>);
    llOwnerSay("baseOffsetH after rotation by slopeAngle=" + (string)baseOffsetH);
    
    //rotate to face bearing
    
    baseOffsetH *= llEuler2Rot(<0,0,-relativeBearing>); //undo relative rotation
    llOwnerSay("baseOffsetH after rotation by -relativeBearing=" + (string)baseOffsetH);
    baseOffsetH *= llEuler2Rot(<0,0,bearing>); 
    llOwnerSay("baseOffsetH after rotation by bearing=" + (string)baseOffsetH);

    //got horizontal component..
    
    //==
    
    //calculate vertical component of offset
    vector baseOffsetV = <0, 0, verticalPMVOffset>;
    llOwnerSay("baseOffsetV=" + (string)baseOffsetV);
            
    baseOffsetV *= llEuler2Rot(<0,slopeAngle,0>);
    llOwnerSay("baseOffsetV after rotation by slopeAngle=" + (string)baseOffsetV);
    
    //rotate to down slope bearing    
    baseOffsetV *= llEuler2Rot(<0,0,downSlopeBearing>); 
    llOwnerSay("baseOffsetV after rotation by downSlopeBearing=" + (string)baseOffsetV);

    //got vertical component..
    
    
   
    //create the staircase pmv 
    llRezObject("sPMV", llGetPos() + baseOffsetH + baseOffsetV, <0,0,0>, llEuler2Rot(<0,0,bearing>), talkToPed);
    
    //make sure pmv has time to start listening
    //llSleep(1000);
    
    //send length + slope angle + down-slope-bearing message
    lastLengthMessage = "lns:" + (string)currentVelocity + ";" + (string)slopeAngle + "/" + (string)downSlopeBearing;
    
    llSay(talkToPMV, lastLengthMessage);
}


CreateSPMV(float bearing)
{
    
        
    //calculate slope vector
    vector slopeVector = <currentVelocity/2, 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,verticalPMVOffset>, <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,verticalPMVOffset>, <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");    
}

//CastCOLLISIONRay(float bearing)
//{
    
//    rotation facing = llEuler2Rot(<0, 0, bearing>);
    
        
//    llOwnerSay("pedestrian: firing COLLISION ray on bearing: " + (string)bearing);
    
    //prevent looking up and down    ?
    //vector casterPos = llGetPos();
    //target.z = casterPos.z;
    
    
    //llRotLookAt(getRotToPointAxisAt(<0,1,0>, target), 1.0, 1.0);
//    llSetRot(facing);
 
    //start pos is 0.6 metres along positive Y-axis
//    vector startPos = llGetPos() + (<0, 0.6, 0> * facing);
    
//    llRezObject("COLLISIONray", startPos, <0, 0, 0>, facing, rayChannel);   
//}


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

integer roughlyEqualStaircase(vector a, vector b)
{
    
    if( (llFabs(a.x - b.x) < rough_threshold_staircase) &&
        ((llFabs(a.y - b.y) < rough_threshold_staircase) && 
        (llFabs(a.z - b.z) < rough_threshold_staircase))
        )
    {
    
        //llOwnerSay("roughlyEqual(" + (string)a + "," + (string)b + ") returns TRUE"); 
        return TRUE;   
    }
    else
    {
     
        //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();
    
    //pointBarrierArcSize = PI / 16; //radians
    //lineBarrierArcSize = PI / 2; //radians
    //pedestrianArcSize = PI / 2; //radians
    //exitArcSize = PI / 8; //radians
    
    //maxNormalTurn = PI / 16; //radians
    minDirMod = PI / 16; //radians
    maxDirMod = PI / 8; //radians
}

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




















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



default
{
    state_entry()
    {
        llOwnerSay("wander script ready");
        
        state wait;
    }
}

state wait
{
 
    link_message(integer sender_num, integer num, string list_argument, key id) {
        
        
        if(list_argument == "wander")
        {
            llOwnerSay("got message: wander on");
            
            talkToPed = num;
            talkToPMV = num + 1; 
            
            state wander;
        }
    }   
}

state wander
{
    state_entry()
    {
        
        if(stopWandering)
        {
         
            llOwnerSay("cancelled wander script");
            
            ClearOldPMV();
            
            
            llSleep(1.0);
            
            //send confirmation to main script
            llMessageLinked(LINK_THIS, 0, "confirmstopwander", NULL_KEY);
         
            state wait;   
        }
        
        //this just goes through three substates
        
        //1) look 
        //2) think
        //3) act
          
        state wander_SUBSTATE_look;
    }
    
    link_message(integer sender_num, integer num, string list_argument, key id) {
        
        if(list_argument == "stopwander")
        {
         
            stopWandering = TRUE; 
        }        
    }   
}

state wander_SUBSTATE_look
{
    state_entry()
    {
        
        
        
        //calculate current velocity
        currentVelocity = minFlatVel;
                        
        state wander_SUBSTATE_raycast_think;
    }
    
    link_message(integer sender_num, integer num, string list_argument, key id) {
        
        if(list_argument == "stopwander")
        {
         
            stopWandering = TRUE; 
        }        
    }    
}


state wander_SUBSTATE_raycast_think
{
    
    
    
    
    state_entry()
    {
        
        //clear any existing PMV - we only ever want one
        ClearOldPMV();
        
        
        //if last move not safe set new random direction
        if(lastMoveSafe != TRUE)
        {
            
            savedMoveBearing = llFrand(2 * PI);
        }
        
        
        
        //listen for PMV replies
        llListen(talkToPed, "", NULL_KEY, "");
        
        //start looping move-test process...
        cumulativeDirectionModifier = 0;
        //llOwnerSay("cumulativeDirectionModifier=0");
        firstMoveCheck = TRUE;
                        
        CreatePMV(savedMoveBearing);        
    }
    
    listen(integer channel, string name, key id, string message)
    {
     
        
        if(message == "pmvbroken")
        {
            
            //rez a replacement
            CreatePMV(capBearingRange(savedMoveBearing + cumulativeDirectionModifier));
        }
        
        
        if(message == "lostmsg")
        {
         
            //PMV hasn't received length message
            
            //resend previous message
            ResendPMVLength();
        }
        
        if(message == "safe")
        {
            
            lastMoveSafe = TRUE;
         
            //move OK - keep the new PMV                    
            savedMoveBearing += cumulativeDirectionModifier;
            
            if(firstMoveCheck == TRUE)
            {
             
                //ATCF move worked - reset favoured side
                favouredSide = fsNEITHER;   
            }
            
            //fix the new PMV
            ConfirmNewPMV();
            
            
            
            state wander_SUBSTATE_act;  
        }
        
        if(message == "unsafe")
        {
                    
            lastMoveSafe = FALSE;
            
            //don't move
            state wander;            
        }     
    }
    
    link_message(integer sender_num, integer num, string list_argument, key id) {
        
        if(list_argument == "stopwander")
        {
         
            stopWandering = TRUE; 
        }        
    }      
}

state wander_SUBSTATE_act
{
    
    state_entry()
    {
        
        //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 wander;
    }
    
    link_message(integer sender_num, integer num, string list_argument, key id) {
        
        if(list_argument == "stopwander")
        {
         
            stopWandering = TRUE; 
        }        
    }     
}

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