//////////////////////////////////////////////////////////
//   (C) Wizardry and Steamworks 2011, license: GPLv3   //
// License at: http://www.gnu.org/licenses/gpl.html     //
//////////////////////////////////////////////////////////
 
// A quicksort, providing a stable map between two sets of elements.
///////////////////////////////////////////////////////////////////////////
//    Copyright (C) 2013 Wizardry and Steamworks - License: GNU GPLv3    //
///////////////////////////////////////////////////////////////////////////
list wasDualQuicksort(list a, list b) {
    if(llGetListLength(a) <= 1) return a+b;
 
    float pivot_a = llList2Float(a, 0);
    a = llDeleteSubList(a, 0, 0);
    string pivot_b = llList2String(b, 0);
    b = llDeleteSubList(b, 0, 0);
 
    list less = [];
    list less_b = [];
    list more = [];
    list more_b = [];
 
    do {
        if(llList2Float(a, 0) > pivot_a) {
            less += llList2List(a, 0, 0);
            less_b += llList2List(b, 0, 0);
            jump continue;
        }
        more += llList2List(a, 0, 0);
        more_b += llList2List(b, 0, 0);
@continue;
        a = llDeleteSubList(a, 0, 0);
        b = llDeleteSubList(b, 0, 0);
    } while(llGetListLength(a));
    return wasDualQuicksort(less, less_b) + [ pivot_a ] + [ pivot_b ] + wasDualQuicksort(more, more_b);
}
 
// wNFA States corresponding to directions.
list qD = [ "forward", "backward", "left", "right", "up", "down" ];
// wNFA Weights for each direction. Must add up to 1 (invariant).
// assert: length(qD) == length(QT)
// assert: sum(QT) == 1 
list QT = [ .15, .17, .17, .17, .17, .17 ];
 
// (the clever bit) wNFA and weighted probabilities algorithm that 
// calculate the next state of the wNFA based on every state's weight
// The amount by which every weight's probability will be decreased
// in case of a collision.
float CORRECTION_AFFINITY = .2;
// (the clever bit) wNFA
string getNextState(string current_state) {
 
    integer i;
    integer idx = llListFindList(qD, (list)current_state);
    if(idx == -1) jump initial;
    float diff = 0;
    float pIdx = llList2Float(QT, idx);
    if(pIdx <= CORRECTION_AFFINITY) {
        diff = CORRECTION_AFFINITY-pIdx;
        pIdx = 0;
        jump correct;
    }
    pIdx = pIdx - CORRECTION_AFFINITY;
@correct;
    QT = llListReplaceList(QT, (list)pIdx, idx, idx);
    i = llGetListLength(QT)-1;
    do {
        if(i == idx) jump continue;
        // Subtract the affinity from the current direction's probability and redistribute the remainder to all the other directions.
        QT = llListReplaceList(QT, (list)(llList2Float(QT, i)+(CORRECTION_AFFINITY-diff)/(float)(llGetListLength(qD)-1)), i, i);
@continue;
    } while(--i>-1);
 
    //DEBUG - Uncomment for probability debug.
    //llOwnerSay("Directions: " + llDumpList2String(qD, " ") + "\nProbabilities: " + llDumpList2String(QT, " "));
    //llOwnerSay("Probabilities add up to: " + (string)llListStatistics(LIST_STAT_SUM, QT));
 
@initial;
    // Now, compute next hop based on the highest probability in the list.
    list P = wasDualQuicksort(QT, qD);
    list dirs = llList2ListStrided(llDeleteSubList(P, 0, 0), 0, llGetListLength(P)-1, 2);
    list dirs_prob = llList2ListStrided(P, 0, llGetListLength(P)-1, 2);
 
    //DEBUG - Uncomment for probability debug.
    //llOwnerSay("\Probabilities add up to: " + (string)llListStatistics(LIST_STAT_SUM, QT) + "\nSorted probabilities: " + llDumpList2String(P, ","));
 
    float rnd = llFrand(1);
    float cumulative = 0;
    i=llGetListLength(dirs_prob)-1;
    do {
        cumulative += llList2Float(dirs_prob, i);
        if(cumulative < rnd) jump next;
        if(llList2String(dirs, i) == "forward") return "forward";
        if(llList2String(dirs, i) == "backward") return "backward";
        if(llList2String(dirs, i) == "left") return "left";
        if(llList2String(dirs, i) == "right") return "right";
        if(llList2String(dirs, i) == "up") return "up";
        if(llList2String(dirs, i) == "down") return "down";
@next;
    } while(--i>-1);
 
    // This should not happen. The invariant has been violated somehow.
    // DEBUG - Notify the user of invariant violation.
    llSay(DEBUG_CHANNEL, "Invariant violation.");
    // The universe as we know it has ceased to exist.
    return "ERROR";
 
}
 
// Thrust corrections based on current movmement.
// ENGINE_SPEED - the velocity in m/s that the k-pROV will move at.
float ENGINE_SPEED = .2;
// Thurst and anti-thrust: anti-thrust = -thrust.
// Thrust. Set velocity corresponding to x,y,z-axis.
thrust(string direction) {
    // Fix for OpenSim, since llSetStatus for preventing rotations 
    // does not work as defined by the LSL Api.
    llSetStatus(STATUS_PHYSICS, FALSE);
    llSetRot(llEuler2Rot(<90,0,0>*DEG_TO_RAD));
    // Turn engines on.
    llSetStatus(STATUS_PHYSICS, TRUE);
    llSetBuoyancy(1.0);
 
    // Thrust.
    if(direction == "forward") { llApplyImpulse(<0,0,ENGINE_SPEED>, TRUE); return; }
    if(direction == "backward") { llApplyImpulse(<0,0,-ENGINE_SPEED>, TRUE); return; }
    if(direction == "left") { llApplyImpulse(<ENGINE_SPEED,0,0>, TRUE); return; }
    if(direction == "right") { llApplyImpulse(<-ENGINE_SPEED,0,0>, TRUE); return; }
    if(direction == "up") { llApplyImpulse(<0,ENGINE_SPEED,0>, TRUE); return; }
    if(direction == "down") { llApplyImpulse(<0,-ENGINE_SPEED,0>, TRUE); return; }
 
}
 
// Save the current state.
string currentState = "";
// Anti-stuck.
vector cPos = ZERO_VECTOR;
 
//////////////////////////////////////////////////////////
//   (C) Wizardry and Steamworks 2011, license: GPLv3   //
// License at: http://www.gnu.org/licenses/gpl.html     //
//////////////////////////////////////////////////////////
list wasListReverse(list lst) {
    if(llGetListLength(lst)<=1) return lst;
    return wasListReverse(llList2List(lst, 1, llGetListLength(lst))) + llList2List(lst,0,0);
}
 
showStats(string action) {
    string vel = llGetSubString((string)llVecMag(llGetVel()), 0, 3);
 
    list P = wasDualQuicksort(QT, qD);
    list dirs = llList2ListStrided(llDeleteSubList(P, 0, 0), 0, llGetListLength(P)-1, 2);
    list dirs_prob = llList2ListStrided(P, 0, llGetListLength(P)-1, 2);
 
    dirs_prob = wasListReverse(dirs_prob);
    integer i = llGetListLength(dirs_prob)-1;
    string probabilities = "";
    do {
        probabilities += llGetSubString(llList2String(dirs_prob, i), 0, 3) + " ";
    } while(--i>-1);
    llSetText("Velocity: " + (string)vel + "m/s" + "\n" +
                "qD: [ " + llDumpList2String(dirs, " ") + " ]" + "\n" +
                "QT: [ " + probabilities + "]" + "\n" + "Action: " + action, <1,1,0>, 1);
}
 
default
{
    state_entry() {
        llSetStatus(STATUS_ROTATE_X|STATUS_ROTATE_Y|STATUS_ROTATE_Z, FALSE);
        // Align initial rotation.
        llSetRot(llEuler2Rot(<90,0,0>*DEG_TO_RAD));
        // Jump directly to the compute state.
        state compute;
    }
 
}
 
state forward
{
    state_entry() {
        // DEBUG movement.
        //llOwnerSay("moving foward");
        currentState = "forward";
        thrust(currentState);
        cPos = llGetPos();
        llSetTimerEvent(1);
    }
    timer() {
        showStats(currentState);
        if(llVecDist(llGetPos(), cPos) == 0) {
            llSetStatus(STATUS_PHYSICS, FALSE);
            state compute;
        }
        cPos = llGetPos();
    }
    collision_start(integer num) {
        llSetStatus(STATUS_PHYSICS, FALSE);
        state compute;
    }
}
 
state backward
{
    state_entry() {
        // DEBUG movement.
        //llOwnerSay("moving backward");
        currentState = "backward";
        thrust(currentState);
        cPos = llGetPos();
        llSetTimerEvent(1);
    }
    timer() {
        showStats(currentState);
        if(llVecDist(llGetPos(), cPos) == 0) {
            llSetStatus(STATUS_PHYSICS, FALSE);
            state compute;
        }
        cPos = llGetPos();
    }
    collision_start(integer num) {
        llSetStatus(STATUS_PHYSICS, FALSE);
        state compute;
    }
}
 
state left
{
    state_entry() {
        // DEBUG movement.
        //llOwnerSay("moving left");
        currentState = "left";
        thrust(currentState);
        cPos = llGetPos();
        llSetTimerEvent(1);
    }
    timer() {
        showStats(currentState);
        if(llVecDist(llGetPos(), cPos) == 0) {
            llSetStatus(STATUS_PHYSICS, FALSE);
            state compute;
        }
        cPos = llGetPos();
    }
    collision_start(integer num) {
        llSetStatus(STATUS_PHYSICS, FALSE);
        state compute;
    }
}
 
state right
{
    state_entry() {
        // DEBUG movement.
        //llOwnerSay("moving right");
        currentState = "right";
        thrust(currentState);
        cPos=llGetPos();
        llSetTimerEvent(1);
    }
    timer() {
        showStats(currentState);
        if(llVecDist(llGetPos(), cPos) == 0) {
            llSetStatus(STATUS_PHYSICS, FALSE);
            state compute;
        }
        cPos = llGetPos();
    }
    collision_start(integer num) {
        llSetStatus(STATUS_PHYSICS, FALSE);
        state compute;
    }
}
 
state up
{
    state_entry() {
        // DEBUG movement.
        //llOwnerSay("moving up");
        currentState = "up";
        thrust(currentState);
        cPos=llGetPos();
        llSetTimerEvent(1);
    }
    timer() {
        showStats(currentState);
        if(llVecDist(llGetPos(), cPos) == 0) {
            llSetStatus(STATUS_PHYSICS, FALSE);
            state compute;
        }
        cPos = llGetPos();
    }
    collision_start(integer num) {
        llSetStatus(STATUS_PHYSICS, FALSE);
        state compute;
    }
}
 
state down
{
    state_entry() {
        // DEBUG movement.
        //llOwnerSay("moving down");
        currentState = "down";
        thrust(currentState);
        cPos = llGetPos();
        llSetTimerEvent(1);
    }
    timer() {
        showStats(currentState);
        if(llVecDist(llGetPos(), cPos) == 0) {
            llSetStatus(STATUS_PHYSICS, FALSE);
            state compute;
        }
        cPos = llGetPos();
    }
    collision_start(integer num) {
        llSetStatus(STATUS_PHYSICS, FALSE);
        state compute;
    }
}
 
state compute
{
    state_entry() {
        // Fix for OpenSim, since llSetStatus for preventing rotations 
        // does not work as defined by the LSL Api.
        llSetRot(llEuler2Rot(<90,0,0>*DEG_TO_RAD));
        // TRACKER comment the next line if not needed.
        llMessageLinked(LINK_THIS, 0, (string)llGetPos(), "");
        showStats("Computing...");
        string nextState = getNextState(currentState);
@retry;
        if(nextState == currentState) {
            nextState = getNextState(currentState);
            jump retry;
        }
        //DEBUG previous and next states.
        //llOwnerSay("prev: " + currentState);
        //llOwnerSay("next: " + nextState);
        if(nextState == "forward") state forward;
        if(nextState == "backward") state backward;
        if(nextState == "left") state left;
        if(nextState == "right") state right;
        if(nextState == "up") state up;
        if(nextState == "down") state down;
    }
}