--# Main
-- PhysicsButton

-- Use this function to perform your initial setup
function setup()
    pb = PhysicsButton(vec2(WIDTH/2,HEIGHT/2+200),
    readImage("Cargo Bot:Title Large Crate 1"),function() print("hit") end)
end


function angleOfPoint(pt)
    return math.deg(math.atan2(pt.y,pt.x))
end

function touched(t)
    pb:touched(t)
end
-- This function gets called once every frame
function draw()
    -- This sets a dark background color
    background(40, 40, 50)

    -- This sets the line thickness
    strokeWidth(5)

    -- Do your drawing here
    pb:draw()
    fill(255)
    text(math.ceil(1/DeltaTime),100,100)
end


--# PhysicsButton
PhysicsButton = class()

function PhysicsButton:init(pos,img,callback)
    -- you can accept and set parameters here
    local w,h = img.width,img.height
    self.w,self.h = w,h
    local verts = {vec2(-w/2,-h/2),vec2(-w/2,h/2),vec2(w/2,h/2),vec2(w/2,-h/2)}
    self.anc = physics.body(CIRCLE,10)
    self.anc.sensor = true
    self.anc.position = pos
    self.anc.type = STATIC
    self.btn = physics.body(POLYGON,unpack(verts))
    self.btn.position = pos+vec2(0,-300)
    self.btn.linearVelocity=vec2(math.random(-20,20)*20)
    self.btn.gravityScale = 1
    self.rj = physics.joint(ROPE,self.anc,self.btn,self.anc.position,self.btn.position+vec2(0,h/2.05),
    self.anc.position:dist(self.btn.position+vec2(0,h/2.05)))
    self.length = self.anc.position:dist(self.btn.position+vec2(0,h/2.05))
    self.rope = Roped(self.rj,8)
    self.rj.maxLength = math.huge
    self.cb = callback
    self.img = img
    self.m = mesh()
    self.r = self.m:addRect(pos.x,pos.y,w,h)
    self.m.texture = img
    self.hpd = nil
    self.hang = nil
    self.finished = nil
end

function PhysicsButton:draw()
    if self.finished then
        if self.btn then
            self.btn:destroy()
            self.anc:destroy()
            self.m:clear()
            self.m = nil
            self.anc = nil
            self.btn = nil
        end
        return
    end
    --Equation----------
    local b,bl,bp,ap = self.btn,self.btn.linearVelocity,self.btn.position,self.anc.position
    local blp = self.btn:getLinearVelocityFromWorldPoint(bp)
    local ba = bp+vec2(0,self.h/2.05):rotate(math.rad(b.angle))
    local stiffness = 1
    local strength = 1
    if ba:dist(ap)>self.length then
        local dp = ap+(ba-ap):normalize()*(self.length)
        local dl = (dp-ba):normalize()
        self.btn:applyForce(b.mass*(((dp-ba)*36)*strength-(dl:dot(vec2(blp.x,blp.y))*dl*0.5))*stiffness-bl/36,ba)
    end
    --------------------
    if self.held and self.hang then
        --self.btn:applyForce((self.held-self.btn.position)*10-self.btn.linearVelocity/10,self.held)
        local pos = (self.btn.position+(self.hpd):rotate(math.rad(self.btn.angle-self.hang)))
        self.btn:applyLinearImpulse(self.btn.mass*((self.held-pos)*16-self.btn.linearVelocity)/64,
        pos)
    end
    self.rope:draw()
    self.m:setRect(self.r,bp.x,bp.y,self.w,self.h,math.rad(self.btn.angle))
    self.m:draw()
end

function PhysicsButton:touched(t)
    local p = vec2(t.x,t.y)
    if t.state == BEGAN and self.btn:testPoint(p) then
        self.held = p
        self.hang = self.btn.angle
        self.tap = p
        self.hpd = (p-self.btn.position)
    elseif t.state == BEGAN then
        self.held = p
        self.hang = nil
    end
    if self.held then
        self.held = p
        if self.tap and p:dist(self.tap)>8 then self.tap = nil end
    end
    if t.state == ENDED then
        if self.tap then
            self.cb()
            self.finished = true
        end
        self.held = nil
        self.hang = nil
        self.hpd = nil
        self.tap = nil
    end
end


Roped = class()

function Roped:init(joint,segamnt)
    -- you can accept and set parameters here
   -- local joint = joint.j
    local pos1,pos2 = joint.anchorA,joint.anchorB
    --self.id = level.ropes:amount() + 1
    self.joint = joint
    self.p1 = pos1
    self.p2 = pos2
    self.m = mesh()
    self.m.texture = readImage("Cargo Bot:Claw Arm")
    self.r = {}
    self.dst = pos1:dist(pos2)*0.8
    self.segamnt = segamnt
    self.prevnt = DeltaTime
    self.seglen = self.dst/segamnt
    self.segtbl = {}
    for i=1,segamnt do
        self.segtbl[i] = {}
        self.segtbl[i].pos = pos1+(pos2-pos1):normalize()*self.seglen*i
        self.segtbl[i].prev = self.segtbl[i].pos
        self.segtbl[i].next = self.segtbl[i].pos +vec2(0,0)
        self.segtbl[i].ang = 0
        self.r[i] = self.m:addRect(self.segtbl[i].pos.x,self.segtbl[i].pos.y,5,self.seglen)
    end
    self.r[segamnt+1] = self.m:addRect(self.p2.x,self.p2.y,5,self.seglen)
    self.i = 0
end

function Roped:draw()
    self.p1 = self.joint.anchorA
    self.p2 = self.joint.anchorB
    local p,dpos,len,va,da,ea,vb,db,eb,grav,stiffness,dt,nt,np,delta
    local vd,vdl,diff
    grav,stiffness,nt = vec2(0,0.1),0.01,DeltaTime
    local segamnt = self.segamnt
    local segtbl = self.segtbl
    local maxl = self.seglen
    local prevpos = vec2()
    local nextpos = vec2()
    local pos = vec2()
    local segi = nil
    for i=1,segamnt do
        segi = segtbl[i]
        pos = segi.pos
        if i==1 then
            prevpos = self.p1
            nextpos = segtbl[i+1].pos
        elseif i==segamnt then
            prevpos = segtbl[i-1].pos
            nextpos = self.p2
        else
            prevpos = segtbl[i-1].pos
            nextpos = segtbl[i+1].pos
        end
        if not pos or not prevpos or not nextpos then  return end
        va = (pos-prevpos)
        da = (maxl-va:len())/va:len()
        ea = va:len()-maxl
        vb = (pos-nextpos)
        db = (maxl-vb:len())/vb:len()
        eb = vb:len()-maxl
        dt = (va*stiffness*da+vb*stiffness*db)
        if dt:len() > 0 then
            np = (pos-segtbl[i].prev) *(nt/self.prevnt) +(dt)*nt*nt
            segi.pos = segi.pos + np
        end
        if i == segamnt then
            vd = self.p2-segi.pos
            vdl = vd:len()
            diff = vdl-maxl
            vd = vd:normalize()
            if vd:len()>0 then
                segtbl[i].pos = segtbl[i].pos + vd*diff*0.5
            end
        end
        if i > 1 then
            vd = segtbl[i].pos-segtbl[i-1].pos
            vdl = vd:len()
            diff = vdl-maxl
            vd = vd:normalize()
            if vd:len()>0 then
                segi.pos = segi.pos - vd*diff*0.5
                segtbl[i-1].pos = segtbl[i-1].pos + vd*diff*0.5
            end
            elseif i==1 then
            vd = segi.pos-self.p1
            vdl = vd:len()
            diff = vdl-maxl
            vd = vd:normalize()
            if vd:len()>0 then
                segi.pos = segi.pos - vd*diff*0.5
            end
        end

        segi.prev = pos+grav
        pos = segi.pos
        self.prevnt = DeltaTime
        segtbl[i].ang = math.rad(angleOfPoint(pos-prevpos))+math.pi/2
        dpos = (prevpos+pos)/2
        len = (prevpos:dist(pos))
        self.m:setRect(self.r[i],dpos.x,dpos.y,10,len,segtbl[i].ang)
    end
    self.segtbl = segtbl
    p = segtbl[segamnt].pos
    dpos = p-(p-self.p2)*0.5
    len = (p:dist(self.p2))
    self.m:setRect(self.r[segamnt+1],dpos.x,dpos.y,10,len,math.rad(angleOfPoint(p-self.p2))-math.pi/2)
    self.m:draw()
end