local Turtle = {}
local matrix = {}

local x = { x=1, y=0, z=0 }
local y = { x=0, y=1, z=0 }
local z = { x=0, y=0, z=1 }

local round = function(n)
   return math.floor(n+0.5)
end

Turtle.__index = Turtle

function Turtle.new(pos)
   local self = setmetatable({}, Turtle)
   
   self.min = { x = pos.x, y = pos.y, z = pos.z }
   self.max = { x = pos.x, y = pos.y, z = pos.z }

   self.pos = vector.new(pos)
   self.rot = matrix.orient(matrix.new(), math.pi/2, z)
   self.stk = {}

   self.mat = 1
   self.pen = true

   self.nodes = {}
   return self
end

function Turtle:checkPos(pos)
   if self.pen == true then
      if pos.x < self.min.x then self.min.x = pos.x end
      if pos.y < self.min.y then self.min.y = pos.y end
      if pos.z < self.min.z then self.min.z = pos.z end
      if pos.x > self.max.x then self.max.x = pos.x end
      if pos.y > self.max.y then self.max.y = pos.y end
      if pos.z > self.max.z then self.max.z = pos.z end
      local rpos = { x=round(pos.x), y=round(pos.y), z=round(pos.z)}
      table.insert(self.nodes, { pos=rpos, mat=self.mat } )
   end
end

function Turtle:forward(n)
   for i=1, n do
      self:checkPos(self.pos)
      self.pos = self.pos + matrix.transpose(self.rot, x)
   end
end

function Turtle:penDown()
   self.pen = true
end

function Turtle:penUp()
   self.pen = false
end

function Turtle:push()
   table.instert(self.stk, { rot = self.rot, pos = self.pos })
end

function Turtle:pop()
   local stack = table.remove(self.stk)
   self.pos = stack.pos
   self.rot = stack.rot
end

function Turtle:roll(angle)
   local tmp = matrix.id()
   matrix.orient(tmp, angle*math.pi/180, x)
   self.rot = matrix.mul(tmp, self.rot)
end

function Turtle:pitch(angle)
   local tmp = matrix.id()
   matrix.orient(tmp, angle*math.pi/180, y)
   self.rot = matrix.mul(tmp, self.rot)
end

function Turtle:yaw(angle)
   local tmp = matrix.id()
   matrix.orient(tmp, angle*math.pi/180, z)
   self.rot = matrix.mul(tmp, self.rot)
end

function Turtle:setMat(mat)
   self.mat =  minetest.get_content_id(mat) 
end

function Turtle:spawn()
   local vm = VoxelManip()
   local min, max = vm:read_from_map(self.min, self.max)
   local area = VoxelArea:new({ MinEdge = min, MaxEdge = max})
   local data = vm:get_data()
   
   for _, node in pairs(self.nodes) do
      local index = area:indexp(node.pos)
      data[index] = node.mat
   end
   
   vm:set_data(data)
   vm:write_to_map()
   vm:update_map()
   print(string.format("placed %i nodes", #self.nodes))
end

function turtfunk(pos)
   local t = Turtle.new(pos)
   t:setMat("wool:red")
   t:penDown()
   t:roll(45)
   t:forward(20)
   t:yaw(90)
   t:forward(20)
   t:spawn()
end

matrix.new = function()
   local m = {}
   for i = 1,4 do m[i] = {}
   for j = 1,4 do m[i][j] = 0
   end
   end
   return m
end

matrix.id = function()
   local m = {}
   for i = 1, 4 do m[i] = {}
   for j = 1, 4 do
      if i == j then
	 m[i][j] = 1
      else
	 m[i][j] = 0
      end
   end
   end
   return m
end

matrix.mul = function(m1, m2)
   local m = {}
   for i = 1, 4 do m[i] = {}
   for j = 1, 4 do
      local num = 0
      for n = 1, 4 do
	 num = num + m1[i][n] * m2[n][j]
      end
      m[i][j] = num
   end
   end
   return m
end

matrix.orient = function(m, angle, axis)
   local c = math.cos(angle)
   local s = math.sin(angle)
   local t = 1.0 - c
   
   local tx  = t * axis.x
   local ty  = t * axis.y
   local tz  = t * axis.z
   local sx  = s * axis.x
   local sy  = s * axis.y
   local sz  = s * axis.z
   
   m[1][1] = tx * axis.x + c
   m[1][2] = tx * axis.y + sz
   m[1][3] = tx * axis.z - sy
   
   m[2][1] = ty * axis.x - sz
   m[2][2] = ty * axis.y + c
   m[2][3] = ty * axis.z + sx
   
   m[3][1] = tz * axis.x + sy
   m[3][2] = tz * axis.y - sx
   m[3][3] = tz * axis.z + c
   return m
end

matrix.transpose = function (m, v)
   local x = m[1][1]*v.x + m[2][1]*v.y + m[3][1]*v.z + m[4][1]
   local y = m[1][2]*v.x + m[2][2]*v.y + m[3][2]*v.z + m[4][2]
   local z = m[1][3]*v.x + m[2][3]*v.y + m[3][3]*v.z + m[4][3]
   return vector.new(x,y,z)
end

minetest.register_tool("fuckit:tool", {
   description = "turtle spawner/editor",
   inventory_image = "default_stick.png",
   on_use = function(itemstack, user, pointed_thing)
      local t1 = os.clock()
      local pos = pointed_thing.above
      if not pos then return end
      turtfunk(pos)
      local time = (os.clock() - t1) * 1000
      print(string.format("elapsed time: %.2fms", time))
   end
})