-- Use this function to perform your initial setup
function setup()
    joint = nil
    parameter.action("Play",play)
    parameter.action("Stop",stop)
    parameter.action("Pause",pause)
    parameter.action("Undo",undo)
    parameter.action("Redo",redo)
    parameter.action("Save",save)
    parameter.action("Load",lode)
    parameter.watch("scan")
    parameter.watch("#points")
    physics.continuous = true
    physics.pixelToMeterRatio = 32
    physics.iterations(1,4)
    physics.gravity(0,physics.gravity().y)
    fps = 60
    points = {}
    newpoint = false
    pedit = nil
    adjust = vec2(WIDTH/2,HEIGHT/2)
    ply = Ply(adjust)
    playing = false
    joints = {}
    scr = vec2()
    scan = vec2()
    scl = 1
    odist = nil
    oscl = nil
    opos = nil
    oscr = nil
    touches = {}
    undone = {}
    tmpobj = nil
    id = 0
    --joint:setCV(300,0.9)
    --joint:setFriction(10)
end

function save()
    local str = ""
    for k,v in pairs(points) do
        str=str.."^"
        for i,j in pairs(v) do
            str=str.."-"..tostring(j.x)..","..tostring(j.y)
        end
    end
    print(str)
    saveLocalData("lr",str)
end

function lode() --load
    local str = readLocalData("lr")
    if not str then return end
    print(str)
    local objsplit = split(str,"^")
    for k,v in pairs(objsplit) do
        local pnts = {}
        local pntsplit = split(v,"-")
        for i,j in pairs(pntsplit) do
            local vecsplit = split(j,",")
            local x = vecsplit[1]
            local y = vecsplit[2]
            pnts[i] = vec2(x,y)
        end
        table.insert(points,pnts)
    end
end

function moveScreen()
    if playing then
        local ball = ply:getPos()-adjust
        if scr.x~=ball.x and scr.y~=ball.y then
            scr = scr + ((adjust-ball)-scr)
        end
        local mt = adjust/scl-adjust
        scale(scl)
        translate(-ball.x+mt.x,-ball.y+mt.y)
    else
        local ball = scan
        if scr.x~=ball.x and scr.y~=ball.y then
            scr = scr + ((adjust-ball)-scr)
        end
        local mt = adjust/scl-adjust
        scale(scl)
        translate(-ball.x+mt.x,-ball.y+mt.y)
    end
end


function play()
    if playing then return end
    undone = {}
    for i=1,#points do
        id = i
        joints[i] = RailJoint(points[i],ply)
    end
    ply.body.type = DYNAMIC
    if tmpobj then
        ply.body.linearVelocity = tmpobj.vel
        ply.body.angle = tmpobj.ang
        ply.body.angularVelocity = tmpobj.angvel
    end
    playing = true
end

function stop()
     if not playing then return end
    for i=1,#joints do
        joints[i]=nil
    end
    tmpobj = nil
    ply.body.position = ply.initp
    ply.body.linearVelocity = vec2()
    ply.body.type = STATIC
    scr = adjust
    scl = 1
    scan = ply:getPos()-adjust
    playing = false
end

function pause()
    if not playing then return end
    for i=1,#joints do
        joints[i]=nil
    end
    --ply.body.position = ply.initp
    tmpobj = {vel = ply.body.linearVelocity, ang = ply.body.angle, angvel = ply.body.angularVelocity}
    ply.body.type = STATIC
    scr = adjust
    scl = 1
    scan = ply:getPos()-adjust
    playing = false
end

function undo()
    if not playing then
        table.insert(undone,points[#points])
        table.remove(points,#points)
    end
end

function redo()
    if not playing then
        table.insert(points,undone[#undone])
        table.remove(undone,#undone)
    end
end

function touched(t)
    if playing then return end
    local tp = ((vec2(t.x,t.y)-adjust)/scl+(scan+adjust))
    local jedit = false--currently does nothing
    local td = Touch(t)
    if t.state == BEGAN then
        table.insert(touches,td)
    end
    if t.state == MOVING then
        for k,v in pairs(touches) do
            if v.id == td.id then
                touches[k] = td
            end
        end
    end
    if t.state == ENDED then
        for k,v in pairs(touches) do
            if v.id == td.id then
                --touches[k] = nil
                table.remove(touches,k)
            end
        end
    end
    if not jedit then
        if t.state == BEGAN then
            cedit = {math.huge,nil}
            for k,v in pairs(points) do
                for i=1,#v do
                    local pnt = v[i]
                    if tp:dist(pnt)<10/scl and tp:dist(pnt)<cedit[1] then
                        cedit = {tp:dist(pnt),points[k][i]}
                    end
                end
            end
            pedit = cedit[2]
            if not pedit then
                newpoint = true
            end

            if newpoint and #touches == 1 then
                points[#points+1] = {tp}
            end
            if #touches==2 then
                newpoint = nil
                pedit = nil
                table.remove(points,#points)
                if points[#points] and #points[#points]==1 then
                    table.remove(points,#points)
                end
                odist = touches[1].pos:dist(touches[2].pos)
                oscl = scl
                opos = (touches[1].pos+touches[2].pos)/2
                oscr = scan
            end
        end

        if t.state == MOVING then
            if #touches == 1 then
                if pedit then
                    pedit.x = tp.x
                    pedit.y = tp.y
                end
                if newpoint then
                    --print(vec2(tp.x,tp.y))
                    --print(points[#points][#points[#points]])
                    if vec2(tp.x,tp.y):dist(points[#points][#points[#points]])>10 then
                        table.insert(points[#points],tp)
                    end
                end
            end
            if #touches == 2 then
                local tf = touches
                local dps = (tf[1].pos+tf[2].pos)/2
                local dst = tf[1].pos:dist(tf[2].pos)
                scl = oscl+(dst-odist)/(vec2(WIDTH,HEIGHT):len()/2)*scl
                scan = oscr+(opos-dps)
                --print(scl)
            end
        end
        if t.state == ENDED then
            if pedit then
                pedit = nil
            end
            if newpoint then
                undone = {}
                newpoint = false
            end
            --scl = oscl+(dist-odist)/vec2(WIDTH,HEIGHT):len()
            oscl = nil
            odist = nil
            oscr = nil
        end
    end

end

function clear()
    for k,v in pairs(bodies) do
        bodies[k]=nil
    end
    collectgarbage("collect")
    bodies = {}
    joint = nil
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)
    text(#touches,100,50)
    pushMatrix()
        moveScreen()
    ply:draw()
    if not playing then
        for k,v in pairs(points) do
            ellipse(v[1].x,v[1].y,10)
            for i=2,#v do
                local p1 = v[i-1]
                local p2 = v[i]
                line(p1.x,p1.y,p2.x,p2.y)
            end
        end
    else
        scl = 1-ply.body.linearVelocity:len()/(ply.body.linearVelocity:len()/3+2000)
        for k,v in pairs(joints) do
            v:draw()
        end
    end
    popMatrix()
    if newpoint then
        local tbl = points[#points]
        for i=2,#tbl do
            local p1 = tbl[i-1]
            local p2 = tbl[i]
            --line(p1.x,p1.y,p2.x,p2.y)
        end
    end

end
--closest point on line (point,start,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

linesIntersect = function( a, b, c, d )
        -- parameter conversion
        local L1 = {X1=a.x,Y1=a.y,X2=b.x,Y2=b.y}
        local L2 = {X1=c.x,Y1=c.y,X2=d.x,Y2=d.y}

        -- Denominator for ua and ub are the same, so store this calculation
        local d = (L2.Y2 - L2.Y1) * (L1.X2 - L1.X1) - (L2.X2 - L2.X1) * (L1.Y2 - L1.Y1)

        -- Make sure there is not a division by zero - this also indicates that the lines are parallel.
        -- If n_a and n_b were both equal to zero the lines would be on top of each
        -- other (coincidental).  This check is not done because it is not
        -- necessary for this implementation (the parallel check accounts for this).
        if (d == 0) then
                return false
        end

        -- n_a and n_b are calculated as seperate values for readability
        local n_a = (L2.X2 - L2.X1) * (L1.Y1 - L2.Y1) - (L2.Y2 - L2.Y1) * (L1.X1 - L2.X1)
        local n_b = (L1.X2 - L1.X1) * (L1.Y1 - L2.Y1) - (L1.Y2 - L1.Y1) * (L1.X1 - L2.X1)

        -- Calculate the intermediate fractional point that the lines potentially intersect.
        local ua = n_a / d
        local ub = n_b / d

        -- The fractional point will be between 0 and 1 inclusive if the lines
        -- intersect.  If the fractional calculation is larger than 1 or smaller
        -- than 0 the lines would need to be longer to intersect.
        if (ua >= 0 and ua <= 1 and ub >= 0 and ub <= 1) then
                local x = L1.X1 + (ua * (L1.X2 - L1.X1))
                local y = L1.Y1 + (ua * (L1.Y2 - L1.Y1))
                return {true, x=x, y=y}
        end

        return false
end

function linesIntersects(p0x, p0y, p1x, p1y, p2x, p2y, p3x, p3y)

    local s1x, s1y, s2x, s2y;
    s1x = p1x - p0x;
    s1y = p1y - p0y;
    s2x = p3x - p2x;
    s2y = p3y - p2y;

    local s, t;
    s = (-s1y * (p0x - p2x) + s1x * (p0y - p2y)) / (-s2x * s1y + s1x * s2y);
    t = ( s2x * (p0y - p2y) - s2y * (p0x - p2x)) / (-s2x * s1y + s1x * s2y);

    if s >= 0 and s <= 1 and t >= 0 and t <= 1 then
        return true
    end

    return false
end
function vec(vec)
    return vec2(math.floor(vec.x*10)/10,math.floor(vec.y*10)/10)
end

function cross(i,b)
  return -i * b.y - i * b.x;
end
function crossvec(a,b)
    return -a.x*b.y - a.y*b.x
end

function split(inputstr, sep)
        local t={}
        local i=1
        for str in string.gmatch(inputstr, "([^"..sep.."]+)") do
                t[i] = str
                i = i + 1
        end
        return t
end

Ply = class()

function Ply:init(pos)
    self.body = physics.body(POLYGON,vec2(-30,-10),vec2(-30,10),vec2(30,10),vec2(30,-10))
    self.initp = pos
    self.body.position = pos
    self.body.type = STATIC
    self.body.gravityScale = 1
    self.body.mass = 1
    self.body.inertia = 1
    self.gravity = vec2(0,-100)
end


function Ply:getPos()
    return self.body.position
end


function Ply:draw()
    self.body.x = self.body.position.x
    self.body.y = self.body.position.y
    --self.body.linearVelocity = (self.body.linearVelocity)
    pushMatrix()
        translate(self.body.x,self.body.y)
        rotate(self.body.angle)
        sprite("SpaceCute:Rocketship",0,0,60,40)
    popMatrix()
end

--# RailJoint
RailJoint = class()

function RailJoint:init(points,...)
    self.id = id
    self.points = points
    local args = {...}
    self.bodies = {}
    for k,v in pairs(args) do
        self.bodies[k] = v
        self.bodies[k].body = v.body
        v.linearDamping=0
    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 cart = body.body
    local cld = vec2(1,0):rotate(math.rad(cart.angle))
    local clp = (cart.linearVelocity)
    local tposr = (cart.position+vec2(25,-20):rotate(math.rad(cart.angle)))
    local cposr = (cart.position+vec2(25,10):rotate(math.rad(cart.angle)))
    local bposr = (cart.position+vec2(25,20):rotate(math.rad(cart.angle)))
    local tposl = (cart.position+vec2(-25,-20):rotate(math.rad(cart.angle)))
    local cposl = (cart.position+vec2(-25,10):rotate(math.rad(cart.angle)))
    local bposl = (cart.position+vec2(-25,20):rotate(math.rad(cart.angle)))
    --right intersection--
    local closest = {math.huge,0,nil,nil,nil}
    local intersectr
    for i=2,#self.points do
        local p1,p2 = self.points[i-1],self.points[i]
        local cp = cpol(tposr,p1,p2)
        if not intersectr then
            intersectr = linesIntersects(cposr.x,cposr.y,tposr.x,tposr.y,p1.x,p1.y,p2.x,p2.y)
        end
        if closest[1]>cp:dist(tposr) then
            closest = {cp:dist(tposr),i,cp,p1,p2}
        end
    end
    local distr = closest[1]
    local posr = closest[3]
    local p1r,p2r = closest[4],closest[5]
    local dlr = (tposr-posr):normalize()
    local posrr = cposr+((posr)-cposr):normalize()*30
    --intersectr = linesIntersects(cposr.x,cposr.y,posrr.x,posrr.y,p1r.x,p1r.y,p2r.x,p2r.y)
    pushStyle()
        resetStyle()
        fill(255,0,0)
        ellipse(posr.x,posr.y,10)
        stroke(255,0,0,255)
        strokeWidth(3)
        line(p1r.x,p1r.y,p2r.x,p2r.y)
        line(cart.x,cart.y,cart.x+cld.x*50,cart.y+cld.y*50)
    popStyle()

    --left intersection--
    local closest = {math.huge,0,nil,nil,nil}
    local intersectl
    for i=2,#self.points do
        local p1,p2 = (self.points[i-1]),(self.points[i])
        local cp = cpol(tposl,p1,p2)
        if not intersectl then
            intersectl = linesIntersects(cposl.x,cposl.y,tposl.x,tposl.y,p1.x,p1.y,p2.x,p2.y)
        end
        if closest[1]>cp:dist(tposl) then
            closest = {cp:dist(tposl),i,cp,p1,p2}
        end
    end
    local distl = closest[1]
    local posl = closest[3]
    local p1l,p2l = closest[4],closest[5]
    local dll = (tposl-posl):normalize()
    local posll = cposl+((posl)-cposl):normalize()*30
    --intersectl = linesIntersects(cposl.x,cposl.y,posll.x,posll.y,p1l.x,p1l.y,p2l.x,p2l.y)
    pushStyle()
        resetStyle()
        fill(0,0,255)
        ellipse(posl.x,posl.y,12)
        stroke(0, 0, 255, 255)
        strokeWidth(3)
        line(p1l.x,p1l.y,p2l.x,p2l.y)
    popStyle()


    --[[if intersectr and intersectl then
        local cang = angleOfPoint(posr-posl)-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]]
    local impr = 0
    if intersectl and posl:dist(tposl)>1 then
        --
        ellipse(tposl.x,tposl.y,8)
        local vapl = cart:getLinearVelocityFromWorldPoint(tposl)*cart.mass*0.5
        local normal = (p1l-p2l):rotate(math.pi/2):normalize()
        cart:applyForce(cart.mass*((posl-tposl)*4)-(dll):dot(vapl)*(dll),tposl)
    end
    local impl = 0
    if intersectr and posr:dist(tposr)>1 then
        ellipse(tposr.x,tposr.y,8)
        local vapr = cart:getLinearVelocityFromWorldPoint(tposr)*cart.mass*0.5
        local normal = (p1r-p2r):rotate(math.pi/2):normalize()
        cart:applyForce(cart.mass*((posr-tposr)*4)-(dlr):dot(vapr)*(dlr),tposr)
    end

    if intersectl or intersectr then

    end
end

function RailJoint:json()
    local str = "&"
    for k,v in pairs(self.points) do
        str = str..tostring(v.x)..","..tostring(v.y)
    end
    return str
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]
        end
    end
end

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


Touch = class()

function Touch:init(t)
    if t then
    self.x = t.x
    self.y = t.y
    self.pos = vec2(t.x,t.y)
    self.id = t.id
    self.deltaX = t.deltaX
    self.deltaY = t.deltaY
    self.prevX = t.prevX
    self.prevY = t.prevY
    self.state = t.state
    self.tapCount = t.tapCount
        self.time = 2
    return self
    end
end