/*
-----------------------------------------------------------------------------
This source file is part of OpenSpace3D
For the latest info, see http://www.openspace3d.com

Copyright (c) 2010 I-maginer

This program is free software; you can redistribute it and/or modify it under
the terms of the GNU Lesser General Public License as published by the Free Software
Foundation; either version 2 of the License, or (at your option) any later
version.

This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License along with
this program; if not, write to the Free Software Foundation, Inc., 59 Temple
Place - Suite 330, Boston, MA 02111-1307, USA, or go to
http://www.gnu.org/copyleft/lesser.txt

You may alternatively use this source under the terms of a specific version of
the OpenSpace3D Unrestricted License provided you have obtained such a license from
I-maginer.
-----------------------------------------------------------------------------
*/

#include "Prerequisites.h"
#include "ArManager.h"
#include "ArCameraParam.h"

#include <vector>

// Initialize singleton
ArManager* ArManager::_singleton = 0;

ArManager::ArManager()
{
	detector = new aruco::MarkerDetector();
}

ArManager::~ArManager()
{
	SAFE_DELETE(detector);
}

ArManager* ArManager::GetInstance()
{
  if (0 == _singleton)
  {
    _singleton =  new ArManager();
  }

  return _singleton;
}

void ArManager::Kill()
{
  if (0 != _singleton)
  {
    delete _singleton;
    _singleton = 0;
  }
}

//imgSize : size of image in meters
void ArManager::UpdateMarkers(cv::Mat image, ArCameraParam arCameraParam, bool reversedBitmap, bool debugDraw)
{
  // sharpen picture for better detection result
	/*
  cv::Mat frame;
  cv::GaussianBlur(image, frame, cv::Size(0, 0), 3);
  cv::addWeighted(image, 3.5, frame, -3.0, 0, frame);
  */

	// Put all markers' visibility to false value
	reinitVisibilityMarkers();
  
  // camera param
  aruco::CameraParameters camParam = arCameraParam.GetCameraParameter();
    
	//temp id detected
	int detectedID;
	MarkerList::iterator imarker;

	// List of dectected markers
	vector<aruco::Marker> markersDetected;

  /* Get Matrix*/
  double modelview_matrix[16];

  // Detect all markers seen on image
	detector->detect(image, markersDetected, camParam);
  
  for(unsigned int i = 0; i < markersDetected.size(); i++)
  {
	  // get id of the detected marker
	  detectedID = markersDetected[i].id;
	  for (imarker=markerList.begin(); imarker!=markerList.end(); ++imarker)
	  {
		  if((*imarker)->GetID() == detectedID)
		  {
        //recalculate Rvec and Tvec with marker size
        markersDetected[i].calculateExtrinsics((*imarker)->GetSize(), camParam);
			  markersDetected[i].glGetModelViewMatrix(modelview_matrix);
			  if(debugDraw)
			  {
				  markersDetected[i].draw(image, cv::Scalar(255, 0, 0), 1, true);
				  aruco::CvDrawingUtils::draw3dAxis(image, markersDetected[i], camParam);
			  }

				//determine the centroid
        Vector3 pixelPosition(0.0,0.0,0.0);
        for (int j=0;j<4;j++)
        {
            pixelPosition.x += markersDetected[i][j].x;
            pixelPosition.y += markersDetected[i][j].y; 
        }
        pixelPosition.z = sqrt(pow((markersDetected[i][1].x - markersDetected[i][0].x),2) + pow((markersDetected[i][1].y - markersDetected[i][0].y),2));
        pixelPosition.z += sqrt(pow((markersDetected[i][2].x - markersDetected[i][1].x),2) + pow((markersDetected[i][2].y - markersDetected[i][1].y),2));
        pixelPosition.z += sqrt(pow((markersDetected[i][3].x - markersDetected[i][2].x),2) + pow((markersDetected[i][3].y - markersDetected[i][2].y),2));
        pixelPosition.z += sqrt(pow((markersDetected[i][0].x - markersDetected[i][3].x),2) + pow((markersDetected[i][0].y - markersDetected[i][3].y),2));

        pixelPosition.x/=4.;
        pixelPosition.y/=4.;
        pixelPosition.z/=4.;

        //cv::circle(image, Point(pixelPosition.x, pixelPosition.y),pixelPosition.z/2,cv::Scalar(255,0,255));

        (*imarker)->SetPixelPosition(pixelPosition);

			  (*imarker)->SetVisible(true);
        (*imarker)->SetPosition(Vector3((reversedBitmap ? -modelview_matrix[12] : modelview_matrix[12]), modelview_matrix[13], modelview_matrix[14]));
        (*imarker)->SetOrientation(Quaternion::FromRotationMatrix(modelview_matrix, reversedBitmap));
		  }
	  }
	}
}

ArMarker* ArManager::AddMarker(int index, float size)
{
	ArMarker* marker = 0;
	if ((index >= 0) || (index < 1024))
	{
		marker = new ArMarker(index, size);
		markerList.insert(marker);
	}
	return marker;
}

void ArManager::RemoveMarker(ArMarker* marker)
{
	MarkerList::iterator imarker = markerList.find(marker);
	if (imarker != markerList.end())
	{
		markerList.erase(imarker);
		SAFE_DELETE(marker);
	}
}

void ArManager::reinitVisibilityMarkers()
{
	MarkerList::iterator imarker;
	for (imarker=markerList.begin(); imarker!=markerList.end(); ++imarker)
	{
		(*imarker)->SetVisible(false);
	}
}