'''
Problem can be adimensionalized like this
m d^2x/dt^2 = m g - k (|x| - lo) x/|x| - d dx/dt
  d^2x/dt^2 =   g - k/m (|x| - lo) x/|x| - d/m dx/dt

x' = x/lo
d^2x'/dt^2 = g - k/m (|x'| - 1) x'/|x'| - dlo/m dx'/dt

t' = t*sqrt(k/m)
k/m d^2x'/dt'^2 = g - k/m (|x'| - 1) x'/|x'| - d lo sqrt(k/m)/m dx'/dt
d^2x'/dt'^2 = mg/k - (|x'| - 1) x'/|x'| - d lo sqrt(k m) dx'/dt

so we just get two constants (plus the initial conditions):
  G = m*g/k  y  D = d lo sqrt(k m)
  d^2x'/dt'^2 = G - (|x'| - 1) x'/|x'| - D dx'/dt
'''
from __future__ import division
from visual import vector, dot
from math import *

L0 = 1
k = 25
g = vector(0,-9.8,0)
mass = 0.2
vec_scale = 0.1  # How long is the vector for a 1N force
gamma = 100
tmax = 2000
dt = 0.0001


def get_components(vec, axis):
    axis_90 = (-axis[1], axis[0], 0)
    return dot(vec, axis), dot(vec, axis_90)

# Create objects
class Point_Mass(object):
    def __init__(self, pos, velocity):
        self.pos = vector(pos)
        self.velocity = vector(velocity)

def simulate(G, D, initial_length, theta0, end):
    pivot = Point_Mass(vector(0,L0,0), (0,0,0))
    bob = Point_Mass(pos=(initial_length * sin(theta0),
                          L0 - initial_length * cos(theta0),
                          0),
                     velocity=(0,0,0))

    # First iteration step
    # Get forces
    fgrav = G
    axis = bob.pos - pivot.pos
    axis_n = axis.norm()
    deltal = axis.mag - 1
    if deltal > 0:
        fstring = -deltal*axis_n
    else:
        fstring = vector(0,0,0)
    v_rad, v_trans = get_components(bob.velocity, axis_n)
    fdamp = -D * v_rad * axis_n
    olddeltal = deltal

    t = 0
    done = False
    final_pos = final_vel = None
    while t < tmax and not done:
        # Update objects
        # m*v = m*v0 + F_net * dt
        oldvelocity = bob.velocity
        bob.velocity = bob.velocity + (fgrav+fstring+fdamp)*dt
        # x = x0 + (m*v - m*v0)*dt/m/2
        bob.pos = bob.pos + 0.5*(bob.velocity + oldvelocity)*dt

        # Update counters and indicators
        axis = bob.pos - pivot.pos
        axis_n = axis.norm()
        deltal = axis.mag - 1
        old_vtrans = v_trans
        v_rad, v_trans = get_components(bob.velocity, axis_n)
        if v_trans < 0 and old_vtrans > 0:
            print deltal
            if abs(deltal-olddeltal) < end:
                done = True
                final_pos = bob.pos
                final_vel = bob.velocity
            olddeltal = deltal
        # Update force(s)
        if deltal > 0:
            fstring = -deltal*axis_n
        else:
            fstring = vector(0,0,0)
        fdamp = -D * v_rad * axis_n
        t += dt
    return deltal, final_pos, final_vel


G = mass*g/k
D = gamma*L0*sqrt(k*mass)
initial_length = (1+0.08476)*L0
angle = atan(0.60461/(L0-0.099364))
deltal, final_pos, final_vel =  simulate(G, D, initial_length, angle, 1e-5)
print (deltal, final_pos, final_vel)
print ("initial length: ", L0+deltal, "initial angle: ", angle)