lineFollower

local ar = peripheral.find("arController")

local draw = require("drawingAPI")
--[[
    bindOutputMode(mode)
    drawTri(p1,p2,p3,c,pr)
]]

local function multiplyVerts(mat, vert) --m == matrix, v == vertice list
    if #mat ~= 16 then
        error("Matrix must have length of 16 (4x4 matrix)")
    end
    local result = {}
    for i = 1, #vert, 4 do --Iterate through each vertice
        for k = 1, 4 do --Multiply said vertice by the given matrix. result[1 -> 4] are equal to a 4x1 matrix.
            result[i+k-1] =
              ( mat[k*4-3] * vert[i]   )
            + ( mat[k*4-2] * vert[i+1] )
            + ( mat[k*4-1] * vert[i+2] )
            + ( mat[k*4]   * vert[i+3] )
        end
    end
    return result
end

local function drawSolidObj(pr, vL) --iL == index List. vL == vertex list. cL == color list
    local cL = iL.colorList
        --OPTIMIZATION here?. Probably should convert to 1D at least
    for i = 1, #iL, 3 do --Try to draw a triangle
        drawTri( vL[i], vL[i+1], vL[i+2], cL[(i+2)/3], pr )
    end
end

local function makeRotation(eulers)
    local x = math.rad(eulers.x)
    local y = math.rad(eulers.y)
    local z = math.rad(eulers.z)

    local sx = math.sin(x)
    local sy = math.sin(y)
    local sz = math.sin(z)

    local cx = math.cos(x)
    local cy = math.cos(y)
    local cz = math.cos(z)

    return
    {
        cy * cz, -cy * sz, sy, 0,
        (sx * sy * cz) + (cx * sz), (-sx * sy * sz) + (cx * cz), -sx * cy, 0,
        (-cx * sy * cz) + (sx * sz), (cx * sy * sz) + (sx * cz), cx * cy, 0,
        0, 0, 0, 1, }
end

local function makeTranslation(translation)
    return
    {
        1, 0, 0, translation.x,
        0, 1, 0, translation.y,
        0, 0, 1, translation.z,
        0, 0, 0, 1, }
end

local function makeScale(scale)
    return
    {
        scale.x, 0, 0, 0,
        0, scale.y, 0, 0,
        0, 0, scale.z, 0,
        0, 0, 0, 1 }
end

local function newSqr()
    local objData = {
        scale = vector.new(1,1,1),
        loc = vector.new(0,0,0),
        rot = vector.new(0,0,0),
        colorList = { 0xE56399, 0x7F96FF, },
        indexList = { 1,3,2, 3,4,2, },
        vertices = {
            -0.5, -0.5, -0.5, 1,
             0.5, -0.5, -0.5, 1,
            -0.5,  0.5, -0.5, 1,
             0.5,  0.5, -0.5, 1, } }
        for i,val in ipairs(objData.indexList) do
            objData.indexList[i] = val*4-3
        end
    return objData
end

local oL = {
    sqr1 = newSqr()
}

oL.sqr1.scale = vector.new(10,10,10)

local rotInc = vector.new(3,3,3)

for i = 1, 100 do
    local projected = {}
    oL.sqr1.rot = oL.sqr1.rot:add(rotInc)

    for name,objectData in pairs(oL) do
        projected[name] = multiplyVerts(makeTranslation(objectData.loc), makeRotation(objectData.rot), makeScale(objectData.scale), objectData.vertices)
        projected[name].colorList = oL.colorList
    end

    sleep(0.8)
    ar.clear()

    for name,vL in pairs(projected) do
        draw.drawSolidObj(ar,vL)
    end
end