#include <stdio.h>
#include "itkRigid2DTransform.h"
#include "itkImage.h"
#include "itkRGBPixel.h"
#include "itkImageFileReader.h"
#include "itkImageFileWriter.h"
#include "itkVectorResampleImageFilter.h"
#include "itkVectorLinearInterpolateImageFunction.h"

int main( int argc, char *argv[] )
{
	double x,y,z;
	
	if( argc < 12 )
    	{
		std::cerr << "Missing Parameters " << std::endl;
		std::cerr << "Usage: " << argv[0] << std::endl;
		std::cerr << " inputFixedImage " << std::endl;
		std::cerr << " inputMovingImage " << std::endl;
		std::cerr << " outputImage " << std::endl;
		std::cerr << " fixed_x1 " << std::endl;
		std::cerr << " fixed_y1 " << std::endl;
		std::cerr << " fixed_x2 " << std::endl;
		std::cerr << " fixed_y2 " << std::endl;
		std::cerr << " moving_x1 " << std::endl;
		std::cerr << " moving_y1 " << std::endl;
		std::cerr << " moving_x2 " << std::endl;
		std::cerr << " moving_y2 " << std::endl;
	}

	const unsigned int Dimension = 2;
	typedef   itk::RGBPixel< unsigned char >        RGBPixelType;
	
	typedef   itk::Image< RGBPixelType, Dimension > FixedImageType;
	typedef   itk::Image< RGBPixelType, Dimension > MovingImageType;
	
	typedef itk::Rigid2DTransform< double > TransformType;
	TransformType::Pointer transform = TransformType::New();

    	typedef TransformType::OutputPointType PointType;
	typedef TransformType::OutputVectorType VectorType;
       
	PointType fixed_point_1;
	PointType fixed_point_2;
	PointType fixed_centroid;
	PointType fixed_centroid_rotated;
	PointType moving_point_1;
	PointType moving_point_2;
	PointType moving_centroid;
	
	fixed_point_1[0] = atof(argv[4]);
	fixed_point_1[1] = atof(argv[5]);
	
	fixed_point_2[0] = atof(argv[6]);
	fixed_point_2[1] = atof(argv[7]);
	
	moving_point_1[0] = atof(argv[8]);
	moving_point_1[1] = atof(argv[9]);
	
	moving_point_2[0] = atof(argv[10]);
	moving_point_2[1] = atof(argv[11]);

	typedef itk::ImageFileReader< FixedImageType >  FixedReaderType;
	FixedReaderType::Pointer freader = FixedReaderType::New();
	freader->SetFileName( argv[1] );

	typedef itk::ImageFileReader< MovingImageType >  MovingReaderType;
	MovingReaderType::Pointer mreader = MovingReaderType::New();
	mreader->SetFileName( argv[2] );

	try
    	{
		freader->Update();
	}
      	catch( itk::ExceptionObject & excp )
    	{
	    	std::cerr << "Exception thrown " << std::endl;
	    	std::cerr << excp << std::endl;
    	}

	
	typedef itk::VectorResampleImageFilter<
		MovingImageType,MovingImageType> ResampleType;
      	ResampleType::Pointer resample = ResampleType::New();

	

	typedef itk::VectorLinearInterpolateImageFunction< 
                       MovingImageType, double >  InterpolatorType;
	InterpolatorType::Pointer interpolator = InterpolatorType::New();

	resample->SetInterpolator( interpolator );

	FixedImageType::Pointer fixedImage = freader->GetOutput();
	
	PointType center;
	center = fixedImage->GetOrigin();
	transform->SetIdentity();
	
	double fdx;
	double fdy;
	double fangle;
	double mdx;
	double mdy;
	double mangle;
	
	fdx = fixed_point_2[0] - fixed_point_1[0];
	fdy = fixed_point_2[1] - fixed_point_1[1];
	fangle = atan2(fdy, fdx);
	mdx = moving_point_2[0] - moving_point_1[0];
	mdy = moving_point_2[1] - moving_point_1[1];
	mangle = atan2(mdy, mdx);
	transform->SetAngle( mangle - fangle );

	fixed_centroid[0] = (fixed_point_2[0] + fixed_point_1[0]) / 2;
	fixed_centroid[1] = (fixed_point_2[1] + fixed_point_1[1]) / 2;
	moving_centroid[0] = (moving_point_2[0] + moving_point_1[0]) / 2;
	moving_centroid[1] = (moving_point_2[1] + moving_point_1[1]) / 2;

	//fixed_centroid_rotated = transform->TransformPoint(fixed_centroid);
	//VectorType translation = moving_centroid - fixed_centroid_rotated;
	transform->SetCenter( fixed_centroid );
	VectorType translation = moving_centroid - fixed_centroid;
	transform->SetTranslation( translation );

	resample->SetTransform( transform );
	
	resample->SetSize(    fixedImage->GetLargestPossibleRegion().GetSize() );
	resample->SetOutputOrigin(  fixedImage->GetOrigin() );
      	resample->SetOutputSpacing( fixedImage->GetSpacing() );
      	resample->SetDefaultPixelValue( 50 );
	
	typedef itk::ImageFileWriter< MovingImageType > WriterType;
	WriterType::Pointer writer = WriterType::New();
	
	writer->SetFileName( argv[3] );
	
	resample->SetInput(mreader->GetOutput());
	writer->SetInput(resample->GetOutput());

	try
    	{
		writer->Write();
	}
      	catch( itk::ExceptionObject & excp )
    	{
	    	std::cerr << "Exception thrown " << std::endl;
	    	std::cerr << excp << std::endl;
    	}

	return 0;
}