list agents = [];

float closingStartAngle;
float currentAngle;
float closedAngle;
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, integer side)
{
    if (llListFindList(agents, [agent]) == -1)
    {
        agents += [agent, side];
    }
}

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, 0,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;
}

float normalize_angle(float angle)
{
    while (angle > 360)
    {
        angle -= 360;
    }
    while (angle < 0)
    {
        angle += 360;
    }
    return angle;
}

init_parameters()
{
    ref_pos = llGetPos();
    ref_rot = llGetRot();
    hinge = get_hinge();
    closedAngle = normalize_angle(getAngle(hinge.x, hinge.y, ref_pos.x, ref_pos.y));
}


default
{
    state_entry()
    {
        init_parameters();
        llSetStatus(STATUS_SANDBOX, TRUE);
        state closed;
    }
}

state closed
{
    state_entry()
    {
        agents = [];
        llVolumeDetect(TRUE);
        vector euler = llRot2Euler(ref_rot);
        llRotTarget(ref_rot, 0);
        currentAngle = 0;
    }

    collision_start(integer num_detected)
    {
        integer i;
        for (i = 0; i < num_detected; i++)
        {
            if (llDetectedType(i) & ACTIVE)
            {
                vector agentOffset = (llDetectedPos(i) - llGetPos()) / llGetRot();
                track_agent(llDetectedKey(i), (agentOffset.y > 0));
            }
        }
        if (llGetListLength(agents) > 0)
        {
            state open;
        }
    }

    moving_end()
    {
        init_parameters();
    }

    not_at_rot_target()
    {
        // owner rotated the object whilst it's closed.
        init_parameters();
    }
}

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

    timer()
    {
        integer i;
        integer numAgents = llGetListLength(agents) / 2;
        integer agentsInRange = 0;
        for (i = 0; i < numAgents; i = i + 2)
        {
            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++;
                // Figure out what side the agent is on.
                integer side = llList2Integer(agents, i + 1);
                integer extraDegrees = 20;

                if (side == 0) extraDegrees = -20;


                float thisAngle = getAngle(hinge.x, hinge.y, agentPos.x, agentPos.y) + extraDegrees + closedAngle + 180;
                thisAngle = normalize_angle(thisAngle);
                if ((thisAngle > 0 && thisAngle < 145) ||(thisAngle > 215 && thisAngle < 360))
                {

                    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;
        }
    }

    collision_start(integer num_detected)
    {
        integer i;
        for (i = 0; i < num_detected; i++)
        {
            if (llDetectedType(i) & ACTIVE)
            {
                vector agentOffset = (llDetectedPos(i) - llGetPos()) / llGetRot();
                track_agent(llDetectedKey(i), (agentOffset.y > 0));
            }
        }
    }
}

state closing
{
    state_entry()
    {
        agents = [];
        closingStartAngle = currentAngle;
        llSetTimerEvent(1 / llGetRegionFPS());
        llResetTime();
    }

    collision_start(integer num_detected)
    {
        integer i;
        for (i = 0; i < num_detected; i++)
        {
            if (llDetectedType(i) & ACTIVE)
            {
                vector agentOffset = (llDetectedPos(i) - llGetPos()) / llGetRot();
                track_agent(llDetectedKey(i), (agentOffset.y > 0));


            }
        }
        if (llGetListLength(agents) > 0)
        {
            state open;
        }
    }

    timer()
    {
        float elapsed_time = llGetTime();

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

        }
        if (elapsed_time < 3.0)
        {
            float endAngle = 0;
            if (closingStartAngle > 180)
            {
                endAngle = 360;
            }
            currentAngle = sine_easeInOut2(elapsed_time, closingStartAngle, endAngle, 3.0);
            set_door_rotation();
        }
    }
}