require 'csvigo'
require 'image'
local pl = require 'pl.import_into' ()
cv = require 'cv'
require 'cv.imgproc'

local utils = {}

function utils.project(transformation, points)
   if points:dim() == 1 then
      points = points:repeatTensor(1, 1)
   end
   points = torch.cat(points, torch.ones(points:size(1)), 2)
   points2D = (transformation * points:transpose(1, 2)):transpose(1, 2)
   points2D = torch.cdiv(points2D[{{}, {1, 2}}], points2D[{{}, {3}}]:expand(points2D:size(1), 2))
   return points2D:squeeze()
end

function utils.loadPointCloud(filename)
   local representationExists, representationInfo = pcall(function()
         return csvigo.load({path=filename, mode="large", header=false, separator=' ', verbose=false})
   end)
   local points = {}
   if representationExists and representationInfo ~= nil then
      local numPoints = tonumber(representationInfo[1][3])
      for pointIndex, point in pairs(representationInfo) do
	 if pointIndex >= 3 then
	    table.insert(points, {point[2], point[3], point[4]})
	 end
	 if pointIndex - 2 == numPoints then
	    break
	 end
      end
   end
   return torch.Tensor(points)
end

function utils.drawTopDownView(width, height, points)
   local X = points[{{}, 1}]
   local Y = points[{{}, 2}]
   local points2D = torch.cat(X, Y, 2)
   
   local mean = torch.mean(points2D, 1)
   points2D = points2D - mean:expandAs(points2D)
   points2D:div(math.sqrt(points2D:size(1) - 1))
   local indices = torch.randperm(points2D:size(1)):narrow(1, 1, 10000):long()
   points2D = points2D:index(1, indices)
   local w, _, _ = torch.svd(points2D:t())
   local angle = torch.atan2(w[1][2], w[1][1])
   local newX = X * torch.cos(angle) + Y * torch.sin(angle)
   local newY = -X * torch.sin(angle) + Y * torch.cos(angle)
   local newPoints2D = torch.cat(newX, newY, 2)

   local mins = torch.min(newPoints2D, 1)[1]
   local maxs = torch.max(newPoints2D, 1)[1]
   local paddingRatio = 0.05
   local padding = (maxs - mins) * paddingRatio
   mins = mins - padding
   maxs = maxs + padding
   
   local u = torch.round((newX - mins[1]) / (maxs[1] - mins[1]) * width)   
   local v = torch.round((newY - mins[2]) / (maxs[2] - mins[2]) * height)
   local uv = torch.cat(u, v, 2)


   local transformation = torch.zeros(3, 4)
   transformation[2][1] = torch.cos(angle)   
   transformation[2][2] = torch.sin(angle)
   transformation[2][4] = -mins[1]
   transformation[1][1] = -torch.sin(angle)   
   transformation[1][2] = torch.cos(angle)
   transformation[1][4] = -mins[2]
   transformation[3][4] = 1
   transformation[2] = transformation[2] * width / (maxs[1] - mins[1])
   transformation[1] = transformation[1] * height / (maxs[2] - mins[2])
   --transformation[1], transformation[2] = transformation[2], transformation[1]

   uv = utils.project(transformation, points)
   print(transformation)
   print(points[{{1, 5}}])
   print(uv[{{1, 5}}])
   
   local topDownView = torch.zeros(height, width)   
   for i = 1, uv:size(1) do
      local point = uv[i]
      topDownView[math.max(point[2], 1)][math.max(point[1], 1)] = topDownView[math.max(point[2], 1)][math.max(point[1], 1)] + 1
   end
   --image.save('test/pointcloud.png', topDownView)
   local pointDensity = 10.0
   topDownView = topDownView / pointDensity
   topDownView[topDownView:gt(1)] = 1

   
   return topDownView, transformation
end

return utils