/*
 * Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
 *
 * This file is part of RTAB-Map.
 *
 * RTAB-Map is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * RTAB-Map 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 General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with RTAB-Map.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <cppunit/config/SourcePrefix.h>
#include "Tests.h"

//Headers for the test BEGIN
#include "rtabmap/core/Camera.h"
#include "rtabmap/core/Signature.h"
#include "rtabmap/core/VWDictionary.h"
#include "rtabmap/core/EpipolarGeometry.h"
#include "rtabmap/core/Rtabmap.h"
#include "rtabmap/core/VerifyHypotheses.h"
#include "rtabmap/core/Parameters.h"
#include "rtabmap/core/BayesFilter.h"
#include "rtabmap/core/KeypointMemory.h"
#include "rtabmap/core/VisualWord.h"

#include "rtabmap/core/RtabmapEvent.h"

#include "rtabmap/core/DBDriverFactory.h"
#include "rtabmap/core/DBDriver.h"

//Util lib
#include "utilite/UtiLite.h"

//Database
#include <sqlite3.h>

#include <string.h>

//Headers for the test END

CPPUNIT_TEST_SUITE_REGISTRATION( Tests );

using namespace rtabmap;

void Tests::testAvpd()
{
	printf("\n");
	UTimer timer;
	timer.start();
	//Logger::setType(Logger::kTypeConsole);
	//Logger::setType(Logger::kTypeFile, "LogTestAvpdCore/testSurfStrategy.txt", false);
	//Logger::setLevel(Logger::kDebug);
	//Logger::setType(Logger::kTypeInvalid);

	std::string path = "./data/samples";

	CameraImages camera(path);
	CPPUNIT_ASSERT_MESSAGE("Camera initialization failed!\n", camera.init());

	/* Create tasks */
	Rtabmap rtabmap;
	ParametersMap parameters;
	parameters.insert(ParametersPair(Parameters::kRtabmapSMStateBufferSize(), "0"));
	parameters.insert(ParametersPair(Parameters::kDbSqlite3InMemory(), "true"));
	parameters.insert(ParametersPair(Parameters::kMemRawDataKept(), "true"));
	parameters.insert(ParametersPair(Parameters::kRtabmapPublishStats(), "true"));
	parameters.insert(ParametersPair(Parameters::kRtabmapTimeThr(), "0"));
	parameters.insert(ParametersPair(Parameters::kSURFHessianThreshold(), "500"));
	UDirectory::makeDir("./LogTestAvpdCore/");
	rtabmap.setWorkingDirectory("./LogTestAvpdCore/");
	rtabmap.init(parameters);


	/* Start thread's task */
	cv::Mat image;

	image = camera.takeImage();
	int imgCount = 0;
	while(!image.empty())
	{
		++imgCount;
		printf("Processing image %d/84...\n", imgCount);
		rtabmap.process(Sensor(image, Sensor::kTypeImage));
		image = camera.takeImage();
	}

	CPPUNIT_ASSERT(imgCount == 84);

	rtabmap.dumpData();

	ULogger::write("testSurfStrategy end...");
	//ULOGGER_INFO("\nTime = %fs", timer.ticks());



	//adjustLikelihood()
	std::map<int, float> likelihood;
	likelihood.clear();
	likelihood.insert(std::pair<int, float>(-1, 0));
	likelihood.insert(std::pair<int, float>(1, 0));
	likelihood.insert(std::pair<int, float>(2, 0));
	likelihood.insert(std::pair<int, float>(3, 0));
	rtabmap.adjustLikelihood(likelihood);
	CPPUNIT_ASSERT(likelihood.size() == 4);
	std::vector<float> values = uValues(likelihood);
	for(unsigned int i=0; i<values.size(); ++i)
	{
		CPPUNIT_ASSERT(values[i] == 1.0);
	}
	likelihood.clear();
	likelihood.insert(std::pair<int, float>(-1, 0.3));
	likelihood.insert(std::pair<int, float>(1, 0.4));
	likelihood.insert(std::pair<int, float>(2, 0.2));
	likelihood.insert(std::pair<int, float>(3, 0.9));
	rtabmap.adjustLikelihood(likelihood);
	CPPUNIT_ASSERT(likelihood.size() == 4);
	values = uValues(likelihood);
	// Result wanted generated by the TestAdjustLikelihood.m script (MatLab/Tests)
	int resultWanted2[4] = {1000,1000,1000,1309};
	for(unsigned int i=0; i<values.size(); ++i)
	{
		//ULOGGER_DEBUG("%d vs %d", (int)(values[i]*1000), resultWanted2[i]);
		CPPUNIT_ASSERT(int(values[i]*1000) == resultWanted2[i]);
	}
}

void Tests::testCamera()
{
	std::string path;
	cv::Mat image;
	int count;

	//CameraVideo class FIXME add a video in svn and reactivate this test
	/*path = "data/std_cam.avi";
	CameraVideo cameraVideo(path, false, 1);
	CPPUNIT_ASSERT( cameraVideo.init() );
	CPPUNIT_ASSERT( cameraVideo.isIdle() == true);
	image = cameraVideo.takeImage();
	count = 0;
	while(image)
	{
		cvReleaseImage(&image);
		image = 0;
		++count;
		if(count == 10)
		{
			break;
		}
		image = cameraVideo.takeImage();
	}
	if(image)
		cvReleaseImage(&image);
	CPPUNIT_ASSERT( count == 10 );*/

	//CameraImages class
	path = "data/samples";
	CameraImages cameraImages(path, 1, false, 0, false, 80);
	CPPUNIT_ASSERT( cameraImages.init() );
	CPPUNIT_ASSERT( cameraImages.isIdle() == true);
	image = cameraImages.takeImage();
	count = 0;
	while(!image.empty())
	{
		++count;
		image = cameraImages.takeImage();
	}
	CPPUNIT_ASSERT( count == 84 );
}

void Tests::testDBDriverFactory()
{
	ULogger::setType(ULogger::kTypeFile, "LogTestAvpdCore/testSqlite3Database.txt", false);
	DBDriver * dbDriver = 0;

	dbDriver = DBDriverFactory::createDBDriver("sqlite3");
	CPPUNIT_ASSERT( dbDriver );
	delete dbDriver;

	dbDriver = DBDriverFactory::createDBDriver("unknownDriver");
	CPPUNIT_ASSERT( dbDriver == 0 );
}

// TODO not finished
void Tests::testSqlite3Database()
{
	ParametersMap parameters;
	parameters.insert(ParametersPair(Parameters::kDbSqlite3InMemory(), "false"));
	DBDriver * driver = DBDriverFactory::createDBDriver("sqlite3", parameters);

	CPPUNIT_ASSERT(driver);
	CPPUNIT_ASSERT(driver->openConnection("LogTestAvpdCore/tmpDatabase.db"));
	delete driver;

	//== Test SMSignature save/load ==//
	UFile::erase("LogTestAvpdCore/tmpDatabase.db");
	parameters.clear();
	parameters.insert(ParametersPair(Parameters::kMemSignatureType(), "1"));
	driver = DBDriverFactory::createDBDriver("sqlite3", parameters);
	CPPUNIT_ASSERT(driver);
	CPPUNIT_ASSERT(driver->openConnection("LogTestAvpdCore/tmpDatabase.db"));
	std::list<std::vector<int> > sensors;
	std::list<std::vector<int> > sensors1;
	std::list<std::vector<int> > sensors2;
	CameraImages camera("./data/samples");
	CPPUNIT_ASSERT_MESSAGE("Camera initialization failed!\n", camera.init());

	cv::Mat image = camera.takeImage();
	CPPUNIT_ASSERT(!image.empty());
	std::list<Sensor> rawData;
	rawData.push_back(Sensor(image, Sensor::kTypeImage));
	SMSignature * sm1 = new SMSignature(sensors, 1, rawData);
	sensors1 = sm1->getData();
	sm1->addNeighbor(NeighborLink(2));

	image = camera.takeImage();
	CPPUNIT_ASSERT(!image.empty());
	rawData.clear();
	rawData.push_back(Sensor(image, Sensor::kTypeImage));
	SMSignature * sm2 = new SMSignature(sensors, 2, rawData);
	sensors2 = sm2->getData();
	sm2->addNeighbor(NeighborLink(1));

	//save them
	driver->asyncSave(sm1);
	sm1 = 0;
	driver->asyncSave(sm2);
	sm2 = 0;
	driver->emptyTrashes(false);

	//load them
	Signature * s1 = 0;
	Signature * s2 = 0;
	driver->getSignature(1, &s1);
	driver->getSignature(2, &s2);
	CPPUNIT_ASSERT(s1);
	CPPUNIT_ASSERT(s2);
	sm1 = dynamic_cast<SMSignature*>(s1);
	sm2 = dynamic_cast<SMSignature*>(s2);
	CPPUNIT_ASSERT(sm1);
	CPPUNIT_ASSERT(sm2);

	//compare
	CPPUNIT_ASSERT(sensors1.size() > 0 && sensors1.size() == sm1->getData().size());
	CPPUNIT_ASSERT(sensors2.size() > 0 && sensors2.size() == sm2->getData().size());
	CPPUNIT_ASSERT(sm1->getNeighbors().size() == 1);
	CPPUNIT_ASSERT(sm2->getNeighbors().size() == 1);

	delete sm1;
	delete sm2;
	delete driver;
}

// TODO add some tests to test when a signature is forgotten or reactivated
void Tests::testBayesFilter()
{
	BayesFilter bayes;

	//Parameters checks
	CPPUNIT_ASSERT( bayes.getVirtualPlacePrior() == Parameters::defaultBayesVirtualPlacePriorThr() );
	CPPUNIT_ASSERT( bayes.getPredictionLCStr().compare(Parameters::defaultBayesPredictionLC()) == 0 );

	ParametersMap parameters;
	parameters.insert(ParametersPair(Parameters::kBayesVirtualPlacePriorThr(), "0.01"));
	parameters.insert(ParametersPair(Parameters::kBayesPredictionLC(), "0.01 0.02 0.03 0.04"));
	bayes.parseParameters(parameters);
	CPPUNIT_ASSERT( uNumber2Str(bayes.getVirtualPlacePrior()).compare("0.01") == 0 );
	CPPUNIT_ASSERT( bayes.getPredictionLCStr().compare("0.01 0.02 0.03 0.04") == 0 );
	bayes.setVirtualPlacePrior(-1);
	CPPUNIT_ASSERT( bayes.getVirtualPlacePrior() == 0 );
	bayes.setVirtualPlacePrior(1.1);
	CPPUNIT_ASSERT( bayes.getVirtualPlacePrior() == 1 );
	bayes.setVirtualPlacePrior(0.6);
	CPPUNIT_ASSERT( uNumber2Str(bayes.getVirtualPlacePrior()).compare("0.6") == 0 );
	bayes.setPredictionLC("");
	CPPUNIT_ASSERT( bayes.getPredictionLCStr().compare("0.01 0.02 0.03 0.04") == 0 );
	bayes.setPredictionLC("0,01 0,02");
	CPPUNIT_ASSERT( bayes.getPredictionLCStr().compare("0.01 0.02 0.03 0.04") == 0 );
	bayes.setPredictionLC("0.01 0.02");
	CPPUNIT_ASSERT( bayes.getPredictionLCStr().compare("0.01 0.02") == 0 );
	bayes.setPredictionLC("0.01 0.02 0.03");
	CPPUNIT_ASSERT( bayes.getPredictionLCStr().compare("0.01 0.02 0.03") == 0 );

	//reset parameters...
	bayes = BayesFilter();

	//computePosterior()
	// Load memory with some data...
	KeypointMemory mem;

	//parameters
	mem.setCommonSignatureUsed(true);
	mem.setMaxStMemSize(1);
	bayes.setPredictionLC("0.1 0.24 0.18 0.1 0.04 0.01");
	bayes.setVirtualPlacePrior(0.9);

	std::map<int, float> likelihood;
	std::map<int, float> posterior;
	float sum;
	likelihood.insert(std::pair<int, float>(-1, 1));
	int result[100] = {0};
	int ri = 0;

	for(int i=1; i<11; ++i)
	{
		//ULOGGER_DEBUG("--- %d ---", i);
		std::map<std::string, float> memStats;
		std::list<Sensor> emptySensors;
		std::list<Actuator> emptyActuators;
		mem.update(emptySensors, emptyActuators, memStats);
		posterior = bayes.computePosterior(&mem, likelihood);
		likelihood.insert(std::pair<int, float>(i, 1));
		sum = uSum(uValues(posterior));
		CPPUNIT_ASSERT(sum > 1.0-0.0001 && sum < 1.0+0.0001);
		for(std::map<int, float>::const_iterator iter=posterior.begin(); iter!= posterior.end(); ++iter)
		{
			ULOGGER_DEBUG("%f", (*iter).second);
			result[ri++] = int((*iter).second*1000);
		}
		while((ri) % 10 != 0)
		{
			result[ri++] = 0;
		}
	}
	// Result wanted generated by the TestBayesFilter.m script (MatLab/Tests)
	int resultWanted1[100] = {1000,0,0,0,0,0,0,0,0,0,900,99,0,0,0,0,0,0,0,0,820,117,62,0,0,0,0,0,0,0,756,111,82,50,0,0,0,0,0,0,704,103,84,67,40,0,0,0,0,0,663,96,82,69,54,32,0,0,0,0,631,90,79,69,58,44,26,0,0,0,604,84,76,68,58,48,36,21,0,0,583,79,73,66,58,49,40,30,17,0,567,74,69,64,58,50,41,33,25,14};
	for(int i=0; i<100; ++i)
	{
		ULOGGER_DEBUG("%d vs %d", result[i], resultWanted1[i]);
		CPPUNIT_ASSERT(result[i] >= resultWanted1[i]-1 && result[i] <= resultWanted1[i]+1);
	}
}

void Tests::testKeypointMemory()
{
	//Util::Logger::setLevel(Logger::kInfo);
	//Util::Logger::setType(Logger::kTypeConsole);

	KeypointMemory mem;
	std::map<int, float> likelihood;

	ParametersMap parameters;
	parameters.insert(ParametersPair(Parameters::kDbSqlite3InMemory(), "false")); // It will be like read-only (database won't be changed in SVN)
	parameters.insert(ParametersPair(Parameters::kKpTfIdfLikelihoodUsed(), "true"));
	mem.setCommonSignatureUsed(true);
	mem.setMaxStMemSize(1);
	mem.init("sqlite3", "./data/samples.db", false, parameters);
	//ULOGGER_INFO("mem.getStMemIds().size() = %d, mem.getStMemIds().size()=%d", mem.getWorkingMemIds().size(), mem.getStMemIds().size());
	CPPUNIT_ASSERT( mem.getWorkingMem().size() + mem.getStMem().size() == 83 );

	//updateCommonSignature()
	const KeypointSignature * virtualPlace = dynamic_cast<const KeypointSignature *>(mem.getSignature(Memory::kIdVirtual));
	CPPUNIT_ASSERT(virtualPlace != 0);
	std::list<int> wordIds = uKeys(virtualPlace->getWords());
	//ULOGGER_INFO("wordIds.size()=%d", wordIds.size());
	// Result wanted generated by the TestUpdateCommonWords.m script (MatLab/Tests)
	int commonWordsWanted[163] = {5,8,9,21,22,23,27,30,32,34,36,40,45,53,61,62,64,67,68,77,85,86,97,98,99,100,103,106,122,124,125,127,129,131,143,157,161,164,168,169,172,175,181,182,187,196,197,208,210,217,218,219,220,234,235,237,244,252,286,307,308,310,315,327,328,346,347,350,355,362,372,373,387,393,394,398,404,412,423,428,429,441,442,456,465,470,476,495,496,498,506,520,558,572,584,585,587,596,620,634,637,643,646,647,658,667,668,681,709,721,726,741,766,777,785,790,791,793,808,809,880,896,906,908,919,933,940,954,958,974,981,1002,1023,1026,1058,1060,1071,1074,1097,1146,1152,1166,1215,1217,1229,1334,1341,1387,1478,1510,1539,1549,1566,1601,1624,1649,1693,1814,2046,2141,2424,2442,2674};
	int j=0;
	CPPUNIT_ASSERT(wordIds.size() == 163);
	for(std::list<int>::iterator i=wordIds.begin(); i!=wordIds.end(); ++i)
	{
		//ULOGGER_INFO("%d vs %d", *i, commonWordsWanted[j]);
		CPPUNIT_ASSERT(*i == commonWordsWanted[j]);
		++j;
	}

	//computeLikelihood()
	const KeypointSignature * lastSign = dynamic_cast<const KeypointSignature *>(mem.getLastSignature());
	CPPUNIT_ASSERT(lastSign != 0);
	wordIds = uKeys(lastSign->getWords());
	j=0;
	int signWordsRequired[136] = {9,24,27,37,39,40,45,46,64,64,67,67,68,68,69,69,79,79,81,85,87,95,109,113,115,117,122,124,127,135,135,139,142,143,145,157,159,161,168,181,188,188,191,192,196,197,208,208,211,213,219,239,241,247,256,257,258,307,310,343,558,587,643,760,896,960,1019,1023,1074,1197,1207,1292,1401,1403,1587,2151,2419,2571,2623,2654,2664,2674,2674,2674,2777,2780,2796,2808,2835,2844,2844,2851,2855,2862,2900,3036,3143,3238,3238,3315,3531,3531,3766,3803,4296,4405,4462,4502,4506,4509,4509,4516,4523,4533,4533,4534,4535,4536,4537,4538,4539,4540,4541,4542,4543,4544,4545,4546,4547,4548,4549,4550,4551,4552,4553,4554};
	CPPUNIT_ASSERT(wordIds.size() == 136);
	for(std::list<int>::iterator i=wordIds.begin(); i!=wordIds.end(); ++i)
	{
		//ULOGGER_INFO("%d vs %d", *i, signWordsRequired[j]);
		CPPUNIT_ASSERT(*i == signWordsRequired[j]);
		++j;
	}
	float maximumScore;
	likelihood = mem.computeLikelihood(lastSign, std::list<int>(mem.getWorkingMem().begin(), mem.getWorkingMem().end()), maximumScore);
	//ULOGGER_INFO("likelihood.size() = %d", likelihood.size());
	std::vector<float> values = uValues(likelihood);
	int likelihoodWanted[82] = {109,157,263,203,87,66,78,49,60,40,47,43,43,55,102,102,147,0,38,61,64,74,69,103,39,20,44,33,14,14,20,12,18,8,59,19,41,26,45,117,124,173,223,74,0,10,17,53,33,24,33,43,52,68,119,124,146,159,28,68,59,115,71,95,37,18,16,49,9,28,20,9,15,11,10,35,45,73,18,92,167,219};
	CPPUNIT_ASSERT(values.size() == 82);
	for(unsigned int i=0; i<values.size(); ++i)
	{
		ULOGGER_INFO("%d vs %d", int(values[i]*1000), likelihoodWanted[i]);
		CPPUNIT_ASSERT(int(values[i]*1000) == likelihoodWanted[i]);
	}
}

void Tests::testVWDictionary()
{
	VWDictionary dictionary;
	dictionary.setNndrUsed(false);
	std::list<cv::KeyPoint> keypoints;
	cv::Mat descriptors;
	unsigned int dim = 2;
	std::vector<float> v(dim);

	keypoints.push_back(cv::KeyPoint(cv::Point2f(1,1), 10, 30, 500, 2, 0));
	v[0] = 3;
	v[1] = 4;
	descriptors = cv::Mat(0, 2, CV_32F);
	descriptors.push_back(v);

	dictionary.addNewWords(descriptors, 1);
	CPPUNIT_ASSERT(dictionary.getVisualWords().size() == 1);

	//Create a word with the next descriptor (the distance^2 with the first word added = 2)
	v[0] = 4;
	v[1] = 3;
	VisualWord word(2, v.data(), dim, 0);
	std::list<VisualWord*> words;
	words.push_back(&word);
	std::vector<int> ids;

	//Naive
	dictionary.setNNStrategy(VWDictionary::kNNNaive);
	dictionary.setMinDistUsed(true);
	dictionary.setNndrUsed(false);
	dictionary.setMinDist(2.01f);
	ids = dictionary.findNN(words);
	CPPUNIT_ASSERT(ids.size() == 1);
	CPPUNIT_ASSERT(ids.front() == 1);

	dictionary.setMinDistUsed(true);
	dictionary.setNndrUsed(false);
	dictionary.setMinDist(1.99f);
	ids = dictionary.findNN(words);
	CPPUNIT_ASSERT(ids.size() == 1);
	CPPUNIT_ASSERT(ids.front() == 0);


	//Opencv kdtree
	dictionary.setNNStrategy(VWDictionary::kNNKdTree);
	dictionary.setMinDistUsed(true);
	dictionary.setNndrUsed(false);
	dictionary.setMinDist(2.01f);
	ids = dictionary.findNN(words);
	CPPUNIT_ASSERT(ids.size() == 1);
	CPPUNIT_ASSERT(ids.front() == 1);

	dictionary.setMinDistUsed(true);
	dictionary.setNndrUsed(false);
	dictionary.setMinDist(1.99f);
	ids = dictionary.findNN(words);
	CPPUNIT_ASSERT(ids.size() == 1);
	CPPUNIT_ASSERT(ids.front() == 0);


	//FLANN kdtree
	dictionary.setNNStrategy(VWDictionary::kNNFlannKdTree);
	dictionary.setMinDistUsed(true);
	dictionary.setNndrUsed(false);
	dictionary.setMinDist(2.01f);
	ids = dictionary.findNN(words);
	CPPUNIT_ASSERT(ids.size() == 1);
	CPPUNIT_ASSERT(ids.front() == 1);

	dictionary.setMinDistUsed(true);
	dictionary.setNndrUsed(false);
	dictionary.setMinDist(1.99f);
	ids = dictionary.findNN(words);
	CPPUNIT_ASSERT(ids.size() == 1);
	CPPUNIT_ASSERT(ids.front() == 0);
}

void Tests::testVerifyHypotheses()
{
	std::multimap<int, cv::KeyPoint> wordsA;
	std::multimap<int, cv::KeyPoint> wordsB;
	std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;

	wordsA.insert(std::pair<int, cv::KeyPoint>(1, cv::KeyPoint(0, 0, 1)));
	wordsA.insert(std::pair<int, cv::KeyPoint>(2, cv::KeyPoint(2, 2, 1)));
	wordsA.insert(std::pair<int, cv::KeyPoint>(3, cv::KeyPoint(3, 3, 1)));
	wordsA.insert(std::pair<int, cv::KeyPoint>(4, cv::KeyPoint(4, 4, 1)));
	wordsA.insert(std::pair<int, cv::KeyPoint>(6, cv::KeyPoint(5, 5, 1)));
	wordsA.insert(std::pair<int, cv::KeyPoint>(6, cv::KeyPoint(6, 6, 1)));

	wordsB.insert(std::pair<int, cv::KeyPoint>(1, cv::KeyPoint(0, 0, 1)));
	wordsB.insert(std::pair<int, cv::KeyPoint>(1, cv::KeyPoint(1, 1, 1)));
	wordsB.insert(std::pair<int, cv::KeyPoint>(2, cv::KeyPoint(2, 2, 1)));
	wordsB.insert(std::pair<int, cv::KeyPoint>(4, cv::KeyPoint(3, 3, 1)));
	wordsB.insert(std::pair<int, cv::KeyPoint>(5, cv::KeyPoint(4, 4, 1)));
	wordsB.insert(std::pair<int, cv::KeyPoint>(6, cv::KeyPoint(5, 5, 1)));
	wordsB.insert(std::pair<int, cv::KeyPoint>(6, cv::KeyPoint(6, 6, 1)));

	int total = findPairsAll(wordsA, wordsB, pairs);

	//printf("[%d,%d]\n", total, (int)pairs.size());
	CPPUNIT_ASSERT(total == 5);
	CPPUNIT_ASSERT(pairs.size() == 8);
}