
/* autopano-sift, Automatic panorama image creation
 * Copyright (C) 2004 -- Sebastian Nowozin
 *
 * This program is free software released under the GNU General Public
 * License, which is included in this software package (doc/LICENSE).
 */

/* BondBall.cs
 *
 * Generic preliminary position finding algorithm for panoramic imaging.
 *
 * (C) Copyright 2004 -- Sebastian Nowozin (nowozin@cs.tu-berlin.de)
 *
 * Any kind of panorama made from small angle input pictures (ie. no special
 * fisheye or wideangle lenses). The input images have to fullfil a strict
 * left-to-right or right-to-left order for the first row, which has to be
 * horizontal.
 */

using System;
using System.Collections;


public class BondBall
{
	public enum Direction {
		Unknown,
		LeftToRight,
		RightToLeft,
	}

	Direction dir = Direction.Unknown;
	double center = 0.0;
	double rotation = 0.0;

	int bottomDefault = -1;

	// 0 for left, 1 for right
	public BondBall (int bottomDir)
	{
		this.bottomDefault = bottomDir;
	}

	public BondBall ()
	{
	}

	// Orientation tolerance in degrees, to still be considered of as lying in
	// the same direction.
	double bondOrientationTolerance = 35.0;
	ArrayList sets = new ArrayList ();
	public ArrayList Sets {
		get {
			return (sets);
		}
	}

	MultiMatch.MatchSet first = null;
	public MultiMatch.MatchSet First {
		get {
			return (first);
		}
	}
	MultiMatch.MatchSet last = null;
	public MultiMatch.MatchSet Last {
		get {
			return (last);
		}
	}

	// The filenames of the first row.
	ArrayList firstRow = null;
	public ArrayList FirstRow {
		get {
			return (firstRow);
		}
		set {
			firstRow = value;
		}
	}

	private static bool IsWithinAngleDegree (double left, double right, double test)
	{
		while (left < 0.0)
			left += 360.0;
		while (right < 0.0)
			right += 360.0;
		while (test < 0.0)
			test += 360.0;

		// easy case, no wraparound
		if (left < right) {
			if (test >= left && test < right)
				return (true);

			return (false);
		}

		// left is bigger than right, this means they wrap at 0.0/360.0 degrees
		if (test >= left && test <= 360.0)
			return (true);
		if (test >= 0.0 && test < right)
			return (true);

		return (false);
	}

	// Initiate a new panorama positioning by starting with the first two
	// pictures gives in the 'first' matchset.
	//
	// Return true if we positively set up the first row orientation and its
	// picture orientation.
	// Return false if we cannot determine the orientations.
	public bool InitiateBond (MultiMatch.MatchSet first)
	{
		ImageMatchModel fit = first.BestMatchFit;
		this.first = last = first;
		sets.Add (first);

		double centerDegree = (fit.CenterAngle / (2.0 * Math.PI)) * 360.0;
		if (centerDegree < 0.0)
			centerDegree += 360.0;

		// Simple cases: normalized rotation, left to right or right to left
		// A. Left to right
		if (IsWithinAngleDegree (0.0 - bondOrientationTolerance,
			0.0 + bondOrientationTolerance, centerDegree))
		{
			center = 0.0;
			rotation = 0.0;
			dir = Direction.LeftToRight;

			return (true);
		// B. Right to left
		} else if (IsWithinAngleDegree (180.0 - bondOrientationTolerance,
			180.0 + bondOrientationTolerance, centerDegree))
		{
			center = 180.0;
			rotation = 0.0;
			dir = Direction.RightToLeft;

			return (true);
		}

		// Ambiguous case: 90/270 degrees, tilted by -90 or 90 degrees. Now,
		// we first determine where the bottom lies in the pictures (left or
		// right)
		int[] bottomDir = new int[2];
		for (int n = 0 ; n < 2 ; ++n) {
			if (bottomDefault != -1) {
				bottomDir[n] = bottomDefault;
			} else {
				bottomDir[n] = GuessBottomOrientation (first.GetOriginalKeys (n),
					n == 0 ? first.xDim1 : first.xDim2);
			}
		}

		// Cannot tell or directions mismatch
		if (bottomDir[0] == -1 || bottomDir[1] == -1 ||
			bottomDir[0] != bottomDir[1])
		{
			Console.WriteLine ("Error: The picture orientation is ambiguous.");
			Console.WriteLine ("       We have either -90 or 90 degree input pictures.\n");

			Console.WriteLine ("       To possibly resolve this, please try to use the --bottom-is-left");
			Console.WriteLine ("       or --bottom-is-right option.");

			return (false);
		}

		Console.WriteLine ("First row pictures have the bottom on the {0} side.",
			bottomDir[0] == 0 ? "left" : "right");

		// Resolve the ambiguity among four cases
		if (IsWithinAngleDegree (90.0 - bondOrientationTolerance,
			90.0 + bondOrientationTolerance, centerDegree))
		{
			dir = Direction.LeftToRight;
			center = 90.0;

			if (bottomDir[0] == 0)
				rotation = -90.0;
			else
				rotation = 90.0;
		} else if (IsWithinAngleDegree (270.0 - bondOrientationTolerance,
			270.0 + bondOrientationTolerance, centerDegree))
		{
			dir = Direction.RightToLeft;
			center = 270.0;

			if (bottomDir[0] == 0)
				rotation = -90.0;
			else
				rotation = 90.0;
		}

		return (true);
	}

	// Return true on end.
	public bool AddRowImage (MultiMatch.MatchSet next)
	{
		if (String.Compare (last.File2, next.File1) != 0)
			throw (new ArgumentException
				("The row is not continuous in the next matchset."));

		// Get angle between pictures
		ImageMatchModel fit = next.BestMatchFit;
		double centerDegree = (fit.CenterAngle / (2.0 * Math.PI)) * 360.0;
		if (centerDegree < 0.0)
			centerDegree += 360.0;

		if (IsWithinAngleDegree (center - bondOrientationTolerance,
			center + bondOrientationTolerance,
			centerDegree))
		{
			last = next;
			sets.Add (next);
			Console.WriteLine ("Angle {0:N2} degrees fits, adding \"{1}\" to row.",
				centerDegree, next.File2);

			return (false);
		} else {
			Console.WriteLine ("Angle {0:N2} degrees of \"{1}\" is outside of {2:N2}+/-{3:N2} range, row end reached",
				centerDegree - 360.0, next.File2, center, bondOrientationTolerance);

			return (true);
		}
	}

	// One of the pictures in ms must be in the positions hashtable already,
	// and the other must be outside.
	public Position EstimateImage (MultiMatch.MatchSet ms)
	{
		if (positions == null)
			throw (new Exception ("Positions hashtable is empty, cannot align."));

		bool image1known = positions.Contains (ms.File1);
		string knownFile = image1known ? ms.File1 : ms.File2;
		string unknownFile = image1known ? ms.File2 : ms.File1;

		Position knownPos = (Position) positions[knownFile];
		ImageMatchModel fit = ms.BestMatchFit;

		/*
		double centerDegree = (fit.CenterAngle / (2.0 * Math.PI)) * 360.0;
		double newRotation = centerDegree; +
			(knownPos.Rotation * 2.0 * Math.PI) / 360.0;
		*/
		//Console.WriteLine ("fit.RotationAngle = {0}", fit.RotationAngle);
		double newRotation = fit.RotationAngle +
			(knownPos.Rotation * 2.0 * Math.PI) / 360.0;
		newRotation = (newRotation / (2.0 * Math.PI)) * 360.0;

		/*Console.WriteLine ("new rotation for image \"{0}\" is {1} degrees",
			unknownFile, newRotation);

		Console.WriteLine ("  centerangle = {0}", fit.CenterAngle);
		Console.WriteLine ("  sin(centerangle) = {0}", Math.Sin (fit.CenterAngle));
		Console.WriteLine ("  cos(centerangle) = {0}", Math.Cos (fit.CenterAngle));
		*/

		double yaw = knownPos.Yaw;
		double pitch = knownPos.Pitch;
		/*Console.WriteLine ("fit.ShiftWidth = {0}", fit.ShiftWidth);
		Console.WriteLine ("ms.xDim1 = {0}", ms.xDim1);*/
		double angleShiftHorizontal = Math.Min (
			((double) fit.ShiftWidth / (double) ms.xDim1) * yawStep * 2.0,
			20.0);
		double angleShiftVertical = Math.Min (
			((double) fit.ShiftWidth / (double) ms.yDim1) * yawStep * 2.0,
			20.0);
		/*Console.WriteLine ("shift h/v: {0}, {1}, yawStep = {2}",
			angleShiftHorizontal, angleShiftVertical, yawStep);*/

		/*
		Console.WriteLine ("## CenterAngle = {0}, RotationAngle = {1}",
			fit.CenterAngle, fit.RotationAngle);
		Console.WriteLine ("## knownPos.Rotation = {0}", knownPos.Rotation);
		Console.WriteLine ("## newPos.Rotation = {0}", newRotation);
		*/

		double ca = fit.CenterAngle + ((knownPos.Rotation / 360.0) * 2.0 * Math.PI);
		/*ca += Math.PI;	// invert direction
		if (ca >= (2.0 * Math.PI))
			ca -= 2.0 * Math.PI;*/
		//Console.WriteLine ("ca = {0}", ca);

		double reverseFactor = image1known ? 1.0 : -1.0;
		//Console.WriteLine ("yaw -= {0}", reverseFactor * Math.Cos (ca) * angleShiftHorizontal);
		//Console.WriteLine ("pitch -= {0}", reverseFactor * Math.Sin (ca) * angleShiftVertical);

		yaw -= Math.Cos (ca) * angleShiftHorizontal;
		pitch -= reverseFactor * Math.Sin (ca) * angleShiftVertical;

		return (new Position (yaw, pitch, newRotation));
		/*
		if (centerDegree < 0.0)
			centerDegree += 360.0;
		*/
	}

	public class Position
	{
		double yaw;
		public double Yaw {
			get {
				return (yaw);
			}
		}

		double pitch;
		public double Pitch {
			get {
				return (pitch);
			}
		}

		double rotation;
		public double Rotation {
			get {
				return (rotation);
			}
		}

		private Position ()
		{
		}

		public Position (double yaw, double pitch, double rotation)
		{
			this.yaw = yaw;
			this.pitch = pitch;
			this.rotation = rotation;
		}

		public override string ToString ()
		{
			return (String.Format ("pos (yaw = {0:N2}, pitch = {1:N2}, rotation = {2:N2})",
				yaw, pitch, rotation));
		}
	}

	// From image filename as key to a Position class.
	Hashtable positions = null;
	public Hashtable Positions {
		get {
			return (positions);
		}
	}

	// One image yaw move step.
	double yawStep;

	public void StretchImages (bool is360)
	{
		positions = new Hashtable ();

		// In case its a 360 degree panorama, things are easy
		yawStep = 360.0 / FirstRow.Count;

		// In case it's not a full pano, we lower the value. To have an exact
		// value is really not so important, its just a rough minimum for the
		// later optimization process to base its work upon.
		if (is360 == false) {
			// Now we employ heuristics/educated guessing... humpf.
			yawStep = Math.Min (20.0, yawStep);
		}

		// Set the yaw, pitch and rotation values
		int xs = 0;
		foreach (string filename in FirstRow) {
			double yawCur = xs * yawStep;

			// Roughly align in center for non 360 degree panoramas.
			if (is360 == false)
				yawCur -= (FirstRow.Count / 2.0) * yawStep;
			Console.WriteLine ("DEBUG {0}: yawCur = {1}, xs: {2}, yawStep = {3}", filename, yawCur, xs, yawStep);

			//Console.WriteLine ("{0}: yaw = {1}, rotation = {2}", filename, yawCur, rotation);
			positions.Add (System.IO.Path.GetFileName (filename),
				new Position (yawCur, 0.0, rotation));
			xs += 1;
		}
	}

	// Guesses where the bottom of the image is, based on keypoint density.
	// This is highly experimental code.
	// The only cases considered is left/right, as we assume no image has the
	// bottom on the top.
	// TODO: maybe add a third case: bottom at bottom, though this could be
	//    reliably estimated based on image order alone.
	//
	// Return -1 in case its not certain,
	// return 0 in case the bottom is estimated to be on the west/left side
	// return 1 in case the bottom is estimated to be on the east/right side
	public static int GuessBottomOrientation (KeypointN[] keys, int xDim)
	{
		double xAccum = 0.0;

		foreach (KeypointN kp in keys)
			xAccum += kp.X;

		xAccum /= keys.Length;
		//Console.WriteLine ("xDim: {0}, xAverage: {1}", xDim, xAccum);

		double averageDivergenceBoundary = xDim / 12.0;
		if (xAccum <= ((xDim / 2) - averageDivergenceBoundary))
			return (0);
		else if (xAccum >= ((xDim / 2) + averageDivergenceBoundary))
			return (1);

		return (-1);
	}

	public override string ToString ()
	{
		return (String.Format ("{0}, center {1}, rotation {2}",
			dir, center, rotation));
	}
}


