#include <vil/vil_rgb.h>
#include <vil/vil_load.h>
#include <vil/vil_save.h>
#include <vil/vil_image_view.h>
#include <vil/vil_convert.h>
#include <vil/vil_image_view.h>
#include <vgl/vgl_point_2d.h>
#include <vgl/vgl_homg_point_2d.h>
#include <vpgl/vpgl_poly_radial_distortion.h>

//#include <vpgl/vpgl_poly_radial_distortion.txx>
//vpgl_POLY_RADIAL_DISTORTION_INSTANTIATE(unsigned int,5);

int main(void)
{
  vil_image_view<vil_rgb<vxl_byte> > img;
  img = vil_load("input_image.jpg");

  // Para crear imagen nueva del mismo tamano
  unsigned ni = img.ni();
  unsigned nj = img.nj();

  vil_image_view<vil_rgb<vxl_byte> > image(ni,nj);

  // Distortion Coefficients, Utilizando el MatLab Calibration ToolBox
  // Camara Panasonic, Lumix DMC-TZ5 de Richard
  vcl_vector<double> k(2);
  k[0] =  -0.0005;	//-0.07536;
  k[1] =  0.000001;	//0.08944;
  //k[2] = -0.00061;
  //k[3] = 0.00176;
  //k[4] = 0.00000;

  // Second parameter of vpgl_poly_radial_dist<unsigned, k.size()>
  vpgl_poly_radial_distortion<double, 2>
    dist(vgl_point_2d<double>(ni/2,nj/2), k);

  // Copy distorted points position to create fixed image.
  for (unsigned j = 0; j < img.nj(); ++j)
  {
    for (unsigned i = 0; i < img.ni(); ++i)
    {
      vgl_homg_point_2d<double> punto = vgl_homg_point_2d<double>(i, j);
      punto = dist.distort(punto);
      if( punto.x()<img.ni() && punto.y()<img.nj() && punto.x() >= 0 && punto.y() >= 0 )
      image(i,j) = img(punto.x(), punto.y());
    }
  }

  vil_save(image, "dist_image.jpg");


  img = vil_load("dist_image.jpg");
  // Copy undistorted points position to create fixed image.

  for (unsigned j = 0; j < img.nj(); ++j)

  {

    for (unsigned i = 0; i < img.ni(); ++i)

    {

      vgl_homg_point_2d<double> punto = vgl_homg_point_2d<double>(i, j);

      punto = dist.undistort(punto);
      if( punto.x()<img.ni() && punto.y()<img.nj()
		&& punto.x() >= 0 && punto.y() >= 0 )
	{
     		image(i,j) = img(punto.x(),punto.y());
	}

    }

  }



  vil_save(image, "undist_image.jpg");

  return 0;
}