local run = nil
local results = {
   speed = {},
   white = {},
   black = {}
}

local ball_collision = {
    white = {}
}

local cushion_collision  = {
    white = {}
}

local dot = function (a, b)
   -- Compute the dot (scalar) product of vectors a and b.
   return a[1]*b[1] + a[2]*b[2]
end

local angle = function (a, b)
   -- Compute the angle between vectors a and b.
   -- Return the angle in degrees.

   -- Use scalar product to compute the angle
   local ang = math.acos(dot(a, b) / (math.sqrt(dot(a, a)) * math.sqrt(dot(b, b))))
   -- Use the 3rd component of vector product to compute the sign of the angle
   local dir = a[1]*b[2] - a[2]*b[1]
   if dir > 0 then
       return math.deg(ang)
   else
       return -math.deg(ang)
   end
end

local splitFloats = function (str)
   --Split the given string to a table
   local t = {}
   local function helper (word)
      table.insert(t, word)
   return "" end
   if not str:gsub("-?%d+.%d+", helper):find"%S" then
      return t
   end
end

local prefix = os.getenv ("HOME") .. "/workspace/Webots/src/billiards/"

-- Create a data structure to hold example data
local examples = {
    white_init_x = {},
    white_init_y = {},
    black_init_x = {},
    black_init_y = {},
    cue_azimuth = {},
    cue_elevation = {},
    cue_sidespin = {},
    cue_follow = {},
    arm_velocity = {}
}

-- Read example data from a file
local i = 0
for line in io.lines(prefix .. "examples.txt") do
   i = i + 1
   local t = splitFloats(line)
   if (#t ~= 9) then
       print ("Wrong number of attributes in example:")
       print (line)
       common.iterate = false
   end
   examples.white_init_x[i] = tonumber(t[1])
   examples.white_init_y[i] = tonumber(t[2])
   examples.black_init_x[i] = tonumber(t[3])
   examples.black_init_y[i] = tonumber(t[4])
   examples.cue_azimuth[i] = tonumber(t[5])
   examples.cue_elevation[i] = tonumber(t[6])
   examples.cue_sidespin[i] = tonumber(t[7])
   examples.cue_follow[i] = tonumber(t[8])
   examples.arm_velocity[i] = tonumber(t[9])
end

--for i = 1, #examples.white_init_x do
--   print( string.format ("Example %i: %f %f %f %f %f %f %f %f %f",
--      i, examples.white_init_x[i], examples.white_init_y[i],
--      examples.black_init_x[i], examples.black_init_y[i],
--      examples.cue_azimuth[i], examples.cue_elevation[i],
--      examples.cue_sidespin[i], examples.cue_follow[i],
--      examples.arm_velocity[i]))
--end         

-- Check if we have any examples
if i == 0 then
   print "No data, quitting!"
   common.iterate = false
end

-- Set the output file
io.output(prefix .. "output.txt")

billiards.opening = {
   {examples.white_init_x[1], examples.white_init_y[1], billiards.ballradius},
   {examples.black_init_x[1], examples.black_init_y[1], billiards.ballradius}
}

billiards.decals = {
   {0.97, 0.97, 0.82},
   resources.patched "billiards/imagery/diffuse/eight.lc"
}

billiards.tipoffset = 0.5

resources.dofile "billiards/restart.lua"

table.insert (billiards.looking,
	      function ()
	         print ( string.format ("In function billiards.looking, run is %i", run))
		 bodies.observer.radius = 3
		 bodies.observer.latitude = 0
		 bodies.observer.longitude = billiards.tablewidth * 0.45
		 bodies.observer.elevation = math.rad (40)
		 bodies.observer.azimuth = math.rad (0)

--		 bodies.observer.isaiming = true
	      end)

table.insert (billiards.aiming,
	      function ()
	         print ( string.format ("In function billiards.aiming, run is %i", run))
		 joints.arm.motor = {examples.arm_velocity[run], billiards.cueforce}

		 bodies.observer.elevation = math.rad (40)

		 bodies.cue.azimuth = math.rad (examples.cue_azimuth[run])
		 bodies.cue.elevation = math.rad (examples.cue_elevation[run])
		 bodies.cue.sidespin = examples.cue_sidespin[run] * billiards.ballradius
		 bodies.cue.follow = examples.cue_follow[run] * billiards.ballradius
	      end)

table.insert (billiards.cuecollision,
	      function ()
		 if not results.speed[run] then
		    results.speed[run] = math.length (bodies.cue.velocity)
		 end
	      end)

table.insert (billiards.ballcollision,
	      function ()
		 print "We have a ball collision!"
		 if not ball_collision.white[run] then
		    ball_collision.white[run] = {bodies.balls[1].position[1],
						 bodies.balls[1].position[2]}
		 end
	      end)

table.insert (dynamics.collision,
	      function (a, b)
		 local ball, other

		 ball = a.isball and a or b
		 other = a.isball and b or a

		 if ball == bodies.balls[1] and other.iscushion then
		    print "White ball hit the cushion!"
		    if not cushion_collision.white[run] then
		       cushion_collision.white[run] = {bodies.balls[1].position[1],
						       bodies.balls[1].position[2]}
		    end
		 end
	      end)


table.insert (billiards.finished,
	      function ()
	         if run == nil then
		     -- Initialize run
		     run = 1
		 elseif run <= #examples.white_init_x then
		    -- Save the final positions of the white and black ball
		    results.white[run] = {bodies.balls[1].position[1],
					  bodies.balls[1].position[2]}

		    results.black[run] = {bodies.balls[2].position[1],
					  bodies.balls[2].position[2]}

		    -- Compute the reflection angle of the white ball
		    reflection_angle = nil
		    if ball_collision.white[run] then
		       -- White ball hit the black ball
		       if cushion_collision.white[run] then
			   -- White ball hit the cushion
			   reflection_angle = angle({-1, 0},
			      {cushion_collision.white[run][1]-ball_collision.white[run][1],
			       cushion_collision.white[run][2]-ball_collision.white[run][2]})
		       else
			   -- White ball stopped before hitting the cushion
			   reflection_angle = angle({-1, 0},
			      {results.white[run][1]-ball_collision.white[run][1],
			       results.white[run][2]-ball_collision.white[run][2]})
		       end
		    else
			-- No collision between the black and white ball
		       reflection_angle = '?'
		    end
		    
		    -- Output the results
		    print (string.format ("Run: %i\n" ..
					  "Cue speed: %s m/s\n" ..
					  "White: %s, %s\n" ..
					  "Black: %s, %s\n" ..
					  "Reflection angle: %s\n\n",
					  run,
				          results.speed[run],
					  results.white[run][1],
					  results.white[run][2],
					  results.black[run][1],
					  results.black[run][2],
					  reflection_angle))
		    io.write (string.format ("%s %s %s %s\n",
					     results.speed[run],
					     results.white[run][1],
					     results.white[run][2],
					     reflection_angle))
		    io.flush ()

		    -- Save current shot image to a file
--		    io.popen (string.format ("wget http://127.0.0.1:%i/drawtable?" ..
--					       "shot=%i -O %sshot%i.svg",
--					       network.port,
--					       run, prefix, run))
		    
		    run = run + 1
		 else
		    -- Quit the simulation
		    common.iterate = false
		 end

		 -- Reposition the balls
		 print ("Repositioning the balls...")
		 for i, ball in ipairs(bodies.balls) do
		    if ball == bodies.balls[1] then
		       -- White ball
		       print ("Setting white ball position")
		       ball.position = {examples.white_init_x[run],
		     		   examples.white_init_y[run],
		     		   billiards.ballradius}
		       
		    elseif ball == bodies.balls[2] then
		       -- Black ball
		       print ("Setting black ball position")
		       ball.position = {examples.black_init_x[run],
		     		   examples.black_init_y[run],
		     		   billiards.ballradius}
		    end
		    -- Set velocity and spin to 0
		    ball.velocity = {0, 0, 0}
		    ball.spin = {0, 0, 0}
		 end
	      end)