integer carChannel = -5436462;
integer carListener;


integer LINK_GRIP = 9;
integer walker_channel = -4357986;
float time_since_walker = 10;


// Modulates amplitude using sine wave where the X axis is time
// Used for pendulums, things that go back and forth..
// References...
// https://en.wikipedia.org/wiki/Damped_sine_wave
// http://www.sengpielaudio.com/calculator-timedelayphase.htm
// http://mriquestions.com/angular-frequency-omega.html
// https://en.wikipedia.org/wiki/Exponential_decay
// ----------------------------------------------
//
// Constants - Don't change
float eulersNumber = 2.718;
// Constants - Programmable
float decay = 0.0;                  // Decay - for damping/amplifying
//float timer_freq = 0.25;            // How often the timer event is called
float frequency = 0.2;              // The frequency of the sine wave.
float startingPhaseAngle = 180;         // The initial phase offset. // 180 to start at 0 amplitude
float startingAmplitude = 0.4;      // The  amplitude at Phase = 0
// Variables - Non Programmable
float current_phase = 0.0;
float phaseAngle;               // The Phase angle at time = 0 - Either 'startingPhaseAngle' or another number if clicked.
float lastTime = 0.0;
float time = 0.0;               // How many simulated seconds have passed. Not real-time
float modulationTime = 0.0;
float offsetTime = 0.0;            // Offset from llGetTime() needed to get the real 'time'
float angularFreq;              // the Angular Frequency of the sine wave
float targetAmplitude = 0.4;
float amplitude;                // The current calculated amplitude at this point in time
float modulationModeChangeTime = 0.0; // Time after llResetTime that modulationMode will change to next modulationMode...

integer modulationMode = 0;
integer MODE_MODULATING = 0;
integer MODE_DAMPING = 1;
float damp_time;

// Used for z rotation smoothing to prevent 'jerkiness'
float zrot_smoothing_current_radians;
float zrot_smoothing_target_radians;
float zrot_smoothing_increment_radians = 0.2; // Supply degrees here. Converted to radians in state_entry.
float zrot_smoothing_error_radians = 4;

// --------------------------------------------------
// Above: Modulation Parameters.
// Below: Movement Parameters.
// --------------------------------------------------
vector posStart     = <135.65067, 118.03327, 3710.18604>;
vector posEnd       = <135.65466, 120.00000, 3710.18604>;
float  speedStart   = 0.0;
float  speedCruise  = 0.4;
float  speedEnd     = 0.0;
float  acceleration = 0.4;

float timeAccStart;
float timeAccEnd;
float distanceAccStart;
float distanceAccEnd;
float distance_travelled;
float distance_cruising;
float time_cruising;
float total_time;

float grip_angle_modifier;

float RAD_33_DEG;
float RAD_90_DEG;
float RAD_180_DEG;
float RAD_360_DEG;

integer movementMode = 0;
integer MODE_TRANSLATION = 0;
integer MODE_ROTATION = 1;
integer MODE_STOPPED = 2;
// --------------------------------------------------
// Modulation Functions

// re-targets to the given amplitude
target_amplitude(float target)
{
    if (target == targetAmplitude) return;

    decay = 0.3;

    if (target > targetAmplitude)
    {
        decay = -0.3;
    }

    modulationTime                 = 0.0;
    phaseAngle                     =  current_phase + startingPhaseAngle;
    startingPhaseAngle             = phaseAngle;

   //amplitude                     = startingAmplitude * (llPow(eulersNumber, 0-(decay * modulationTime))) * (llCos((angularFreq * modulationTime) + phaseAngle) + llSin((angularFreq * modulationTime) + phaseAngle));

    modulationModeChangeTime     = (llLog(target/startingAmplitude)/llLog(eulersNumber)) / -(decay);
    targetAmplitude             = target;
    modulationMode                 = MODE_DAMPING;
}

// ----------------------------------------------------
// Movement Functions
vector move_distance(float distance)
{
    vector norm = llVecNorm((posEnd - posStart));
    return posStart + (norm * distance);
}

// ----------------------------------------------------
// Rotation Functions

vector axis;
integer negative;

vector spin_round_axis(float speedMS,  float time, float swingAngle,integer negative, integer actually_do)
{

    float radius = llVecDist(posEnd, axis);
    float circumference = (2*PI)*radius;
    float distance_covered = speedMS * time;
    float revolutions = distance_covered / circumference;
    rotation modifier = llEuler2Rot(<0,0, zrot_smoothing_target_radians>);
    rotation main_rotation = llEuler2Rot(<(swingAngle*DEG_TO_RAD),0,0>) * modifier;
    main_rotation = main_rotation * (llEuler2Rot(<0,0,(negative) * (( RAD_360_DEG * revolutions) - RAD_180_DEG)> ) );

    vector         angle_step = <swingAngle, 0, (negative) * (360 * revolutions)>;
    rotation     rotation_step = llEuler2Rot(angle_step*DEG_TO_RAD);
    vector       current_offset = posEnd - axis;
    vector         new_offset = current_offset*rotation_step;
    vector         new_position = axis + new_offset;
    // rotation     new_rot = starting_rotation*rotation_step;
    rotation grip_rot = llEuler2Rot(<(swingAngle*-1) - 90, 0 , 90> * DEG_TO_RAD);
    vector new_pos = <new_position.x, new_position.y, posEnd.z>;
    if (actually_do)
    {
        llSetLinkPrimitiveParamsFast(LINK_THIS, [PRIM_ROTATION, main_rotation,PRIM_POSITION, new_pos]);
        llSetLinkPrimitiveParamsFast(LINK_GRIP, [PRIM_ROT_LOCAL, grip_rot]);
    }
    return new_pos;

}

float get_time_to_spin(vector axis, float speedMS, float angleDegrees)
{
    float radius = llVecDist(posEnd, axis);
    float circumference = (2*PI)*radius;
    float revolutions_to_cover = angleDegrees / 360;
    float distance_to_cover = revolutions_to_cover * circumference;
    if (angleDegrees < 0) negative = -1;
    else negative = 1;
    return distance_to_cover / speedMS;
}

// The Step Parameters is a strided list of sorts.
//
//          +-----------------------+-----------------------+------------------+
// Type     | Movement Instructions | Rotation Instructions | Stop Instruction |
//          +-----------------------+-----------------------+------------------+
// List     | "M"                   | "R"                   |"S"               |
// Items    | target_amplitude      | <axis>                |station_num       |
//          | <endPos>              | angleDegrees          |                  |
//          | cruise_speed          |                       |                  |
//          | end_speed             |                       |                  |
//          +-----------------------+-----------------------+------------------+
// total    |                      5|                      3|3                 |
//          +-----------------------+-----------------------+------------------+
// 0.2 SpeedCruise for open doors
list step_parameters = [
"M",0.05,<168.19083, 84.28532, 3723.62354>,0.6,0.8, // Town Platform Exit
"M",0.06,<166.94867, 85.78530, 3723.24854>,0.8, 0.9, // Ramp Midway
"M",0.07,<165.58499, 87.31174, 3722.27954>,1.0, 1.1,// Ramp exit
"M",0.1,<157.17195, 98.16995, 3715.39746>,1.3, 1.3, // Slope End
"M",0.12,<155.88412, 99.68120, 3714.31323>,1.3, 1.3, // Lower Ramp Midway
"M",0.12,<154.80302, 101.10777, 3714.05908>,1.3, 1.3, // Lower Ramp Exit
"M",0.07,<149.26172, 107.48204, 3714.10522>,1.3, 1.3, // School Approach Slowdown
"M",0.05,<145.51732, 111.78938, 3714.06177>,1.3, 0.4, // School Approach End
"R",<144.50839, 111.09032, 3714.04321>,180, // School Platform Pivot
"M",0.05,<144.76619, 108.84672, 3714.03052>, 0.4, 0.0, // School Platform Middle
"S", 2 , // Stop in middle of school platform
"M",0.05,<147.41536, 105.85649, 3714.06714>,0.2, 0.3, // School Platform End
"M",0.06,<152.87383, 99.49060, 3714.07178>,1.2, 1.2, // Lower Ramp Entry
"M",0.06,<154.05394, 97.92750, 3714.39868>,1.3, 1.2, // Lower Ramp Midway
"M",0.06,<155.23566, 96.47243, 3715.33350>,1.3, 1.4, // Lower Ramp Exit
"M",0.07,<163.87987, 85.29440, 3722.43115>,1.5, 1.4, // Slope End
"M",0.06,<165.12253, 83.91364, 3723.42822>,1.3, 1.3, // Upper Ramp Midway
"M",0.06,<166.92793, 82.09026, 3723.63330>,1.3, 1.3, // Upper Ramp End
"M",0.04,<175.75577, 73.66718, 3723.64478>,1.0, 0.4, // Town Approach End
"R",<176.72151, 74.37282, 3723.61060>,180, // Town Approach Pivot
"M", 0.05, <176.46262, 76.49358, 3723.63818>, 0.4, 0.4, // Town Platform Start
"M", 0.05, <173.86670, 78.95359, 3723.59595>, 0.4, 0.0, // Town Platform Middle
"S", 1, // Stop in middle of platform.
"M", 0.05, <169.99072, 82.60885, 3723.64185>, 0.4, 0.6 // Town Platform End
];



// list step_parameters = ["M", 0.6,  <136.83017, 120.30188, 3710.65869>, 0.2, 0.1,
//                         "R", <136.93367, 120.30188, 3710.65869>, -180,
//                         "M", 0.2, <137.03252, 119.26787, 3710.65869>, 0.04, 0.04,
//                         "R", <136.92574, 119.26363, 3710.65869>, -180];


// list step_parameters = [
//     "M", 0.05, <144.49481, 111.36368, 3713.60986>, 0.2, 0.3,
//     "R", <145.04198, 110.81207, 3713.61426>, -90,
//     "M", 0.15, <147.55910, 109.49693, 3713.61011>, 0.4, 0.4,
//     "M", 0.2, <148.23454, 108.94102, 3713.91260>, 0.8, 0.8,
//     "M", 0.18, <170.01117, 87.65887, 3724.28296>, 1.5, 1.3,
//     "M", 0.1, <175.27179, 82.58306, 3724.40015>, 0.8, 0.4,
//     "R", <174.53149, 81.93745, 3724.41821>, -180,
//     "M", 0.05, <169.26244, 85.79064, 3724.41797>, 0.2,0.3,
//     "M", 0.15, <168.29089, 86.72087, 3724.08374>, 0.8, 0.8,
//     "M", 0.18, <147.21301, 107.65935, 3713.92896>, 1.5, 1.3,
//     "M", 0.18, <146.57162, 108.16093, 3713.61646>, 0.8, 0.4,
//     "M", 0.15, <145.48149, 109.05575, 3713.61646>, 0.4, 0.4,
//     "R", <144.92361, 108.51511, 3713.61426>, 90,
//     "M", 0.1, <137.67944, 102.29155, 3713.61621>, 0.6, 0.6,
//     "R", <137.11125, 102.88134, 3713.61426>, -180
// ];

// list step_parameters = ["M",0.4,<135.83046, 120.00022, 3710.18286>,0.5,0.08,
//                         "R",<135.93230, 120.00022, 3710.18286>,-180,
//                         "M",0.4,<136.02982, 118.03920, 3710.18286>,0.5,0.08,
//                         "R",<135.92519, 118.03920, 3710.18286>,-180];
integer list_pos = 0;

next_step()
{
    if (movementMode == MODE_TRANSLATION)
    {
        list_pos += 5;
    }
    else if (movementMode == MODE_ROTATION)
    {
        list_pos += 3;
    }
    else if (movementMode == MODE_STOPPED)
    {
        list_pos += 2;
    }
    integer steps_length = llGetListLength(step_parameters);
    if (list_pos >= steps_length)
    {
        list_pos = 0;
    }
    string mode = llList2String(step_parameters, list_pos);
    if (mode == "M")
    {
        movementMode = MODE_TRANSLATION;
        posStart = llGetPos();
        target_amplitude(llList2Float(step_parameters, list_pos + 1));
        posEnd = llList2Vector(step_parameters, list_pos + 2);
        speedStart = speedEnd;
        speedCruise = llList2Float(step_parameters, list_pos + 3);
        if (speedCruise == 0.2)
        {
            llMessageLinked(LINK_SET, 0, "open", NULL_KEY);
        }
        else
        {
            llMessageLinked(LINK_SET, 0, "close", NULL_KEY);

        }
        speedEnd = llList2Float(step_parameters, list_pos + 4);

        // Refer to
        // https://www.dummies.com/education/science/physics/how-to-calculate-time-and-distance-from-acceleration-and-velocity/
        //         Formula
        // (End Speed - Start Speed)/1;
        // Start Speed End Speed
        // 20          30      (-10)/1 = -10s
        // 20          10      (10)/1 = 10s
        // The number may be negative but it's okay to just get the absolute value.
        timeAccStart = llFabs((speedCruise - speedStart)/acceleration);
        timeAccEnd   = llFabs((speedEnd - speedCruise)/acceleration);
        distanceAccStart = llFabs(((speedStart) * timeAccStart) + 0.5 * (acceleration * llPow(timeAccStart, 2)));
        distanceAccEnd = llFabs(((speedCruise) * timeAccEnd) - 0.5 * (acceleration * llPow(timeAccEnd, 2)));
        distance_travelled = llVecDist(posStart, posEnd);
        distance_cruising  = distance_travelled - distanceAccStart - distanceAccEnd;
        time_cruising = distance_cruising / speedCruise;
        total_time = time_cruising + timeAccStart + timeAccEnd;
        // float angle_bearing(float ax, float ay, float bx, float by)
        zrot_smoothing_target_radians = angle_bearing( posStart.x, posStart.y ,posEnd.x, posEnd.y) * DEG_TO_RAD;
        vector current_rot = llRot2Euler(llGetRot());
        zrot_smoothing_current_radians = current_rot.z;
        angle_grip();
        llSetTimerEvent(1.0 / llGetRegionFPS()); // Occasionally we update the timer to avoid queuing up events
        return;
    }
    else if (mode == "R")
    {
        llMessageLinked(LINK_SET, 0, "close", NULL_KEY);
        movementMode = MODE_ROTATION;
        axis = llList2Vector(step_parameters, list_pos + 1);
        total_time = llFabs(get_time_to_spin(axis, speedEnd, llList2Float(step_parameters, list_pos + 2)));
        llSetTimerEvent(1.0 / llGetRegionFPS()); // Occasionally we update the timer to avoid queuing up events

    } if (mode == "S")
    {
        // Stopped.
        llRegionSay(-89993, "Cable Car Used Memory; " + (string)llGetUsedMemory());
        movementMode = MODE_STOPPED;
        llMessageLinked(LINK_SET, 0, "open", NULL_KEY);
        target_amplitude(0.02); // Stop swingin
        carListener = llListen(carChannel, "", NULL_KEY, "depart");
        llRegionSay(carChannel, "arrived");
        llSetTimerEvent((1.0 / llGetRegionFPS()) * 2); // Run a looser timer when stopped - There is not much motion. Let the server catch it's breath.
    }

}

integer is_right_turn(float current, float target)
{

    while (target < current)
    {
        target += RAD_360_DEG;
    }
    // Target is always greater than current.
    // Turning left is the distance from 0 to current + target to 360
    float turning_left = current + (RAD_360_DEG - target);
    // Turning right is simply target - current as target is always bigger than current
    float turning_right = target - current;
    if (turning_left < turning_right) return TRUE;
    return FALSE;
}

vector calculate_pos_from_time(float g_time){
    vector pos;
    if (g_time <= timeAccStart)
    {
        // Before cruise speed.

        float distance = ((speedStart) * g_time) + 0.5 * (acceleration * llPow(g_time, 2));
        pos = move_distance(distance);
    }
    else
    {
        if (g_time > timeAccStart && g_time < (timeAccStart + time_cruising))
        {

            float distance = distanceAccStart + (speedCruise * (g_time - timeAccStart));
            pos = move_distance(distance);
        }
        else
        {

            float distance = distanceAccStart + distance_cruising + ((speedCruise) *(g_time - timeAccStart - time_cruising) + 0.5 * ((acceleration*-1) * llPow(g_time - timeAccStart - time_cruising, 2)));
            pos = move_distance(distance);
        }
    }
    return pos;
}


angle_grip()
{
    vector g_start = llGetPos();
    vector end_horizontal = <posEnd.x, posEnd.y, g_start.z>;
    float  x = llVecDist(g_start, end_horizontal);
    float  y = posEnd.z - g_start.z;
    // float angle = llAtan2(y, x) * RAD_TO_DEG;
    grip_angle_modifier = llAtan2(y, x);
    //llSetText("Grip Angle;\n" + (string) angle, <1,1,1>, 1.0);

}

float angle_bearing(float ax, float ay, float bx, float by)
{
    float theta = llAtan2(bx - ax, ay - by);
    if (theta < 0.0)
    {
        theta += (TWO_PI);

    }
    return RAD_TO_DEG * theta;
}

default
{
    state_entry()
    {
        RAD_33_DEG = 33 * DEG_TO_RAD;
        RAD_90_DEG = 90 * DEG_TO_RAD;
        RAD_180_DEG = 180 * DEG_TO_RAD;
        RAD_360_DEG = 360 * DEG_TO_RAD;
        zrot_smoothing_increment_radians = zrot_smoothing_increment_radians * DEG_TO_RAD;
        zrot_smoothing_error_radians = zrot_smoothing_error_radians * DEG_TO_RAD;
        list_pos = 200;
        next_step();
        // Modulation Initialisation
        angularFreq =  frequency * (2*PI);
        startingPhaseAngle = startingPhaseAngle * DEG_TO_RAD; // TESTING
        phaseAngle = startingPhaseAngle;
        llListen(-206, "", NULL_KEY, "");
        // Movement initialisation
        llSetStatus(STATUS_SANDBOX, TRUE);

        timeAccStart = (speedCruise - speedStart)/acceleration;
        timeAccEnd   = (speedEnd - speedCruise)/-(acceleration);

        distanceAccStart = ((speedStart) * timeAccStart) + 0.5 * (acceleration * llPow(timeAccStart, 2));
        distanceAccEnd = distanceAccStart; // TODO
        distance_travelled = llVecDist(posStart, posEnd);
        distance_cruising  = distance_travelled - distanceAccStart - distanceAccEnd;
        time_cruising = distance_cruising / speedCruise;
        total_time = time_cruising + timeAccStart + timeAccEnd;
        llResetTime();
        llSetTimerEvent(1.0 / llGetRegionFPS());
    }


    timer()
    {
        llSetTimerEvent(1/llGetRegionFPS());
        time = llGetTime();// + time_offset;
        modulationTime += time - lastTime;
        time_since_walker += modulationTime;
        lastTime = time;


        current_phase = (RAD_360_DEG * frequency * modulationTime);
        if (current_phase > RAD_360_DEG)
        {
            current_phase -= RAD_360_DEG; // Prevent overflow
        }
        current_phase = current_phase; // TESTING
        // --------------------------------------------------------------------------
        // modulation handling
        if (modulationMode == MODE_DAMPING && modulationTime > modulationModeChangeTime)
        {
            // llResetTime();
            phaseAngle =  current_phase + startingPhaseAngle;
            startingPhaseAngle = phaseAngle;

            decay = 0.0;
            // //time_offset = time - modulationModeChangeTime;
            // time = 0.0 ; // time_offset;
            startingAmplitude = targetAmplitude;
            modulationTime = 0.0;
            modulationMode = MODE_MODULATING;
        }

        if (movementMode != MODE_STOPPED && time > total_time)
        {
            //offsetTime = (time - total_time);
            llResetTime();
            next_step();
            time = llGetTime() ;
            lastTime = 0.0;
        }

        amplitude = startingAmplitude * (llPow(eulersNumber, 0-(decay * modulationTime))) * (llCos((angularFreq * modulationTime) + phaseAngle) + llSin((angularFreq * modulationTime) + phaseAngle));

        if (movementMode == MODE_ROTATION)
        {
            //spin_round_axis(float speedMS,  float time, float swingAngle)
            spin_round_axis(speedEnd, time, amplitude * 33, negative,TRUE);
            if (time_since_walker > 1.0) {
                llWhisper(walker_channel, (string)spin_round_axis(speedEnd, (time + 2.0), amplitude * 33, negative,FALSE));
            }
            return;
        }
        if (llFabs(zrot_smoothing_current_radians - zrot_smoothing_target_radians) > zrot_smoothing_error_radians && movementMode != MODE_STOPPED)
        {
            //integer angle_direction(float rad_angle_a, float rad_angle_b)
            if (is_right_turn(zrot_smoothing_current_radians, zrot_smoothing_target_radians))
            {
                zrot_smoothing_current_radians += zrot_smoothing_increment_radians;
            } else
            {
                zrot_smoothing_current_radians -= zrot_smoothing_increment_radians;
            }
        }
        vector rot_main = <(amplitude * RAD_33_DEG), 0,0>;
        vector rot_grip = <((amplitude * RAD_33_DEG)*-1) - RAD_90_DEG ,0 ,RAD_90_DEG>;
        rot_grip = < rot_grip.x + grip_angle_modifier, rot_grip.y, rot_grip.z>;
        // llSetText((string)(amplitude*33) + "deg\n" + (string)((amplitude*33)*-1), <1,1,1>, TRUE);
        rotation modifier = llEuler2Rot(<0,0, zrot_smoothing_current_radians>);
        rotation r_main = llEuler2Rot(rot_main) * modifier;
        rotation r_grip = llEuler2Rot(rot_grip);
        if (movementMode == MODE_TRANSLATION)
        {


            // -------------------------------------------------------------------------
            // position handling

            vector pos = calculate_pos_from_time(time);
            if (speedCruise != 0.2 && time_since_walker > 2.0) {
                // TODO - Restore this so people can board
                llWhisper(walker_channel, (string)calculate_pos_from_time(time + 2));
                time_since_walker = 0.0;
            }

            // finally, set the params.
            llSetLinkPrimitiveParamsFast(LINK_THIS,[ PRIM_ROTATION, r_main, PRIM_POSITION, pos]);
            llSetLinkPrimitiveParamsFast(LINK_GRIP, [PRIM_ROT_LOCAL, r_grip]);
            return;
        }
        // beyond: movementMode == MODE_STOPPED - The cable car has been stopped.
        llSetLinkPrimitiveParamsFast(LINK_THIS,[ PRIM_ROTATION, r_main]);
        llSetLinkPrimitiveParamsFast(LINK_GRIP, [PRIM_ROT_LOCAL, r_grip]);
        // Aight cool. So now the car needs to

    }

    listen(integer channel, string name, key id, string message)
    {
        if (llGetOwnerKey(id) != llGetOwner()) return; // noise on channel
        // It's the depart command.
        llWhisper(walker_channel, "COMMIT");
        next_step();
    }
}