#include "MbariVisualObjectDB.H"
#include "SIFT/Keypoint.H"
#include "SIFT/KDTree.H"
#include "Image/ColorOps.H"
#include "Image/Image.H"
#include "Image/Pixels.H"
#include "Util/Timer.H"

#include <iostream>
#include <string>
using namespace std;

#include <fstream>

// #######################################################################
MbariVisualObjectDB::MbariVisualObjectDB() :
  itsName(), itsObjects(), itsKDTree(), itsKDindices()
{ }

// #######################################################################
MbariVisualObjectDB::~MbariVisualObjectDB()
{ }

// ######################################################################
//
//  Modify by Francois Boudet
//
// ######################################################################
bool MbariVisualObjectDB::loadFrom(const std::string& fname)
{
  const char *fn = fname.c_str();

 // LINFO("Loading Visual Object database '%s'...", fn);

  std::ifstream inf(fn);
  if (inf.is_open() == false)
    { LERROR("Cannot open '%s' -- USING EMPTY", fn); return false; }
    
  createMbariVODB(inf, *this);
 
  inf.close();
 // LINFO("Done. Loaded %u VisualObjects.", numObjects());
  return true;
}

// ######################################################################
bool MbariVisualObjectDB::saveTo(const std::string& fname)
{
  const char *fn = fname.c_str();
 // LINFO("Saving database '%s'...", fn);

  std::string filepathstr = fname.substr(0, fname.find_last_of('/')+1);

  std::ofstream outf(fn);
  if (outf.is_open() == false)
    { LERROR("Cannot open %s for writing -- NOT SAVED", fn); return false; }

  outf<<(*this);
  outf.close();
  LINFO("Done. Saved %u VisualObjects.", numObjects());

  return true;
}

// #######################################################################
//
// Modify by Francois Boudet, comparaison is now on ImageFname
//
// #######################################################################
bool MbariVisualObjectDB::addObject(const rutz::shared_ptr<MbariVisualObject>& obj)
{
  std::string objectName = obj->getImageFname();

  std::vector< rutz::shared_ptr<MbariVisualObject> >::const_iterator
    vo = itsObjects.begin(), stop = itsObjects.end();

  while(vo != stop)
    {
      if ((*vo)->getImageFname().compare(objectName) == 0) return false;
      ++ vo;
    }

  // not found, add it to the end of our list:
  itsObjects.push_back(obj);

  // any KDTree we may have had is now invalid:
  itsKDTree.reset();
  itsKDindices.clear();

  return true;
}

// ######################################################################
// functor to assist with VisualObjectMatch sorting:
class moreVOM
{
public:
  moreVOM(const float kcoeff, const float acoeff) :
    itsKcoeff(kcoeff), itsAcoeff(acoeff)
  { }

  bool operator()(const rutz::shared_ptr<MbariVisualObjectMatch>& x,
                  const rutz::shared_ptr<MbariVisualObjectMatch>& y)
  { return ( x->getScore(itsKcoeff, itsAcoeff) >
             y->getScore(itsKcoeff, itsAcoeff) ); }

private:
  float itsKcoeff, itsAcoeff;
};

// ######################################################################
void MbariVisualObjectDB::buildKDTree()
{
  // if we have one, no-op:
  if (itsKDTree.is_valid()) return;

  LINFO("Building KDTree for %"ZU" objects...", itsObjects.size());

  // go over all our objects and build a giant vector of keypoints,
  // remembering the mapping between indices:
  itsKDindices.clear(); uint objidx = 0U;
  std::vector< rutz::shared_ptr<Keypoint> > allkps;

  std::vector< rutz::shared_ptr<MbariVisualObject> >::const_iterator
    obj = itsObjects.begin(), stop = itsObjects.end();

  while (obj != stop)
    {
      const std::vector< rutz::shared_ptr<Keypoint> >& kps = (*obj)->getKeypoints();
      uint kidx = 0U;
      std::vector< rutz::shared_ptr<Keypoint> >::const_iterator
        kp = kps.begin(), stopk = kps.end();

      while(kp != stopk)
        {
          // get the keypoint:
          allkps.push_back(*kp);

          // remember its object number and index within that object:
          itsKDindices.push_back(std::pair<uint, uint>(objidx, kidx));

          ++ kp; ++kidx;
        }
        ++ obj; ++objidx;
    }

  // all right, 'allkps' now has all our keypoints, and itsKDindices
  // their object number and indices within objects. Let's build a
  // KDTree from allkps:
  itsKDTree.reset(new KDTree(allkps));

  LINFO("Done. KDTree initialized with %"ZU" keypoints.", allkps.size());
}

// ######################################################################
uint MbariVisualObjectDB::
getObjectMatches(const rutz::shared_ptr<MbariVisualObject>& obj,
                 std::vector< rutz::shared_ptr<MbariVisualObjectMatch> >& matches,
                 const VisualObjectMatchAlgo algo, const uint maxn,
                 const float kcoeff, const float acoeff,
                 const float minscore, const uint mink,
                 const uint kthresh, const bool sortbypf)
{
 // LINFO("Matching '%s' against database...", obj->getName().c_str());
  Timer tim(1000000);
  matches.clear(); uint nm = 0U;

  switch(algo)
    {
      // ####################################################################
    case VOMA_SIMPLE:                    // #################### simple match
      {
        const uint nobj = itsObjects.size();

        std::vector<uint> sidx;
        if (sortbypf) computeSortedIndices(sidx, obj);

        for (uint i = 0; i < nobj; i ++)
          {
            // get the index:
            const uint index = sortbypf ? sidx[i] : i;

            // attempt a match:
            rutz::shared_ptr<MbariVisualObjectMatch>
              match(new MbariVisualObjectMatch(obj, itsObjects[index],
                                          algo, kthresh));

            // apply some standard pruning:
            match->prune(std::max(25U, mink * 5U), mink);

            // if the match is good enough, store it:
            if (match->size() >= mink &&
                match->getScore(kcoeff, acoeff) >= minscore &&
                match->checkSIFTaffine())
              {
                matches.push_back(match); ++nm;

                // have we found enough matches?
                if (nm >= maxn) break;
              }

            // otherwise, match runs out of scope here and its memory is
            // deallocated thanks to rutz::shared_ptr
          }
      }
      break;

      // ####################################################################
    case VOMA_KDTREE:                    // #################### KDTree match
    case VOMA_KDTREEBBF:
      {
        const uint nobj = itsObjects.size();
        const uint kthresh2 = kthresh * kthresh;

        // build a KDTree if we don't already have one:
        buildKDTree();

        // get matches between our KDTree and the test object:
        const uint tstnkp = obj->numKeypoints();
        if (tstnkp == 0U) break; // nothing to do
        const int maxdsq = obj->getKeypoint(0)->maxDistSquared();

        // prepare a list to get our keypoint matches: first index is
        // object number, second keypoint match number:
        std::vector< std::vector<KeypointMatch> > kpm(nobj);

        // the code here is similar to VisualObjectMatch::matchKDTree()
        // loop over all of our test object's keypoints:
        for (uint i = 0; i < tstnkp; i++)
          {
            int distsq1 = maxdsq, distsq2 = maxdsq;
            rutz::shared_ptr<Keypoint> tstkey = obj->getKeypoint(i);

            // find nearest neighbor in our KDTree:
            uint matchIndex = (algo == VOMA_KDTREEBBF) ?
              itsKDTree->nearestNeighborBBF(tstkey, 40, distsq1, distsq2) :
              itsKDTree->nearestNeighbor(tstkey, distsq1, distsq2);

            // Check that best distance less than 0.6 of second best distance:
            if (100U * distsq1 < kthresh2 * distsq2)
              {
                const uint refobjnum = itsKDindices[matchIndex].first;
                const uint refkpnum = itsKDindices[matchIndex].second;

                // note how we swap the ref/test here. This is for
                // compatibility with the Simple matching above. In
                // the list of object matches we will eventually
                // return, 'obj' is our ref object while an object in
                // the database is considered test object:
                KeypointMatch m;
                m.refkp = tstkey;
                m.tstkp = itsObjects[refobjnum]->getKeypoint(refkpnum);
                m.distSq = distsq1;
                m.distSq2 = distsq2;
                kpm[refobjnum].push_back(m);
              }
          }

        // let's see which objects accumulated enough keypoint matches:
        for (uint i = 0U; i < nobj; i ++)
          {
            // do we have nough keypoint matches for that object?
            if (kpm[i].size() >= mink)
              {
                // ok, let's make a pre-build VisualObjectMatch out of it:
                rutz::shared_ptr<MbariVisualObjectMatch>
                  match(new MbariVisualObjectMatch(obj, itsObjects[i], kpm[i]));

                // apply some standard pruning:
                match->prune(std::max(25U, mink * 5U), mink);

                // if the match is good enough, store it:
                if (match->size() >= mink &&
                    match->getScore(kcoeff, acoeff) >= minscore &&
                    match->checkSIFTaffine())
                  {
                    matches.push_back(match); ++nm;

                    // have we found enough matches?
                    if (nm >= maxn) break;
                  }
              }
          }
      }
      break;

      // ####################################################################
    default:
      LFATAL("Unknown matching algo %d", int(algo));
    }

  // finally, sort our matches:
  std::sort(matches.begin(), matches.end(), moreVOM(kcoeff, acoeff));

  uint64 t = tim.get();
 // LINFO("Found %u database object matches for '%s' in %.3fms",
 //       nm, obj->getName().c_str(), float(t) * 0.001F);

  return nm;
}


// #######################################################################
void MbariVisualObjectDB::computeSortedIndices(std::vector<uint>& indices,
                                          const rutz::shared_ptr<MbariVisualObject>&
                                          obj) const
{
  // delete any old indices:
  indices.clear();

  // get a list of pairs <featuredistsq, index>:
  std::vector< std::pair<double, uint> > lst;

  for(uint i = 0; i < itsObjects.size(); i++)
    lst.push_back(std::pair<double, uint>(itsObjects[i]->
                                          getFeatureDistSq(obj), i));

  // sort the list. NOTE: operator< on std::pair is lexicographic,
  // i.e., here, first by our first element (the distance), then by
  // our second element (the index, in the unlikely event we have two
  // equal distances):
  std::sort(lst.begin(), lst.end());

  // pull all the indices out:
  std::vector< std::pair<double, uint> >::const_iterator
    ptr = lst.begin(), stop = lst.end();
  while (ptr != stop)
    { indices.push_back(ptr->second); ++ptr; }
}

// #######################################################################
std::istream& operator>>(std::istream& is, MbariVisualObjectDB& vdb)
{
  vdb.createMbariVisualObjectDB(is, vdb);
  return is;
}


// ###############
void MbariVisualObjectDB::
createMbariVisualObjectDB(std::istream& is, MbariVisualObjectDB& vdb,
                     std::string filepath)
{
 
  std::string name;
  std::getline(is, name); 
  vdb.setName(name);
  uint siz; is>>siz;
  vdb.itsObjects.clear(); vdb.itsObjects.resize(siz);
  std::vector< rutz::shared_ptr<MbariVisualObject> >::iterator
  vo = vdb.itsObjects.begin(), stop = vdb.itsObjects.end();
  while (vo != stop)
    {
      rutz::shared_ptr<MbariVisualObject> newvo(new MbariVisualObject());
     /* LINFO (" coco l'asticot : %s \n", filepath.c_str()); // test francois
      filepath = "";
	  LINFO (" le sanglier de cornouialle : %s \n", filepath.c_str());*/
      if (filepath == "") {
        is>>(*newvo);
      }
      else {
        newvo->createMbariVisualObject(is, *newvo, filepath);
      }
      *vo++ = newvo;
    }
    
}


// #######################################################################
std::ostream& operator<<(std::ostream& os, const MbariVisualObjectDB& vdb)
{
  os<<vdb.getName()<<std::endl;
  os<<vdb.itsObjects.size()<<std::endl;

  std::vector< rutz::shared_ptr<MbariVisualObject> >::const_iterator
    vo = vdb.itsObjects.begin(), stop = vdb.itsObjects.end();

  while (vo != stop) { os<<(**vo); ++ vo; }

  return os;
}

// ######################################################################
//
// Creat by Francois Boudet
//
// ######################################################################

void MbariVisualObjectDB::
createMbariVODB(std::istream& is, MbariVisualObjectDB& vdb,
                     std::string filepath)
{
 
  std::string name;
  getline(is, name);
  vdb.setName(name);
  uint siz; is>>siz;
  vdb.itsObjects.clear(); vdb.itsObjects.resize(siz);

  std::vector< rutz::shared_ptr<MbariVisualObject> >::iterator
    vo = vdb.itsObjects.begin(), stop = vdb.itsObjects.end();
   
    

  while (vo != stop)
    {
      rutz::shared_ptr<MbariVisualObject> newvo(new MbariVisualObject());     
      newvo->createMbariVO(is, *newvo);  
      *vo++ = newvo;
    }
    
}

// ######################################################################
//
// Create by Francois Boudet
// This fonction add one images to the vector and also write it into the database
//
// ######################################################################
bool MbariVisualObjectDB::addOneImageDB(std::string classe, std::string fileName)
{
		
	std::string name(fileName);
	
    uint idx = name.rfind('.'); if (idx > 0) name = name.substr(0, idx);
    //LINFO("##### Processing object : %s",name.c_str());
        
	// get input image:
	Image< PixRGB<byte> > colim = Raster::ReadRGB(fileName);

    // create visual object and extract keypoints:      		
    rutz::shared_ptr<MbariVisualObject> vo(new MbariVisualObject(name, fileName, colim));
	vo->setName(classe);
    // add the object to the db:
    if (addObject(vo))
    {   
    	// if the object is added, we write it in the database     	
    	std::ofstream outfile;
    	const char *fn = getName().c_str();
    	outfile.open(fn, ios_base::in);
  		if (outfile.is_open())
  		{
  			// save the database name
    		outfile <<this->getName()<<std::endl;
    		// update the nomber of images 
  			outfile <<this->itsObjects.size()<<std::endl;
  			// go to the end of the database file  			
  			outfile.seekp(0, ios_base::end); 	
  			// save the MbariViualObject  			
    		outfile << (*vo);
    		// close the file
    		outfile.close();
  		}
		else {
			LERROR(" \n FAILED adding VisualObject '%s' to database -- IGNORING \n ",vo->getName().c_str() );
		}	
  		return true;
    }
    else
    {
    	LERROR("FAILED adding VisualObject '%s' to database -- IGNORING",vo->getName().c_str());
    	return false;
    }  	
  	
}
	
// ######################################################################
//
// Create by Francois Boudet
// This fonction add one images to the vector
//
// ######################################################################
bool MbariVisualObjectDB::addOneImage(std::string classe, std::string fileName)
{
		
	std::string name(fileName);
	
    uint idx = name.rfind('.'); if (idx > 0) name = name.substr(0, idx);
    //LINFO("##### Processing object : %s",name.c_str());
        
	// get input image:
	Image< PixRGB<byte> > colim = Raster::ReadRGB(fileName);

    // create visual object and extract keypoints:      		
    rutz::shared_ptr<MbariVisualObject> vo(new MbariVisualObject(name, fileName, colim));
	vo->setName(classe);
    // add the object to the db:
    if (addObject(vo))     	
  		return true;
    else
    {
    	LERROR("FAILED adding VisualObject '%s' to database -- IGNORING",vo->getImageFname().c_str());
    	return false;
    }  	
  	
}