/**                                                                       
 * @file                                                                  
 * $Revision$
 * $Date$
 * $Id$
 * 
 *   Unless noted otherwise, the portions of Isis written by the USGS are 
 *   public domain. See individual third-party library and package descriptions 
 *   for intellectual property information, user agreements, and related  
 *   information.                                                         
 *                                                                        
 *   Although Isis has been used by the USGS, no warranty, expressed or   
 *   implied, is made by the USGS as to the accuracy and functioning of such 
 *   software and related material nor shall the fact of distribution     
 *   constitute any such warranty, and no responsibility is assumed by the
 *   USGS in connection therewith.                                        
 *                                                                        
 *   For additional information, launch                                   
 *   $ISISROOT/doc//documents/Disclaimers/Disclaimers.html                
 *   in a browser or see the Privacy &amp; Disclaimers page on the Isis website,
 *   http://isis.astrogeology.usgs.gov, and the USGS privacy and disclaimers on
 *   http://www.usgs.gov/privacy.html.                                    
 */ 


#include <string>
#include <vector>
#include <numeric>
#include <iostream>
#include <sstream>

#include <QList>
#include <QStringList>
#include <QTime>

#include <boost/foreach.hpp>

#include "Angle.h"
#include "Application.h"
#include "ControlMeasure.h"
#include "ControlMeasureLogData.h"
#include "ControlPoint.h"
#include "IException.h"
#include "IString.h"
#include "Latitude.h"
#include "Longitude.h"
#include "MatcherSolution.h"
#include "MatchMaker.h"
#include "QDebugLogger.h"
#include "RobustMatcher.h"
#include "Statistics.h"

namespace Isis {

MatchMaker::MatchMaker() : QLogger(), m_name("MatchMaker"), 
                           m_parameters(), m_query(),  m_trainers(), 
                           m_geomFlag(None) { }

MatchMaker::MatchMaker(const QString &name,const PvlFlatMap &parameters,
                       const QLogger &logger) : QLogger(logger), 
                       m_name(name), m_parameters(parameters), 
                       m_query(), m_trainers(), m_geomFlag(None)  { }


QString MatchMaker::name() const {
  return ( m_name );
}

/** Returns the number of trainers to match */
int MatchMaker::size() const {
  return ( m_trainers.size() );
}

void MatchMaker::setQueryImage(const MatchImage &query) {
  m_query = query;
  return;
}

void MatchMaker::addTrainImage(const MatchImage &train) {
  m_trainers.push_back(train);
  return;
}


const MatchImage &MatchMaker::query() const {
  return ( m_query );
}

MatchImage &MatchMaker::query()  {
  return ( m_query );
}

const MatchImage &MatchMaker::train(const int &index) const {
  Q_ASSERT ( (index >= 0) && (index < size()) );
  return ( m_trainers[index] );
}

MatchImage &MatchMaker::train(const int &index) {
  Q_ASSERT ( (index >= 0) && (index < size()) );
  return ( m_trainers[index] );
}

void MatchMaker::setGeometrySourceFlag(const MatchMaker::GeometrySourceFlag &source) {
  if ( (Train == source) && ( size() > 1) )  {
    QString mess = "Cannot choose Train image as geometry source when matching "
                   "more than one train image to match";
    throw IException(IException::Programmer, mess, _FILEINFO_);
  }
  m_geomFlag = source;
}

MatchMaker::GeometrySourceFlag MatchMaker::getGeometrySourceFlag() const {
  return ( m_geomFlag );
}

MatchImage MatchMaker::getGeometrySource() const {
  if ( ( Train == m_geomFlag ) && ( size() > 1) )  {
    QString mess = "Cannot choose Train image as geometry source when matching "
                   "more than one train image to match";
    throw IException(IException::Programmer, mess, _FILEINFO_);
  }  

  if ( Query == m_geomFlag )  return ( query() );
  if ( Train == m_geomFlag )  return ( train() );
  return ( MatchImage() );  // Got none
}

MatcherSolution *MatchMaker::match(const SharedRobustMatcher &matcher) {

  // Pass along logging status
  matcher->setDebugLogger( stream(), isDebug() );

  MatcherSolution *m(0);
  if ( m_trainers.size() == 1 ) {
    // Run a pair only matcher
    m = new MatcherSolution(matcher,
                            matcher->match(m_query, m_trainers[0]), 
                            *this /* MatchMaker and/or QLogger */ ); 
  }
  else {
    // Run the multi-matcher
    m = new MatcherSolution(matcher,
                            matcher->match(m_query, m_trainers), 
                            *this /* MatchMaker and/or QLogger */ );
  }
  return ( m ); 
}

MatcherSolutionList MatchMaker::match(const RobustMatcherList &matchers) {
  // Thread here!!!!
  MatcherSolutionList solutions;
  for (int i = 0 ; i < matchers.size() ; i++) {
    solutions.push_back(SharedMatcherSolution( match(matchers[i]) )); 
  }
  return ( solutions );
}


 PvlGroup MatchMaker::network(ControlNet &cnet, const MatcherSolution &solution, 
                              ID &pointMaker) const {


   PvlGroup cnetinfo("ControlNetInfo");
  if ( isDebug() ) {
    logger() << "Entering MatchMaker::network(cnet, solution, pointmaker)...\n"; 
    logger().flush();
  }

  cnetinfo += PvlKeyword("SolutionSize", toString(solution.size()) );
  if ( solution.size() <= 0 ) { 
    cnetinfo += PvlKeyword("Error", "No matches, no network!!");
    return (cnetinfo); 
  }

  MatchImage v_query = (*solution.begin()).query();
  QVector<ControlPoint *> points(v_query.size(), 0);
  int nMeasures(0);
  int nImages(0);
  MatcherSolution::MatchPairConstIterator v_pair = solution.begin();
  for ( ; v_pair != solution.end() ; ++v_pair) {
    if ( v_pair->size() > 0 ) { nImages++; }
    for (int m = 0 ; m < v_pair->size() ; m++) {
      int index = v_pair->match(m).queryIdx;
      nMeasures += addMeasure(&points[index], *v_pair, 
                              (*v_pair).match(m), solution); 
    }
  }

  if ( isDebug() ) {
    logger().flush();
  }

  // Create control network
  int nPoints = 0;
  int nBad = 0;
  Statistics pointStats;
  for (int i = 0 ; i < points.size() ; i++) {
    if ( points[i] != 0 ) {
      if ( points[i]->GetNumValidMeasures() > 1 ) {
        pointStats.AddData((double)points[i]->GetNumValidMeasures()); 
        points[i]->SetId(pointMaker.Next());
        cnet.AddPoint(points[i]);
        points[i] = 0;
        nPoints++;
      }
      else {
        // Be sure to delete single measure points!!  This is as good as any
        // other place to do it.
        delete points[i];
        points[i] = 0;
        nBad++;
      }
    }
  }

  cnetinfo += PvlKeyword("ImagesMatched", toString(nImages) );
  cnetinfo += PvlKeyword("ControlPoints", toString(nPoints) );
  cnetinfo += PvlKeyword("ControlMeasures", toString(nMeasures) );
  cnetinfo += PvlKeyword("SingleMeasurePoints", toString(nBad) );
  if ( isDebug() ) {
    logger() << "  Images Matched:                 " << nImages << "\n";
    logger() << "  ControlPoints created:          " << nPoints << "\n";
    logger() << "  ControlMeasures created:        " << nMeasures << "\n";
    logger() << "  Excluded single measure points: " << nBad << "\n";
    logger().flush();
  }

  if ( pointStats.ValidPixels() > 0 ) {
    cnetinfo += PvlKeyword("MinimumMeasures", toString(pointStats.Minimum()) );
    cnetinfo += PvlKeyword("MaximumMeasures", toString(pointStats.Maximum()) ); 
    cnetinfo += PvlKeyword("StdDevMeasures", toString(pointStats.StandardDeviation()) );
    if ( isDebug() ) {
      logger() << "  MinimumMeasures:       " << pointStats.Minimum() << "\n";
      logger() << "  MaximumMeasures:       " << pointStats.Maximum() << "\n";
      logger() << "  StdDevMeasures:        " << pointStats.StandardDeviation() << "\n";
      logger().flush();
    }
  }

  return ( cnetinfo );
}


int MatchMaker::addMeasure(ControlPoint **cpt, const MatchPair &mpair, 
                           const cv::DMatch &point, 
                           const MatcherSolution &solution) const {

  int nMade(0);
  Q_ASSERT ( ( point.queryIdx >= 0) && 
             ( point.queryIdx < mpair.query().size() ) );
  Q_ASSERT ( ( point.trainIdx >= 0) && 
             ( point.trainIdx < mpair.train().size()) );

  // If no point created at this query keypoint, create a ControlPoint and add
  // the query keypoint as the reference
  if ( !(*cpt) ) {
    // std::cout << "\nMaking new ControlPoint...\n";
    *cpt = new ControlPoint(); 
    (*cpt)->SetDateTime(Application::DateTime());
    ControlMeasure *reference = makeMeasure(mpair.query(), point.queryIdx, 
                                            solution.matcher()->name());
    reference->SetType(ControlMeasure::Candidate);
    //std::cout << "  Image Coordinate: " << reference->GetSample() << ", " 
    //                                    << reference->GetLine() << "\n";

    (*cpt)->Add(reference);
    (*cpt)->SetRefMeasure(reference);

    // Set lat/lon if requested for Query image
    if ( Query == m_geomFlag ) {
      // What to do when it fails??
      (void) setAprioriLatLon(*(*cpt), *reference, mpair.query());
    }
    nMade++; 
  }

  // Add a measure to the existing control point std::cout << "Adding new
  // ControlMeasure...\n";
  QScopedPointer<ControlMeasure> trainpt(makeMeasure(mpair.train(), 
                                                     point.trainIdx, 
                                                     solution.matcher()->name() ) );

  trainpt->SetType(ControlMeasure::RegisteredSubPixel);

  const cv::KeyPoint &query = mpair.query().keypoint(point.queryIdx);
  const cv::KeyPoint &train = mpair.train().keypoint(point.trainIdx);

  // Compute the estimated point using the homography and the translation 
  // chain from matched image to source image coordintes
  cv::Point2f est = mpair.train().imageToSource(mpair.forward(query.pt));

  cv::Point2f tpoint( trainpt->GetSample(), trainpt->GetLine() );
  cv::Point2f diff = tpoint - est;

  // Compute the estimated residual. Wildly disparate matches will not be added
  // to the point so single points should be checked for and not added to the
  // network. Use 2 * homography tolerance as a limit unless the user has added
  // a ResidualTolerance parameter to the matcher.
  double residual = std::sqrt( diff.x*diff.x + diff.y*diff.y);
  double residTol = toDouble(solution.matcher()->parameters().get("HmgTolerance")) * 2.0;
  residTol = toDouble(solution.matcher()->parameters().get("ResidualTolerance",
                                                          toString(residTol)));

  // Don't add the measure to the point if it exceeds tolerance
  if ( residual <= residTol ) {
    trainpt->SetResidual(diff.x, diff.y);
    trainpt->SetLogData(ControlMeasureLogData(ControlMeasureLogData::GoodnessOfFit,
                                              goodnessOfFit(query, train)));

    // Set lat/lon if requested for train image
    if ( Train == m_geomFlag ) {
      // What to do when it fails??
      (void) setAprioriLatLon(*(*cpt), *trainpt, mpair.train() );
    }

    nMade++;
    (*cpt)->Add( trainpt.take() );
  }
  return (nMade);
}

ControlMeasure *MatchMaker::makeMeasure(const MatchImage &image, 
                                        const int &keyindex, 
                                        const QString &name) const { 
  QScopedPointer<ControlMeasure> v_measure(new ControlMeasure()); 
  v_measure->SetChooserName(name);
  v_measure->SetCubeSerialNumber(image.id());

  // imgpt is unused variable, so it has been commented out
  /*cv::Point2f imgpt = image.keypoint(keyindex).pt; */
  // std::cout << "ImageCoordinate: " << imgpt << "\n";

  cv::Point2f query = image.imageToSource(image.keypoint(keyindex).pt);
  // std::cout << "SourceCoordinate: " << query << "\n";
  v_measure->SetCoordinate(query.x, query.y, ControlMeasure::Candidate);
  return ( v_measure.take() );
}

bool MatchMaker::setAprioriLatLon(ControlPoint &point, 
                                  const ControlMeasure &measure, 
                                  const MatchImage &image) const {
  // Check if the source has geometry
  if ( !image.source().hasGeometry() ) { return (false); }

  // Compute lat/lon
  double samp = measure.GetSample(); 
  double line = measure.GetLine();
  SurfacePoint latlon = image.source().getLatLon(line,samp);

  // std::cout << "  Lat/Lon Coordinate: " << latlon.GetLatitude().degrees() << ", " 
  //           << latlon.GetLongitude().degrees() << "\n";
  point.SetAprioriSurfacePoint(latlon);  
  return ( latlon.Valid() );
}


double MatchMaker::getParameter(const QString &name, 
                                const PvlFlatMap &parameters, 
                                const double &defaultParm) const {
  double parm(defaultParm);
  if ( parameters.count(name) > 0 ) {
    QString value = parameters.get(name);
    if ( !value.isEmpty() ) {
      parm = toDouble(value);
    }
  }

  return ( parm );
}

double MatchMaker::goodnessOfFit(const cv::KeyPoint &query, 
                                 const cv::KeyPoint &train) const {
 return ( (query.response + train.response) / 2.0 );
}



}// namespace Isis
