/*****************************
Copyright 2011 Rafael Muñoz Salinas. All rights reserved.

Redistribution and use in source and binary forms, with or without modification, are
permitted provided that the following conditions are met:

   1. Redistributions of source code must retain the above copyright notice, this list of
      conditions and the following disclaimer.

   2. Redistributions in binary form must reproduce the above copyright notice, this list
      of conditions and the following disclaimer in the documentation and/or other materials
      provided with the distribution.

THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED
WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR
CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

The views and conclusions contained in the software and documentation are those of the
authors and should not be interpreted as representing official policies, either expressed
or implied, of Rafael Muñoz Salinas.
********************************/

#include <iostream>
 
#include "Ogre.h"
#include "OgreRectangle2D.h"
//#include <OIS/OIS.h>
 
#include <aruco/aruco.h>  
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <markermap.cpp> 
 
/// Ogre general variables
Ogre::Root* root;
//OIS::InputManager* im;
//OIS::Keyboard* keyboard;

/// Ogre background variables
Ogre::PixelBox mPixelBox;
Ogre::TexturePtr mTexture;
 
/// Ogre scene variables
Ogre::SceneNode* ogreNode;
Ogre::AnimationState *baseAnim, *topAnim;
 Ogre::SceneNode *ogreNodeChild;
/// ArUco variables
cv::VideoCapture TheVideoCapturer;
cv::Mat TheInputImage, TheInputImageUnd;
aruco::CameraParameters TheCameraParams, TheCameraParamsUnd;
float TheMarkerSize=0.045;
aruco::MarkerMap TheMarkerMap;
aruco::MarkerDetector TheMarkerDetector;   
aruco::MarkerMapPoseTracker TheMSPoseTracker;
float sindbad_angle = 0.0;
float sindbad_radius = 5.0;
float sindbad_t = 0.0;
float sindbad_speed = 1.0;
float sindbad_flip = 1.0;
 
  
int initOgreAR(aruco::CameraParameters camParams, unsigned char* buffer, std::string resourcePath="");
bool readParameters(int argc, char** argv);

void usage()
{
    cout<<" This program test Ogre version of ArUco (board version) \n\n";
    cout<<" Usage <video.avi>|live <camera.yml> <markermapconfig.yml> <markersize>"<<endl;
    cout<<" <video.avi>|live: specifies a input video file. Use 'live' to capture from camera"<<endl;
    cout<<" <camera.yml>: camera calibration file"<<endl;
    cout<<" <markermapconfig.yml>: board configuration file"<<endl;
    cout<<" <markersize>: in meters "<<endl;
}

 /**
  * 
  */
int main(int argc, char** argv)
{
  
      /// READ PARAMETERS
      if(!readParameters(argc, argv))
	return false;  

        std::cout << "parameters read!" << std::endl; 

      /// CREATE UNDISTORTED CAMERA PARAMS
      TheCameraParamsUnd=TheCameraParams;
      TheCameraParamsUnd.Distorsion=cv::Mat::zeros(4,1,CV_32F);      
      
      /// SET BOARD DETECTOR PARAMETERS
      //TheMarkerDetector.setParams(TheMarkerMap,CameraParamsUnd,TheMarkerSize);
      
      /// CAPTURE FIRST FRAME
      TheVideoCapturer.grab();
      TheVideoCapturer.retrieve ( TheInputImage );
      cv::undistort(TheInputImage,TheInputImageUnd,TheCameraParams.CameraMatrix,TheCameraParams.Distorsion);      
    
     
      /// INIT OGRE
      initOgreAR(TheCameraParamsUnd, TheInputImageUnd.ptr<uchar>(0));

        //prepare the detector
        string dict=TheMarkerMap.getDictionary();//see if the dictrionary is already indicated in the configuration file. It should!
        if(dict.empty()) dict="ARUCO";
        TheMarkerDetector.setDictionary(dict);///DO NOT FORGET THAT!!! Otherwise, the ARUCO dictionary will be used by default!
        if ( TheMarkerMap.isExpressedInPixels() && TheMarkerSize>0)
                    TheMarkerMap=TheMarkerMap.convertToMeters(TheMarkerSize);
        {
            cout<<"TheCameraParams.isValid()="<<TheCameraParams.isValid()<<" "<<TheMarkerMap.isExpressedInMeters()<<endl;
        }
        if (TheCameraParams.isValid() && TheMarkerMap.isExpressedInMeters() ) {
            TheMSPoseTracker.setParams(TheCameraParams, TheMarkerMap, TheMarkerSize);
}



        std::cout << "start of while loop" << std::endl;


      while (TheVideoCapturer.grab())
      {

        std::cout << "processing image" << std::endl;
	      /// READ AND UNDISTORT IMAGE
	      TheVideoCapturer.retrieve ( TheInputImage );
	      cv::undistort(TheInputImage,TheInputImageUnd,TheCameraParams.CameraMatrix,TheCameraParams.Distorsion);
	      
	      /// DETECT BOARD
	      std::vector<aruco::Marker> detected_markers = TheMarkerDetector.detect(TheInputImageUnd, TheCameraParams, TheMarkerSize);

          //for(auto idx:TheMarkerMap.getIndices(detected_markers))
          //       detected_markers[idx].draw(TheInputImageUnd, Scalar(0, 0, 255), 2);
            bool id10_detected = false;
            bool id11_detected = false;
            bool id12_detected = false;
            bool id13_detected = false;

            cv::Point2f id10_center, id11_center, id12_center, id13_center;

            for (auto const & idx : detected_markers) {
                if (idx.id == 10)
                {
                    id10_detected = true;
                    id10_center = idx.getCenter();
                }
                if (idx.id == 11)
                {
                    id11_detected = true;
                    id11_center = idx.getCenter();
                }
                if (idx.id == 12)
                {
                    id12_detected = true;
                    id12_center = idx.getCenter();
                }
                if (idx.id == 13)
                {
                    id13_detected = true;
                    id13_center = idx.getCenter();
                }
                idx.draw(TheInputImageUnd, Scalar(0,0,255), 2);

            }

            if (id10_detected && id11_detected) {
                double dx = (id10_center.x - id11_center.x);
                double dy = (id10_center.y - id11_center.y);
                double distance = sqrt(dx*dx + dy*dy); 
                
                sindbad_radius = distance/400.0 * 5.0f;
            }   
            if (id10_detected && id12_detected) {
                double dx = (id10_center.x - id12_center.x);
                double dy = (id10_center.y - id12_center.y);
                double distance = sqrt(dx*dx + dy*dy); 
                sindbad_speed = distance/400.0 * 1.0f;
            }   
            if (id13_detected) {
                sindbad_flip = -1.0f; 
            }  
            else
            {
                sindbad_flip = 1.0f;
            } 
	      bool pose_estimated = TheMSPoseTracker.estimatePose(detected_markers);
             std::cout << "sindbad_speed: " << sindbad_speed << std::endl;
            std::cout << "sindbad_flip: " << sindbad_flip << std::endl;
                std::cout << "sindbad_radius: " << sindbad_radius << std::endl;
  
        std::cout << "drawing background image" << std::endl;
	      /// UPDATE BACKGROUND IMAGE
	      mTexture->getBuffer()->blitFromMemory(mPixelBox);	      
	      
	      double position[3], orientation[4];

        std::cout << "drawing scene" << std::endl;
	      /// UPDATE SCENE
	      if ( pose_estimated ) {
            ogreNode->setVisible(true);
           
	      double deltaTime = 1.2*root->getTimer()->getMilliseconds()/1000.;

            sindbad_t += sindbad_flip * sindbad_speed * deltaTime;
            //std::cout << "time: " << sindbad_t << std::endl;
            //std::cout << "x_pos: " << cos(sindbad_t) << std::endl;
            //std::cout << "y_pos: " << sin(sindbad_t) << std::endl;
            ogreNodeChild->setPosition(sindbad_radius*cos(sindbad_t),sindbad_radius*sin(sindbad_t),0.0);
            cv::Mat Rvec = TheMSPoseTracker.getRvec(); 
            cv::Mat Tvec = TheMSPoseTracker.getTvec(); 

            // set node pose
          aruco::__OgreGetPoseParameters(position, orientation, Rvec, Tvec);
 

            std::cout << "position = (" << position[0] << "," << position[1] << "," << position[2] << ")" << std::endl;

            std::cout << "MarkerBoard Tvect = " << Tvec << std::endl;
            cv::imwrite("testimage.jpg", TheInputImage);

            std::cout << "wrote image" << std::endl;
           }
	      else ogreNode->setVisible(false);

    //TheMarkerDetector.getDetectedBoard().OgreGetPoseParameters(position, orientation);
	      ogreNode->setPosition( position[0], position[1], position[2]  );
	      ogreNode->setOrientation( orientation[0], orientation[1], orientation[2], orientation[3]  );

	      // Update animation
	      double deltaTime = 1.2*root->getTimer()->getMilliseconds()/1000.;
	      baseAnim->addTime(deltaTime);
	      topAnim->addTime(deltaTime);
	      root->getTimer()->reset();

	      
            /// RENDER FRAME
	      if(root->renderOneFrame() == false) break;
	      Ogre::WindowEventUtilities::messagePump();


	      /// KEYBOARD INPUT
	      //keyboard->capture();
	      //if (keyboard->isKeyDown(OIS::KC_ESCAPE)) break;	      
	      
	      
      }

        std::cout << "end while loop" << std::endl;

      //im->destroyInputObject(keyboard);
      //im->destroyInputSystem(im);
      //im = 0;

      delete root;
      return 0;
}
 

 /**
  * 
  */
bool readParameters(int argc, char** argv)
{
 
      if (argc<4) {
	usage();
	return false;
      }  
       std::cout << "Trying to open Video" << std::endl; 
      // read input video  
      if (true) TheVideoCapturer.open(1);
      else TheVideoCapturer.open(argv[1]);
      if (!TheVideoCapturer.isOpened())
      {
	  cerr<<"Could not open video"<<endl;
	  return false;
      }
        std::cout << "Finished opening Video" << std::endl;
    
      // read intrinsic file
      try {
	  TheCameraParams.readFromXMLFile(argv[2]);
        std::cout << "TheCameraParams read" << std::endl;
      } catch (std::exception &ex) {
	  cout<<ex.what()<<endl;
	  return false;
      }

      // read board file
      try {
	  TheMarkerMap.readFromFile(argv[3]);
        std::cout << "TheMarkerMap read" << std::endl;
      } catch (std::exception &ex) {
	  cout<<ex.what()<<endl;
	  return false;
      }            

      if(argc>4) TheMarkerSize=atof(argv[4]); 
      else TheMarkerSize=0.059;

    
        std::cout << "TheMarkerSize = " << TheMarkerSize  << std::endl;

    return true;
}

 
 
  /**
  * 
  */
int initOgreAR(aruco::CameraParameters camParams, unsigned char* buffer, std::string resourcePath)
{
 
	/// INIT OGRE FUNCTIONS
  	root = new Ogre::Root(resourcePath + "plugins.cfg", resourcePath + "ogre.cfg");
  	if (!root->showConfigDialog()) return -1;
	Ogre::SceneManager* smgr = root->createSceneManager(Ogre::ST_GENERIC);
	
	
	/// CREATE WINDOW, CAMERA AND VIEWPORT
        Ogre::RenderWindow* window = root->initialise(true);
	Ogre::Camera *camera;
	Ogre::SceneNode* cameraNode;
	camera = smgr->createCamera("camera");
	camera->setNearClipDistance(0.01f);
	camera->setFarClipDistance(10.0f);
	camera->setProjectionType(Ogre::PT_ORTHOGRAPHIC);
	camera->setPosition(0, 0, 0);
	camera->lookAt(0, 0, 1);
	double pMatrix[16];
	camParams.OgreGetProjectionMatrix(camParams.CamSize,camParams.CamSize, pMatrix, 0.05,10, false);
	Ogre::Matrix4 PM(pMatrix[0], pMatrix[1], pMatrix[2] , pMatrix[3],
			pMatrix[4], pMatrix[5], pMatrix[6] , pMatrix[7],
			pMatrix[8], pMatrix[9], pMatrix[10], pMatrix[11],
			pMatrix[12], pMatrix[13], pMatrix[14], pMatrix[15]);	
	camera->setCustomProjectionMatrix(true, PM);
	camera->setCustomViewMatrix(true, Ogre::Matrix4::IDENTITY);		
	window->addViewport(camera);
	cameraNode = smgr->getRootSceneNode()->createChildSceneNode("cameraNode");
	cameraNode->attachObject(camera);
		
	
	/// CREATE BACKGROUND FROM CAMERA IMAGE
	int width = camParams.CamSize.width;
	int height = camParams.CamSize.height;
	// create background camera image
	mPixelBox = Ogre::PixelBox(width, height, 1, Ogre::PF_R8G8B8, buffer);
	// Create Texture
	mTexture = Ogre::TextureManager::getSingleton().createManual("CameraTexture",Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
		      Ogre::TEX_TYPE_2D,width,height,0,Ogre::PF_R8G8B8,Ogre::TU_DYNAMIC_WRITE_ONLY_DISCARDABLE);	

	//Create Camera Material
	Ogre::MaterialPtr material = Ogre::MaterialManager::getSingleton().create("CameraMaterial", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
	Ogre::Technique *technique = material->createTechnique();
	technique->createPass();
	material->getTechnique(0)->getPass(0)->setLightingEnabled(false);
	material->getTechnique(0)->getPass(0)->setDepthWriteEnabled(false);
	material->getTechnique(0)->getPass(0)->createTextureUnitState("CameraTexture");	
	
    Ogre::Rectangle2D* rect = new Ogre::Rectangle2D(true);
	rect->setCorners(-1.0, 1.0, 1.0, -1.0);
	rect->setMaterial("CameraMaterial");
 
	// Render the background before everything else
	rect->setRenderQueueGroup(Ogre::RENDER_QUEUE_BACKGROUND);
 
	// Hacky, but we need to set the bounding box to something big, use infinite AAB to always stay visible
	Ogre::AxisAlignedBox aabInf;
	aabInf.setInfinite();
	rect->setBoundingBox(aabInf);
 
	// Attach background to the scene
	Ogre::SceneNode* node = smgr->getRootSceneNode()->createChildSceneNode("Background");
	node->attachObject(rect);		
	
	
	/// CREATE SIMPLE OGRE SCENE
	// add sinbad.mesh
	Ogre::ResourceGroupManager::getSingleton().addResourceLocation("Sinbad.zip", "Zip", "Popular");
 	Ogre::ResourceGroupManager::getSingleton().initialiseAllResourceGroups();	
	Ogre::Entity* ogreEntity = smgr->createEntity("Board", "Sinbad.mesh");
	ogreNode = smgr->getRootSceneNode()->createChildSceneNode();
	// add entity to a child node to correct position (this way, entity axis is on feet of sinbad)
	 ogreNodeChild= ogreNode->createChildSceneNode();
	ogreNodeChild->attachObject(ogreEntity);
	Ogre::Real offset = ogreEntity->getBoundingBox().getHalfSize().y;
    std::cout << "offset:" << offset << std::endl;
	// Sinbad is placed along Y axis, we need to rotate to put it along Z axis so it stands up over the board
	// first rotate along X axis, then add offset in Z dir so it is over the board and not in the middle of it 
	ogreNodeChild->rotate(Ogre::Vector3(1,0,0), Ogre::Radian(Ogre::Degree(-90)));	
	ogreNodeChild->translate(0,0,0,Ogre::Node::TS_PARENT); // board Z axis is pointing down, so we need to negate offset
	// mesh is too big, rescale!
	const float scale = 2.0f * 0.01675f;
	ogreNode->setScale(scale, scale, scale);
	  
    // Init animation
	ogreEntity->getSkeleton()->setBlendMode(Ogre::ANIMBLEND_CUMULATIVE);
	baseAnim = ogreEntity->getAnimationState("RunBase");
	topAnim = ogreEntity->getAnimationState("RunTop");
	baseAnim->setLoop(true);
	topAnim->setLoop(true);
	baseAnim->setEnabled(true);
	topAnim->setEnabled(true);	
	
	
 	/// KEYBOARD INPUT READING
 	size_t windowHnd = 0;
 	window->getCustomAttribute("WINDOW", &windowHnd);
// 	im = OIS::InputManager::createInputSystem(windowHnd);
// 	keyboard = static_cast<OIS::Keyboard*>(im->createInputObject(OIS::OISKeyboard, true));
    std::cout << "finished Setup of Ogre" << std::endl;	
    return 1;
}








