Better Movelock...

I'm a noob, but I'm still trying to design a better movelock system. I've prolly just cocked it up so bad that everyone will laugh at me, but I thought it was worth asking for some help.

The old movelock seemed to try to move you to the same spot all the time, presumably to make you harder to push (?); with this I'm trying to have it set your position whenever you stop moving yourself, and then detect if that position has changed on a timer, and if it has, to use continual MoveTo's (as the wiki sez it'll only go 64.5 m as an attachment) until you get back there.

However, while this works just kinda like old movelock on light push, on heavy push it sends me wherever I'm going, locks me there, and then won't let me move until I disable it. Suggestions?

vector pos;
integer targetid;

default
{
    state_entry()
    {
        state off;
    }
}

state off
{
    state_entry()
    {
        llSetColor(<1,0,0>, ALL_SIDES);
        llSetStatus(STATUS_PHYSICS, FALSE);
        llSetTimerEvent(0);
        llStopMoveToTarget();
        llMessageLinked(4,0,"Movelock deactivated.",NULL_KEY);
    }
    
    touch_start(integer num)
    {
        state on;
    }
}

state on
{
    state_entry()
    {
        llSetColor(<0,1,0>, ALL_SIDES);
        llSetStatus(STATUS_PHYSICS,TRUE);
        llMessageLinked(4,0,"Movelock on.",NULL_KEY);
        llRequestPermissions(llGetOwner(), PERMISSION_TAKE_CONTROLS);
        pos = llGetPos() + <0,0,-0.1>;
        llOwnerSay((string)llGetStatus(STATUS_PHYSICS));
        llSetTimerEvent(0.5);
    }
    
    touch_start(integer num)
    {
        state off;
    }
    
    run_time_permissions(integer perm) 
    { 
        if (perm & PERMISSION_TAKE_CONTROLS) 
        {
            llTakeControls(CONTROL_UP | CONTROL_DOWN | CONTROL_FWD | CONTROL_BACK | CONTROL_LEFT | CONTROL_RIGHT, TRUE, TRUE); 
        }
    }
    
    control(key id, integer held, integer change) 
    { 
        if ((change & ~held & CONTROL_UP) | (change & ~held & CONTROL_DOWN) | (change & ~held & CONTROL_FWD) | (change & ~held & CONTROL_BACK) | (change & ~held & CONTROL_LEFT) |  (change & ~held & CONTROL_RIGHT)) 
        {
            llTargetRemove(targetid);
            llSleep(0.5);
            pos = llGetPos() + <0,0,-0.1>;
            llSetTimerEvent(0.5);
        } 
        if ((change & held & CONTROL_UP) | (change & held & CONTROL_DOWN) | (change & held & CONTROL_FWD) | (change & held & CONTROL_BACK) | (change & held & CONTROL_LEFT) |  (change & held & CONTROL_RIGHT)) 
        {
            llSetTimerEvent(0);
            llTargetRemove(targetid);
            llOwnerSay("Pressed.");
            llStopMoveToTarget();
        } 
    }
    
    on_rez(integer num)
    {
        llSetTimerEvent(0);
        llStopMoveToTarget();
    }
    
    not_at_target()
    {
        llMoveToTarget(pos,0.5);
    }
    
    at_target( integer number, vector targetpos, vector ourpos )
    {
        llSetTimerEvent(0.5);
        llTargetRemove(targetid);
        llStopMoveToTarget();
    }
    
    timer()
    {
        if(llVecDist(pos, llGetPos()) > 10)
        {
            targetid = llTarget(pos, 5);
        }
    }
}

By the way, right now this is just in a plywood cube that changes colors, and there are some debug messages still included. Yeah, I'll clean it up and do something more interesting if I can just get it to work.