local matrix = {};
matrix.new = function(v1, v2, v3)
  local self = {};
  self.x = vector.new(v1.x, v1.y, v1.z);
  self.y = vector.new(v2.x, v2.y, v2.z);
  self.z = vector.new(v3.x, v3.y, v3.z);

  self.multVec = function(self, v)
    return self.x * v.x + self.y * v.y + self.z * v.z;
  end

  self.multMat = function(self, m)
    return matrix.new(self:multVec(m.x), self:multVec(m.y), self:multVec(m.z));
  end

  self.transpose = function(self)
    return matrix.new( vector.new(self.x.x, self.y.x, self.z.x), vector.new(self.x.y, self.y.y, self.z.y), vector.new(self.x.z, self.y.z, self.z.z));
  end

  self.multTransVec = function (self, v)
    return vector.new(v.dot(self.x), v.dot(self.Y), v.dot(self.Z));
  end
end

local turtleState = {}
turtleState.new = function()
  local self = {};

  self.ensureWeHaveModem = function(self, right)
    if right == nil then
      right = true;
    end

    local p = peripheral.find("modem");
    if p == nil then
      for slot = 1, 16 do
        local itemDetail = turtle.getItemDetail(slot);
        if itemDetail ~= nil and itemDetail.name == "computercraft:peripheral" and itemDetail.damage == 1 then
          turtle.select(slot);
          if right then
            turtle.equipRight()
          else
            turtle.equipLeft()
          end
          p = peripheral.find("modem");
          break;
        end
      end
    end
    return p;
  end

  self.ensureWeHaveCompass = function(self, right)
    if right == nil then
      right = true;
    end

    local p = peripheral.find("compass");
    if p == nil then
      for slot = 1, 16 do
        local itemDetail = turtle.getItemDetail(slot);
        if itemDetail ~= nil and itemDetail.name == "minecraft:compass" then
          local oldSlot = turtle.getSelectedSlot();
          turtle.select(slot);
          if right then
            turtle.equipRight()
          else
            turtle.equipLeft()
          end
          turtle.select(oldSlot);
          p = peripheral.find("compass");
          break;
        end
      end
    end
    return p;
  end
  return self;
end

local area = {};
area.new = function(x, y, z, rotation)
  local self = {};
  self.globalPosition = vector.new(x, y, z);
  self.globalRotationMatrix = rotationToMatrix(rotation)

  self.getLocalPosition = function(self, state)
    state:ensureWeHaveModem();
    local globalPosition;
    while globalPosition == nil do
      globalPosition = vector.new(gps.locate(10));
    end
    globalPosition = globalPosition - self.globalPosition;
    return self.rotationToMatrix:multVec(globalPosition);
  end

  self.getLocalRotationMatrix = function(self, state)
    local matrix = rotationToMatrix(state:ensureWeHaveCompass().getFacing());
    return self.rotationToMatrix:multMat(matrix);
  end

  return self;
end

local function rotationToMatrix(rotation)
  if rotation == "north" then
    return matrix.new(vector.new(-1, 0,  0), vector.new(0, 1, 0), vector.new( 0, 0, -1));
  elseif rotation == "east" then
    return matrix.new(vector.new( 0, 0, 1), vector.new(0, 1, 0), vector.new( -1, 0,  0));
  elseif rotation == "south" then
    return matrix.new(vector.new( 1, 0,  0), vector.new(0, 1, 0), vector.new( 0, 0,  1));
  elseif rotation == "west" then
    return matrix.new(vector.new( 0, 0, -1), vector.new(0, 1, 0), vector.new( 1, 0,  0));
  else
    return nil;
  end
end

local state = turtleState.new();
local a = area.new(-272, 12, 244, "west");
print(a:getLocalPosition(state));
local forward = vector.new(0, 0, 1);
print(a:getLocalRotationMatrix(state):multVec(forward));