-- Copyright (c) 2017, United States Government, as represented by the
-- Administrator of the National Aeronautics and Space Administration. 
--
-- All rights reserved. 
--
-- The Astrobee platform is licensed under the Apache License, Version 2.0
-- (the "License"); you may not use this file except in compliance with the
-- License.  You may obtain a copy of the License at
--
--     http://www.apache.org/licenses/LICENSE-2.0
--
-- Unless required by applicable law or agreed to in writing, software
-- distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
-- WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.  See the
-- License for the specific language governing permissions and limitations
-- under the License. 

--[[
  Computes the locations of the AR targets on the dock in World
  frame from measurements performed in the plane of the dock

  AR targets are currently caraterized by the positions of three
  corners.  The naming refering the a coordinate system attached
  to the target itself if looking at it with its label being
  readable:
    - top left
    - top right
    - bottom left

  The markers (from dock_markers) are expressed in a 2D coordinate
  system facilitating the printing (X to the right, Y going down). 

  We define a dock frame like this if the dock is considered
  with its face in the vertical plane
    - origin: coincide with the origin of the target drawing
    - X horizontal, positive to the right if looking at the target
    - Z vertical, positive pointing down
    - Y is thus horizontal, positive pointing out the dock

  We start by placing the markers from the drawing coordinates
  into the dock frame.  Then we apply a transform to find the
  coordinates if world frame by using the dock coordinates in
  the world frame. 

  The result of this script
--]]

require "rotations"
local matrix = require 'matrix'

drawing_unit = "mm"

markers = {
  --{id=41, size=40, pos={91, 15}},
  --{id=42, size=40, pos={-131, -55}},
  --{id=96, size=100, pos={-48, -48, 0}}
  {id=43, size=100, pos={0, 100}},
  {id=44, size=100, pos={100, 0}},
  {id=96, size=100, pos={0, 0}}
}

-- angle of a normal to the target plane with the horizontal plane,
-- positive around the X axis
dock_roll_angle = math.rad(-25)

-- angle of the dock X axis with the world X axis, around the
-- world Z axis
dock_yaw_angle = math.rad(-90)

-- dock origin expressed in world coordinate frame (meters)
dock_position = matrix{-0.7053, 0.3105, -0.8378}

print(string.format("Processing %d markers...\n", table.getn(markers)))

-- Compute the corners of the target in function of its size and position
function marker_corners(m, scale)
  -- express corners in dock frame (see intro)
  top_left = scale * matrix{m['pos'][1], 0.0, m['pos'][2]}
  top_right = top_left + scale * matrix{m['size'], 0.0, 0.0}
  bottom_left = top_left + scale * matrix{0.0, 0.0, m['size']}
  return top_left, top_right, bottom_left
end

-- Simple utility function to print a 3x1 matrix as a vector
function print_vec3(v, str)
  -- TODO should check the size first
  print(string.format("%s=(%.4f, %.4f, %.4f)",
    str, v[1][1], v[2][1], v[3][1]))
end

-- Compute rotation matrix to transform dock frame to world frame
roll_mat = rot_matrix(dock_roll_angle, 'x')
--print("\nroll matrix")
--matrix.print(roll_mat)
yaw_mat = rot_matrix(dock_yaw_angle, 'z')
--print("\nyaw matrix")
--matrix.print(yaw_mat)

Rdock2world = yaw_mat * roll_mat

-- Unit transformation
if ( drawing_unit == "mm") then
  print("tagets drawing expressed in millimeters")
  scale_factor = 0.001
elseif ( drawing_unit =="in") then
  print("targets drawing are expressed in inches")
  scale_factor = 0.0254
else
  print("Warning: targets drawing unit not defined or not recognized")
  print("         assume meters")
  scale_factor = 1.0
end
scaling_mat = matrix{
  {scale_factor, 0.0, 0.0},
  {0.0, scale_factor, 0.0},
  {0.0, 0.0, scale_factor}
}

print("\ndock to world")
matrix.print(Rdock2world)

-- Processing
markers_world = {}
for i=1,table.getn(markers) do
  marker_in_world = {}
  top_left, top_right, bottom_left = marker_corners(markers[i], scale_factor)
  print(string.format("corners for marker # %d", markers[i]['id']))
  print_vec3(top_left, "top_left")
  print_vec3(top_right, "top_right")
  print_vec3(bottom_left, "bottom_left")
  marker_in_world['id'] = markers[i]['id']
  marker_in_world['top_left']= Rdock2world * top_left + dock_position
  marker_in_world['top_right']= Rdock2world * top_right + dock_position
  marker_in_world['bottom_left']= Rdock2world * bottom_left + dock_position
  table.insert(markers_world, marker_in_world)
end

for k,v in pairs(markers_world) do
  print("-------- marker", k)
  print(string.format("id=%d", v['id']))
  print_vec3(v['top_left'], "top_left")
  print_vec3(v['top_right'], "top_right")
  print_vec3(v['bottom_left'], "bottom_left")
end

--[[
Reference positions for testing, computed from the matlab version:

ar43 = [ [0, -100, 0]; [100, -100, 0]; [0, -200, 0] ];
ar44 = [ [100, 0, 0]; [200, 0, 0]; [100, -100, 0] ];
ar96 = [ [0, 0, 0]; [100, 0, 0]; [0, -100, 0] ];

<ar id="43"
    topleft="-0.6630 0.3105 -0.7472"
    topright="-0.6630 0.2105 -0.7472"
    bottomleft="-0.6208 0.3105 -0.6565"
   />
  <ar id="44"
    topleft="-0.7053 0.2105 -0.8378"
    topright="-0.7053 0.1105 -0.8378"
    bottomleft="-0.6630 0.2105 -0.7472"
   />
  <ar id="96"
    topleft="-0.7053 0.3105 -0.8378"
    topright="-0.7053 0.2105 -0.8378"
    bottomleft="-0.6630 0.3105 -0.7472"
   />
   --]]
