// Descripton:    Program calculates optical flow from pair of input images
//
// Synopsis:
//    exLMSOptic image1 image2 [-o op_file_prefix] [-i region_size]
//
// For each pixel, the optic flow is calculated over a square neighbourhood of
// size "region_size" (default = 9 pixels).  The optional PostScript output
// file plots the motion vectors and corresponding covariance matrix ellipses
// over the first image.  The ppm output file option plots the motion as a
// colour overlay.

#include "amma/Option.hh"
#include "amma/DP/FileFormatIO.hh"
#include "amma/Stream.hh"
#include "amma/NumImage.hh"
#include "amma/PSImage.hh"
#include "amma/Pair.hh"
#include "amma/Image/IPConvSym.hh"
#include "amma/Image/IPGaussConv.hh"
#include "amma/Image/IPGradient.hh"
#include "amma/Image/IPSpatDiff.hh"
#include "amma/Motion/LMSOpticFlow.hh"

#define savedouble(im, file) {StdIO::Save(file, im.ClipToFitByte());}

int main (int argc, char **argv) 
{
  // process command-line parameters
  OptionC opt(argc, argv);
  FilenameC ImageFileName1 (opt.String("", NULL, "Input image 1"));
  FilenameC ImageFileName2 (opt.String("", NULL, "Input image 2"));
  FilenameC OPFileName (opt.String("o", "motion", "Output file name prefix"));
  IntT windowSize (opt.Int("w", 9, "Window size"));
  IntT order (opt.Int("g", 5, "Set Gaussian antialias filter order (default is Remex 2:1 antialias)"));
  IntT diffOrder (opt.Int("d", 1, "Spatial gradient difference order"));
  RealT noise (opt.Real("n", 1.0, "Noise s.d. estimate for filtered image"));
  IntT scale (opt.Int("s", 1, "Scale factor for arrow on Postscript plot"));
  RealT subsample(opt.Real("S", 5, "Vector subsample factor"));
  BooleanT forward_diff (opt.Boolean("F", false, "Forward diff instead of average diff for spatial gradient"));
  BooleanT no_erosion (opt.Boolean("ne", false, "Minimal erosion of motion field"));
  opt.CompulsoryArgs(2);
  opt.Check();

  // load 2 images from sequence
  PairC<NumImageC<RealT> > image, filtered;
  StdIO::Load(ImageFileName1, image[0]);
  StdIO::Load(ImageFileName2, image[1]);

  // filter images with antialias filter
  IPConvSymC<RealT,RealT> filter;
  if (opt.IsOnCommandLine("g")) {
     filter = IPGaussConvolveC<RealT>(order, TwoD, (no_erosion) ? Original : Erode);
  }
  else {
    SArray1dC<RealT> coeffs(5);
    IStrStreamC ("5 0 0.308037 1 0.246545 2 0.114345 3 0.0105584 4 -0.0254667") >>coeffs;
    filter = IPConvSymC<RealT,RealT>(coeffs, TwoD, (no_erosion) ? Original : Erode);
  }
  for (UIntT i=0; i<=1; ++i) {
    filtered[i] = filter.Apply(image[i]);
  }
  // compute image gradients:
  //   - temporal
  NumImageC<RealT>  dt = (filtered[1]-filtered[0]); 
  if (!dt.Rectangle().IsValid()) {
    errAMMA << "temporal difference failed";
    errAMMA.Exit(-1);
  }
  //   - and spatial
  NumImageC<RealT> centre ((forward_diff) 
    ? filtered[0] : ((filtered[0] + filtered[1]) / 2.0));  
  NumImageC<Vector2dC> grad (IPGradientC(diffOrder).Apply(centre)); 
  // compute motion
  LMSOpticFlowC motion(true);
  motion.SetNoiseLevel(noise).SetRegionSize(windowSize)
    .SetFilterErode(!no_erosion).Estimate(grad, dt);
  motion.PrintPS(OPFileName+".ps", image[0], 300, subsample, scale);
  motion.PrintPpm(OPFileName+".ppm", image[0]);
  //cout << motion.Eigenvalues()<<'\n';
  OStreamC ("motion") << motion.Motion() << '\n';

}