--# Main
-- RailJoint

-- Use this function to perform your initial setup
function setup()
    bodies = {}
    joint = nil
    parameter.action("Scene 1",scene1)
    parameter.action("Scene 2",scene2)
    parameter.action("Scene 3",scene3)
    fps = 60
end



function clear()
    for k,v in pairs(bodies) do
        bodies[k]=nil
    end
    collectgarbage("collect")
    bodies = {}
    joint = nil
end

function scene1()
    clear()
    local points = {}
    for i=1,61 do
        points[i] = vec2(WIDTH/2,HEIGHT/2)+vec2(math.sin(math.rad(i*6))*300,math.cos(math.rad(i*6))*300)
    end
    bodies[1] = Cart(points[1])
    joint = RailJoint(points,bodies[1].body)
    joint:setCV(-300,0.9)
    joint:setFriction(10)
end

function scene2()
    clear()
    local points = {}
    for i=1,241 do
        points[i] = vec2(WIDTH/2,HEIGHT/2)+vec2(math.sin(math.rad(i*3))*50+math.cos(math.pi/2+math.rad(i*12))*30,
        math.cos(math.rad(i*3))*300)
    end
    bodies[1] = Cart(points[1])
    joint = RailJoint(points,bodies[1].body)
end

function scene3()
    clear()
    local points = {}
    for i=1,361 do
        points[i] = vec2(WIDTH/2,HEIGHT/2)+vec2(math.sin(math.rad(i*2))*300,math.cos(math.rad(i*3))*300)
    end
    bodies[1] = Cart(points[1])
    bodies[2] = Cart(points[50])
    bodies[3] = Cart(points[100])
    bodies[4] = Cart(points[150])
    bodies[5] = Cart(points[200])
    joint = RailJoint(points,unpackbodies(bodies))
    joint:setCV(-100,0.9)
end

function unpackbodies(tbl)
    local btbl = {}
    for k,v in pairs(tbl) do
        btbl[k] = v.body
    end
    return unpack(btbl)
end
function draw()
    background(40, 40, 50)
    strokeWidth(5)
    fps=fps*0.95+(1/DeltaTime)*0.05
    text(fps,100,100)
    for k,v in pairs(bodies) do
        v:draw()
    end
    if joint then joint:draw() end
end


--# Cart
Cart = class()

function Cart:init(pos)
    self.body = physics.body(POLYGON,vec2(-30,-20),vec2(-30,20),vec2(30,20),vec2(30,-20))
    self.body.position = pos
end

function Cart:draw()
    pushMatrix()
        translate(self.body.x,self.body.y)
        rotate(self.body.angle)
        sprite("Cargo Bot:Register Slot",0,0,60,40)
    popMatrix()
end

--# RailJoint
RailJoint = class()

function RailJoint:init(points,...)
    self.points = points
    local args = {...}
    self.bodies = {}
    for k,v in pairs(args) do
        self.bodies[k] = {}
        self.bodies[k].body = v
        v.linearDamping=0.05
    end
    self.m = mesh()
    self.r = {}
    for i=2,#self.points do
        local p1,p2 = self.points[i-1],self.points[i]
        local ang,pos,len = math.rad(angleOfPoint(p1-p2)),(p1+p2)/2,p1:dist(p2)
        self.r[i] = self.m:addRect(pos.x,pos.y,len,2,ang)
    end
    self.conveyor = nil
end

function RailJoint:setCV(speed,stiffness)
    self.conveyor = {speed,stiffness}
end

function RailJoint:setFriction(f)
    for k,v in pairs(self.bodies) do
        v.body.linearDamping = f
    end
end

function RailJoint:cp(body)
    local closest = {math.huge,0,nil,nil,nil}
    local cart = body.body
    local tpos = cart.position
    for i=2,#self.points do
        local p1,p2 = self.points[i-1],self.points[i]
        local cp = cpol(tpos,p1,p2)
        if closest[1]>cp:dist(tpos) then
            closest = {cp:dist(tpos),i,cp,p1,p2}
        end
    end
    local pos = closest[3]
    local p1,p2 = closest[4],closest[5]
    local dl = (pos-tpos):normalize()
    local clp = cart.linearVelocity
    cart:applyForce(cart.mass*((pos-tpos)*32-(dl:dot(vec2(clp.x,clp.y))*dl))*1)
    --while cart.angle>360 do cart.angle=cart.angle-360 end
    --while cart.angle<0 do cart.angle=cart.angle+360 end
    local cang = angleOfPoint(p2-p1)-cart.angle
    while cang>180 do cang = cang-360 end
    while cang<-180 do cang = cang+360 end
    cart.angularVelocity = cart.angularVelocity+(cart.mass*(cang*32-cart.angularVelocity))/2
end

function RailJoint:draw()
    self.m:draw()
    for k,v in pairs(self.bodies) do
        self:cp(v)
        if self.conveyor then
            local sd,sf = self.conveyor[1],self.conveyor[2]
            v.body:applyForce(vec2(sd,0):rotate(math.rad(v.body.angle)))
            v.body.linearVelocity = v.body.linearVelocity*sf
        end
    end
end

function RailJoint:touched(touch)
    -- Codea does not automatically call this method
end

function cpol(p,a,b)
	local c = p - a;
	local v = (b - a):normalize()
	local d = (b - a):len();
	local t = v:dot(c)
	if t < 0 then return a end
	if t > d then return b end
	v = v * t
	return a + v;
end
function angleOfPoint(pt) return math.deg(math.atan2(pt.y,pt.x)) end