list agents = [];

float closingStartAngle;
float currentAngle;
vector hinge;
vector ref_pos;
rotation ref_rot;


float distance_from_hinge(vector aPos)
{
    return llVecDist(<hinge.x, hinge.y, 0>, <aPos.x, aPos.y, 0>);
}

track_agent(key agent)
{
    if (llListFindList(agents, [agent]) == -1)
    {
        agents += [agent];
    }
}

set_door_rotation()
{
    rotation r = llAxisAngle2Rot(<0,0,1>, currentAngle * DEG_TO_RAD);
    vector   p = rotate_around_axis(hinge, ref_pos, r);
    llSetLinkPrimitiveParamsFast(LINK_THIS, [PRIM_ROTATION, r * ref_rot, PRIM_POSITION, p]);
}

//https://stackoverflow.com/questions/1211212/how-to-calculate-an-angle-from-three-points
float getAngle(float centreX, float centreY, float pointX, float pointY)
{
    float x = pointX - centreX;
    float y = pointY - centreY;

    float magnitude = llSqrt(x * x + y * y);
    float angle;
    if (magnitude > 0)
        angle = llAcos(x / magnitude);

    angle = angle * 180 / PI;
    if (y < 0)
        angle = 360 - angle;
    return angle;
}

vector get_hinge()
{
    vector pos = llGetPos();
    rotation rot = llGetRot();
    vector scale = llGetScale();
    // Offset
    vector offset = <scale.x/2, scale.y/2,0> * rot;

    vector hingePos = pos + offset;
    llWhisper(-72, (string)hingePos);
    return hingePos;
}


vector rotate_around_axis(vector axis, vector position, rotation r)
{
    // Calculate position's offset from axis.
    vector offset = position - axis;
    // Rotate the offset
    vector rotOffset = offset * r;

    return axis + rotOffset;
}

float sine_easeInOut2(float time, float begin, float cend, float duration)
{
    cend = cend - begin;
    return -cend/2 * (llCos(PI * time / duration) - 1) + begin;
}


default
{
    state_entry()
    {
        ref_pos = llGetPos();
        ref_rot = llGetRot();
        hinge = get_hinge();
        llSetStatus(STATUS_SANDBOX, TRUE);
        state closed;

    }
}

state closed
{
    state_entry()
    {
        agents = [];
        llVolumeDetect(TRUE);
        vector euler = llRot2Euler(ref_rot);
        llSetText("Closed... ref angle="  + (string)(euler.z * RAD_TO_DEG), <1,1,1>, 0.5);
        currentAngle = 0;
    }

    collision_start(integer num_detected)
    {
        integer i;
        for (i = 0; i < num_detected; i++)
        {
            if (llDetectedType(i) & AGENT)
            {
                track_agent(llDetectedKey(i));
            }
        }
        if (llGetListLength(agents) > 0)
        {
            state open;
        }
    }
}

state open
{
    state_entry()
    {
        llSetTimerEvent(1 / llGetRegionFPS());
    }

    timer()
    {
        integer i;
        integer numAgents = llGetListLength(agents);
        integer agentsInRange = 0;
        for (i = 0; i < numAgents; i++)
        {
            key agent = llList2Key(agents, i);
            vector agentPos = llList2Vector(llGetObjectDetails(agent, [OBJECT_POS]), 0);
            agentPos = rotate_around_axis(hinge, agentPos, ref_rot); // For the purpose of angle calculation, we're making this position rotated to the reference rotation.
            if (distance_from_hinge(agentPos) < 1.25)
            {

                agentsInRange++;
                vector euler = llRot2Euler(ref_rot);
                // float startAngle = euler.z * RAD_TO_DEG;
                float thisAngle = getAngle(hinge.x, hinge.y, agentPos.x, agentPos.y) + 15 ;
                // while (thisAngle > 360)
                // {
                //     thisAngle -= 360;
                // }
                // while (thisAngle < 0)
                // {
                //     thisAngle += 360;
                // }
                llSetText("Tracking... " + (string)thisAngle, <1,1,1>, 0.5);
                if (thisAngle > 0 && thisAngle < 90)
                {

                    if(numAgents == 1 || thisAngle > currentAngle) currentAngle = thisAngle;
                }
            }
        }
        if (agentsInRange > 0)
        {
            //llSetText("Open " + (string) currentAngle, <1,1,1>, 0.5);
            set_door_rotation();
        } else {
            state closing;
        }
    }
}

state closing
{
    state_entry()
    {
        agents = [];
        closingStartAngle = currentAngle;
        llSetTimerEvent(1 / llGetRegionFPS());
        llResetTime();
        llSetText("Closing", <1,1,1>, 0.5);
    }

    collision_start(integer num_detected)
    {
        integer i;
        for (i = 0; i < num_detected; i++)
        {
            if (llDetectedType(i) & AGENT)
            {
                track_agent(llDetectedKey(i));
            }
        }
        if (llGetListLength(agents) > 0)
        {
            state open;
        }
    }

    timer()
    {
        float elapsed_time = llGetTime();

// float sine_easeInOut(float time, float begin, float cend, float duration)
// {
//     return -cend/2 * (llCos(PI * time / duration) - 1) + begin;
// }

        if (elapsed_time > 3.0)
        {
            llSetTimerEvent(FALSE);
            currentAngle = 0;
            set_door_rotation();
            state closed;

        }
        if (elapsed_time < 3.0)
        {
            currentAngle = sine_easeInOut2(elapsed_time, closingStartAngle, 0, 3.0);
            llSetText("Closing " + (string) currentAngle, <1,1,1>, 0.5);
            set_door_rotation();
        }
    }
}