/******************************************************************************
 * Author:   Laurent Kneip                                                    *
 * Contact:  kneip.laurent@gmail.com                                          *
 * License:  Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved.      *
 *                                                                            *
 * Redistribution and use in source and binary forms, with or without         *
 * modification, are permitted provided that the following conditions         *
 * are met:                                                                   *
 * * Redistributions of source code must retain the above copyright           *
 *   notice, this list of conditions and the following disclaimer.            *
 * * 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.     *
 * * Neither the name of ANU nor the names of its contributors may be         *
 *   used to endorse or promote products derived from this software without   *
 *   specific prior written permission.                                       *
 *                                                                            *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 ANU OR THE 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.                                                               *
 ******************************************************************************/

#include "experiment_helpers.hpp"
#include "random_generators.hpp"
#include <opengv/math/cayley.hpp>
#include <iostream>
#include <iomanip>


void
opengv::generateCentralCameraSystem(
    translations_t & camOffsets,
    rotations_t & camRotations )
{
  //create a fake camera system for a central camera
  camOffsets.push_back(Eigen::Vector3d::Zero());
  camRotations.push_back(Eigen::Matrix3d::Identity());
}

void
opengv::generateRandomCameraSystem(
    int numberCameras,
    translations_t & camOffsets,
    rotations_t & camRotations )
{
  double offset = 0.5; //this is the distance from the viewpoint origin

  for( int i = 0; i < numberCameras; i++ )
  {
    translation_t camOffset = generateRandomDirectionTranslation(offset);
    rotation_t camRotation = generateRandomRotation();
    camOffsets.push_back(camOffset);
    camRotations.push_back(camRotation);
  }
}

void
opengv::extractRelativePose(
    const translation_t & position1,
    const translation_t & position2,
    const rotation_t & rotation1,
    const rotation_t & rotation2,
    translation_t & relativePosition,
    rotation_t & relativeRotation,
    bool normalize )
{
  relativeRotation = rotation1.transpose() * rotation2;
  relativePosition = rotation1.transpose() * (position2 - position1);
  if(normalize)
    relativePosition = relativePosition / relativePosition.norm();
}

void
opengv::printExperimentCharacteristics(
    const translation_t & position,
    const rotation_t & rotation,
    double noise,
    double outlierFraction )
{
  std::cout << "the random position is:" << std::endl;
  std::cout << position << std::endl << std::endl;
  std::cout << "the random rotation is:" << std::endl;
  std::cout << rotation << std::endl << std::endl;
  std::cout << "the noise in the data is:" << std::endl;
  std::cout << noise << std::endl;
  std::cout << "the outlier fraction is:" << std::endl;
  std::cout << outlierFraction << std::endl;
}

void
opengv::printBearingVectorArraysMatlab(
    const bearingVectors_t & bearingVectors1,
    const bearingVectors_t & bearingVectors2 )
{
  size_t numberBearingVectors = bearingVectors1.size();

  //temp: print the vectors in matlab format (for debugging purposes)
  size_t precision = 10;
  std::cout << "ov1 = [";
  for( size_t i = 0; i < numberBearingVectors; i++ )
    std::cout << std::setprecision(precision) << bearingVectors1[i](0,0) << " ";
  std::cout << ";" << std::endl;
  for( size_t i = 0; i < 8; i++ )
    std::cout << std::setprecision(precision) << bearingVectors1[i](1,0) << " ";
  std::cout << ";" << std::endl;
  for( size_t i = 0; i < 8; i++ )
    std::cout << std::setprecision(precision) << bearingVectors1[i](2,0) << " ";
  std::cout << "];" << std::endl;
  std::cout << "ov2 = [";
  for( size_t i = 0; i < 8; i++ )
    std::cout << std::setprecision(precision) << bearingVectors2[i](0,0) << " ";
  std::cout << ";" << std::endl;
  for( size_t i = 0; i < 8; i++ )
    std::cout << std::setprecision(precision) << bearingVectors2[i](1,0) << " ";
  std::cout << ";" << std::endl;
  for( size_t i = 0; i < 8; i++ )
    std::cout << std::setprecision(precision) << bearingVectors2[i](2,0) << " ";
  std::cout << "];" << std::endl;
}

void
opengv::printEssentialMatrix(
    const translation_t & position,
    const rotation_t & rotation)
{
  //E transforms vectors from vp 2 to 1: x_1^T * E * x_2 = 0
  //and E = (t)_skew*R
  Eigen::Matrix3d t_skew = Eigen::Matrix3d::Zero();
  t_skew(0,1) = -position[2];
  t_skew(0,2) = position[1];
  t_skew(1,0) = position[2];
  t_skew(1,2) = -position[0];
  t_skew(2,0) = -position[1];
  t_skew(2,1) = position[0];

  essential_t E = t_skew * rotation;
  double matrixNorm = 0.0;
  for(size_t r = 0; r < 3; r++)
  {
    for(size_t c = 0; c < 3; c++)
      matrixNorm += pow(E(r,c),2);
  }
  matrixNorm = sqrt(matrixNorm);
  for(size_t r = 0; r < 3; r++)
  {
    for(size_t c = 0; c < 3; c++)
      E(r,c) = E(r,c) / matrixNorm;
  }

  std::cout << "the random essential matrix is:" << std::endl;
  std::cout << E << std::endl;
}

void
opengv::getPerturbedPose(
    const translation_t & position,
    const rotation_t & rotation,
    translation_t & perturbedPosition,
    rotation_t & perturbedRotation,
    double amplitude )
{
  perturbedPosition = position;
  cayley_t cayley = math::rot2cayley(rotation);
  for( size_t i = 0; i < 3; i++ )
  {
    perturbedPosition[i] =
        perturbedPosition[i] +
        (((double) rand())/ ((double) RAND_MAX)-0.5)*2.0*amplitude;
    cayley[i] =
        cayley[i] + (((double) rand())/ ((double) RAND_MAX)-0.5)*2.0*amplitude;
  }
  perturbedRotation = math::cayley2rot(cayley);
}

std::vector<int>
opengv::getNindices( int n )
{
  std::vector<int> indices;
  for(int i = 0; i < n; i++)
    indices.push_back(i);
  return indices;
}





void
opengv::generateRandom2D3DCorrespondences(
    const translation_t & position,
    const rotation_t & rotation,
    const translations_t & camOffsets,
    const rotations_t & camRotations,
    size_t numberPoints,
    double noise,
    double outlierFraction,
    bearingVectors_t & bearingVectors,
    points_t & points,
    std::vector<int> & camCorrespondences,
    Eigen::MatrixXd & gt )
{
  //initialize point-cloud
  double minDepth = 4;
  double maxDepth = 8;

  for( size_t i = 0; i < (size_t) gt.cols(); i++ )
    gt.col(i) = generateRandomPoint( maxDepth, minDepth );

  //create the 2D3D-correspondences by looping through the cameras
  size_t numberCams = camOffsets.size();
  size_t camCorrespondence = 0;

  for( size_t i = 0; i < (size_t) gt.cols(); i++ )
  {
    //get the camera transformation
    translation_t camOffset = camOffsets[camCorrespondence];
    rotation_t camRotation = camRotations[camCorrespondence];

    //store the point
    points.push_back(gt.col(i));

    //project the point into the viewpoint frame
    point_t bodyPoint = rotation.transpose()*(gt.col(i) - position);

    //project the point into the camera frame
    bearingVectors.push_back(camRotation.transpose()*(bodyPoint - camOffset));

    //normalize the bearing-vector to 1
    bearingVectors[i] = bearingVectors[i] / bearingVectors[i].norm();

    //add noise
    if( noise > 0.0 )
      bearingVectors[i] = addNoise(noise,bearingVectors[i]);

    //push back the camera correspondence
    camCorrespondences.push_back(camCorrespondence++);
    if(camCorrespondence > (numberCams-1) )
      camCorrespondence = 0;
  }

  //add outliers
  //compute the number of outliers based on fraction
  size_t numberOutliers = (size_t) floor(outlierFraction*numberPoints);
  //make the first numberOutliers points be outliers
  for(size_t i = 0; i < numberOutliers; i++)
  {
    //extract the camera transformation
    translation_t camOffset = camOffsets[camCorrespondences[i]];
    rotation_t camRotation = camRotations[camCorrespondences[i]];

    //create random point
    point_t p = generateRandomPoint(8,4);

    //project into viewpoint frame
    point_t bodyPoint = rotation.transpose()*(p - position);

    //project into camera-frame and use as outlier measurement
    bearingVectors[i] = camRotation.transpose()*(bodyPoint - camOffset);

    //normalize the bearing vector
    bearingVectors[i] = bearingVectors[i] / bearingVectors[i].norm();
  }
}

void
opengv::generateMulti2D3DCorrespondences(
    const translation_t & position,
    const rotation_t & rotation,
    const translations_t & camOffsets,
    const rotations_t & camRotations,
    size_t pointsPerCam,
    double noise,
    double outlierFraction,
    std::vector<std::shared_ptr<points_t> > & multiPoints,
    std::vector<std::shared_ptr<bearingVectors_t> > & multiBearingVectors,
    std::vector<std::shared_ptr<Eigen::MatrixXd> > & gt )
{
  //initialize point-cloud
  double minDepth = 4;
  double maxDepth = 8;

  for( size_t cam = 0; cam < camOffsets.size(); cam++ )
  {
    std::shared_ptr<Eigen::MatrixXd> gt_sub(new Eigen::MatrixXd(3,pointsPerCam));
    for( size_t i = 0; i < pointsPerCam; i++ )
      gt_sub->col(i) = generateRandomPoint( maxDepth, minDepth );
    gt.push_back(gt_sub);
  }

  //iterate through the cameras (pairs)
  for( size_t cam = 0; cam < camOffsets.size(); cam++ )
  {
    //create the bearing-vector arrays for this camera
    std::shared_ptr<points_t> points(new points_t());
    std::shared_ptr<bearingVectors_t> bearingVectors(new bearingVectors_t());

    //get the offset and rotation of this camera
    translation_t camOffset = camOffsets[cam];
    rotation_t camRotation = camRotations[cam];

    //now iterate through the points of that camera
    for( size_t i = 0; i < (size_t) pointsPerCam; i++ )
    {
      points->push_back(gt[cam]->col(i));

      //project the point into the viewpoint frame
      point_t bodyPoint = rotation.transpose()*(gt[cam]->col(i) - position);

      //project that point into the camera
      bearingVectors->push_back( camRotation.transpose()*(bodyPoint - camOffset) );

      //normalize the vector
      (*bearingVectors)[i] = (*bearingVectors)[i] / (*bearingVectors)[i].norm();

      //add noise
      if( noise > 0.0 )
        (*bearingVectors)[i] = addNoise(noise,(*bearingVectors)[i]);
    }

    //push back the stuff for this camera
    multiPoints.push_back(points);
    multiBearingVectors.push_back(bearingVectors);
  }

  //add outliers
  size_t outliersPerCam = (size_t) floor(outlierFraction*pointsPerCam);

  //iterate through the cameras
  for(size_t cam = 0; cam < camOffsets.size(); cam++)
  {
    //get the offset and rotation of this camera
    translation_t camOffset = camOffsets[cam];
    rotation_t camRotation = camRotations[cam];

    //add outliers
    for(size_t i = 0; i < outliersPerCam; i++)
    {
      //generate a random point
      point_t p = generateRandomPoint(8,4);

      //transform that point into viewpoint 2 only
      point_t bodyPoint = rotation.transpose()*(p - position);

      //use as measurement (outlier)
      (*(multiBearingVectors[cam].get()))[i] =
          camRotation.transpose()*(bodyPoint - camOffset);

      //normalize
      (*(multiBearingVectors[cam].get()))[i] =
          (*(multiBearingVectors[cam].get()))[i] / (*(multiBearingVectors[cam].get()))[i].norm();
    }
  }
}

void
opengv::generateRandom2D2DCorrespondences(
    const translation_t & position1,
    const rotation_t & rotation1,
    const translation_t & position2,
    const rotation_t & rotation2,
    const translations_t & camOffsets,
    const rotations_t & camRotations,
    size_t numberPoints,
    double noise,
    double outlierFraction,
    bearingVectors_t & bearingVectors1,
    bearingVectors_t & bearingVectors2,
    std::vector<int> & camCorrespondences1,
    std::vector<int> & camCorrespondences2,
    Eigen::MatrixXd & gt )
{
  //initialize point-cloud
  double minDepth = 4;
  double maxDepth = 8;

  for( size_t i = 0; i < (size_t) gt.cols(); i++ )
    gt.col(i) = generateRandomPoint( maxDepth, minDepth );

  //create the 2D3D-correspondences by looping through the cameras
  size_t numberCams = camOffsets.size();
  size_t camCorrespondence = 0;

  for( size_t i = 0; i < (size_t) gt.cols(); i++ )
  {
    //get the camera transformation
    translation_t camOffset = camOffsets[camCorrespondence];
    rotation_t camRotation = camRotations[camCorrespondence];

    //get the point in viewpoint 1
    point_t bodyPoint1 = rotation1.transpose()*(gt.col(i) - position1);

    //get the point in viewpoint 2
    point_t bodyPoint2 = rotation2.transpose()*(gt.col(i) - position2);

    //get the point in the camera in viewpoint 1
    bearingVectors1.push_back(camRotation.transpose()*(bodyPoint1 - camOffset));

    //get the point in the camera in viewpoint 2
    bearingVectors2.push_back(camRotation.transpose()*(bodyPoint2 - camOffset));

    //normalize the bearing-vectors
    bearingVectors1[i] = bearingVectors1[i] / bearingVectors1[i].norm();
    bearingVectors2[i] = bearingVectors2[i] / bearingVectors2[i].norm();

    //add noise
    if( noise > 0.0 )
    {
      bearingVectors1[i] = addNoise(noise,bearingVectors1[i]);
      bearingVectors2[i] = addNoise(noise,bearingVectors2[i]);
    }

    //push back the camera correspondences
    camCorrespondences1.push_back(camCorrespondence);
    camCorrespondences2.push_back(camCorrespondence++);
    if(camCorrespondence > (numberCams - 1) )
      camCorrespondence = 0;
  }

  //add outliers
  size_t numberOutliers = (size_t) floor(outlierFraction*numberPoints);
  for(size_t i = 0; i < numberOutliers; i++)
  {
    //get the corresponding camera transformation
    translation_t camOffset = camOffsets[camCorrespondence];
    rotation_t camRotation = camRotations[camCorrespondence];

    //create random point
    point_t p = generateRandomPoint(8,4);

    //project this point into viewpoint 2
    point_t bodyPoint = rotation2.transpose()*(p - position2);

    //project this point into the corresponding camera in viewpoint 2
    //and use as outlier
    bearingVectors2[i] = camRotation.transpose()*(bodyPoint - camOffset);

    //normalize the bearing vector
    bearingVectors2[i] = bearingVectors2[i] / bearingVectors2[i].norm();
  }
}

void
opengv::generateRandom3D3DCorrespondences(
    const translation_t & position1,
    const rotation_t & rotation1,
    const translation_t & position2,
    const rotation_t & rotation2,
    size_t numberPoints,
    double noise,
    double outlierFraction,
    bearingVectors_t & points1,
    bearingVectors_t & points2,
    Eigen::MatrixXd & gt )
{
  //initialize point-cloud
  double minDepth = 4;
  double maxDepth = 8;

  for( size_t i = 0; i < (size_t) gt.cols(); i++ )
    gt.col(i) = generateRandomPoint( maxDepth, minDepth );

  //create the 3D-3D correspondences
  for( size_t i = 0; i < (size_t) gt.cols(); i++ )
  {
    //transform the points into the frames and store
    points1.push_back(rotation1.transpose()*(gt.col(i) - position1));
    points2.push_back(rotation2.transpose()*(gt.col(i) - position2));

    //add noise
    if( noise > 0.0 )
    {
      points1[i] = points1[i] + generateRandomTranslation(noise);
      points2[i] = points2[i] + generateRandomTranslation(noise);
    }
  }

  //add outliers
  size_t numberOutliers = (size_t) floor(outlierFraction*numberPoints);
  for(size_t i = 0; i < numberOutliers; i++)
  {
    //generate a random point
    point_t p = generateRandomPoint(8,4);

    //push-back in frame 2 only to create outlier
    points2[i] = rotation2.transpose()*(p - position2);
  }
}

void
opengv::generateMulti2D2DCorrespondences(
    const translation_t & position1,
    const rotation_t & rotation1,
    const translation_t & position2,
    const rotation_t & rotation2,
    const translations_t & camOffsets,
    const rotations_t & camRotations,
    size_t pointsPerCam,
    double noise,
    double outlierFraction,
    std::vector<std::shared_ptr<bearingVectors_t> > & multiBearingVectors1,
    std::vector<std::shared_ptr<bearingVectors_t> > & multiBearingVectors2,
    std::vector<std::shared_ptr<Eigen::MatrixXd> > & gt )
{
  //initialize point-cloud
  double minDepth = 4;
  double maxDepth = 8;

  for( size_t cam = 0; cam < camOffsets.size(); cam++ )
  {
    std::shared_ptr<Eigen::MatrixXd> gt_sub(new Eigen::MatrixXd(3,pointsPerCam));
    for( size_t i = 0; i < pointsPerCam; i++ )
      gt_sub->col(i) = generateRandomPoint( maxDepth, minDepth );
    gt.push_back(gt_sub);
  }

  //iterate through the cameras (pairs)
  for( size_t cam = 0; cam < camOffsets.size(); cam++ )
  {
    //create the bearing-vector arrays for this camera
    std::shared_ptr<bearingVectors_t> bearingVectors1(new bearingVectors_t());
    std::shared_ptr<bearingVectors_t> bearingVectors2(new bearingVectors_t());

    //get the offset and rotation of this camera
    translation_t camOffset = camOffsets[cam];
    rotation_t camRotation = camRotations[cam];

    //now iterate through the points of that camera
    for( size_t i = 0; i < (size_t) pointsPerCam; i++ )
    {
      //project a point into both viewpoint frames
      point_t bodyPoint1 = rotation1.transpose()*(gt[cam]->col(i) - position1);
      point_t bodyPoint2 = rotation2.transpose()*(gt[cam]->col(i) - position2);

      //project that point into the cameras
      bearingVectors1->push_back( camRotation.transpose()*(bodyPoint1 - camOffset) );
      bearingVectors2->push_back( camRotation.transpose()*(bodyPoint2 - camOffset) );

      //normalize the vectors
      (*bearingVectors1)[i] = (*bearingVectors1)[i] / (*bearingVectors1)[i].norm();
      (*bearingVectors2)[i] = (*bearingVectors2)[i] / (*bearingVectors2)[i].norm();

      //add noise
      if( noise > 0.0 )
      {
        (*bearingVectors1)[i] = addNoise(noise,(*bearingVectors1)[i]);
        (*bearingVectors2)[i] = addNoise(noise,(*bearingVectors2)[i]);
      }
    }

    //push back the stuff for this camera
    multiBearingVectors1.push_back(bearingVectors1);
    multiBearingVectors2.push_back(bearingVectors2);
  }

  //add outliers
  size_t outliersPerCam = (size_t) floor(outlierFraction*pointsPerCam);

  //iterate through the cameras
  for(size_t cam = 0; cam < camOffsets.size(); cam++)
  {
    //get the offset and rotation of this camera
    translation_t camOffset = camOffsets[cam];
    rotation_t camRotation = camRotations[cam];

    //add outliers
    for(size_t i = 0; i < outliersPerCam; i++)
    {
      //generate a random point
      point_t p = generateRandomPoint(8,4);

      //transform that point into viewpoint 2 only
      point_t bodyPoint = rotation2.transpose()*(p - position2);

      //use as measurement (outlier)
      (*multiBearingVectors2[cam])[i] =
          camRotation.transpose()*(bodyPoint - camOffset);

      //normalize
      (*multiBearingVectors2[cam])[i] =
          (*multiBearingVectors2[cam])[i] / (*multiBearingVectors2[cam])[i].norm();
    }
  }
}