import org.opencv.core.*;
import org.opencv.calib3d.*;
import org.opencv.highgui.*;
import org.opencv.imgproc.*;
import java.util.*;
import java.io.*;

class CameraCalibrationExample {

  static{ System.loadLibrary(Core.NATIVE_LIBRARY_NAME); }

  
  public static void main(String[] args) {
  
	/**
	 *  Input: photo's directory
	 **/ 
  
	String photosDir = "/run/media/gabor/972b117b-9b00-4f18-9bb8-719732e59098/gabor/oktatas/ipari_kepfeldolgozas/pattern_fotos/yyN/";
	
	String[] files = {photosDir+"yy1.jpg", 
					  photosDir+"yy2.jpg", 
					  photosDir+"yy3.jpg", 
					  photosDir+"yy4.jpg", 
					  photosDir+"yy5.jpg",	
					  photosDir+"yy6.jpg",  
					  photosDir+"yy7.jpg",
					  photosDir+"yy8.jpg",
					  photosDir+"yy9.jpg",
					  photosDir+"yy10.jpg",
					  photosDir+"yy11.jpg",
					  photosDir+"yy12.jpg",
					  photosDir+"yy13.jpg"
					  };
	/*
	String[] files = {photosDir+"xx1.jpg", 
					  photosDir+"xx2.jpg", 
					  photosDir+"xx3.jpg", 
					  photosDir+"xx4.jpg", 
					  photosDir+"xx5.jpg",	
					  photosDir+"xx6.jpg",  
					  photosDir+"xx7.jpg",
					  photosDir+"xx8.jpg"
					  };
	*/
	/*
	String[] files = {photosDir+"IMG452.jpg", 
					  photosDir+"IMG453.jpg", 
					  photosDir+"IMG454.jpg", 
					  photosDir+"IMG455.jpg", 
					  photosDir+"IMG456.jpg",	
					  photosDir+"IMG457.jpg",  
					  photosDir+"IMG458.jpg",
					  photosDir+"IMG460.jpg",
					  photosDir+"IMG461.jpg",
					  photosDir+"IMG462.jpg",
					  photosDir+"IMG463.jpg",
					  photosDir+"IMG465.jpg",
					  photosDir+"IMG466.jpg",
					  photosDir+"IMG467.jpg",
					  photosDir+"IMG468.jpg",
					  photosDir+"IMG469.jpg",
					  photosDir+"IMG472.jpg",
					  photosDir+"IMG473.jpg",
					  photosDir+"IMG475.jpg",
					  photosDir+"IMG476.jpg",
					  photosDir+"IMG480.jpg"
					  };
	*/
	/*
	String photosDir = "/run/media/gabor/972b117b-9b00-4f18-9bb8-719732e59098/gabor/oktatas/ipari_kepfeldolgozas/pattern_fotos/";
	
    String[] files = {photosDir+"IMG332.jpg", 
					  photosDir+"IMG333.jpg", 
					  photosDir+"IMG334.jpg", 
					  photosDir+"IMG335.jpg",	
					  photosDir+"IMG336.jpg",  
					  photosDir+"IMG337.jpg",
					  photosDir+"IMG338.jpg",
					  photosDir+"IMG339.jpg",
					  photosDir+"IMG342.jpg",
					  photosDir+"IMG343.jpg",
					  photosDir+"IMG344.jpg",};
    */
    MatOfPoint3f obj = new MatOfPoint3f();
    
    MatOfPoint2f corners = new MatOfPoint2f();
    List<Mat> allCorners = new ArrayList<Mat>();
    List<Mat> objectPoints = new ArrayList<Mat>();
    
    Size patternSize = new Size(8,6);
    
    List<Mat> rvecs = new ArrayList<Mat>();
    List<Mat> tvecs = new ArrayList<Mat>();
    List<Mat> imgList = new ArrayList<Mat>();
    Mat cameraMatrix = new Mat();
    Mat distCoefficient = new Mat();
    double squareSize = 32;
    int numSquares = (int)((patternSize.width) * (patternSize.height));
	for (int i = 0; i < patternSize.height; i++) {
		for (int j = 0; j < patternSize.width; j++) {
			obj.push_back(new MatOfPoint3f(new Point3(j*squareSize, i*squareSize, 0.0f )));
		}
	}
    
    Size imageSize = new Size(1,1);
    
    for (int i =0; i < files.length; i++) {
		System.out.println(files[i]);
		
		Mat img = Highgui.imread(files[i], Highgui.CV_LOAD_IMAGE_GRAYSCALE);
		imageSize= img.size();
		corners = new MatOfPoint2f();
		System.out.println("image size: " + img.size());
		if (Calib3d.findChessboardCorners(img, patternSize, corners, Calib3d.CALIB_CB_ADAPTIVE_THRESH + Calib3d.CALIB_CB_NORMALIZE_IMAGE + Calib3d.CALIB_CB_FAST_CHECK ) ) {
			//System.out.println("corner points: "+corners.dump());
			
			
			// optimization
			TermCriteria term = new TermCriteria(TermCriteria.EPS | TermCriteria.MAX_ITER, 30, 0.1);
			Imgproc.cornerSubPix(img, corners, new Size(11, 11), new Size(-1, -1), term);
			// 11x11 is the window size, size(-1,-1) is the zero zone
			
			allCorners.add(corners);
			objectPoints.add(obj);
			imgList.add(img);
			Mat frame = new Mat();
			frame = img.clone();
			Imgproc.cvtColor(frame, frame, Imgproc.COLOR_GRAY2BGR);
			Calib3d.drawChessboardCorners(frame, patternSize, corners, true);
			String filenameOnly = (new java.io.File(files[i]).getName());
			System.out.println(filenameOnly);
			Highgui.imwrite("foundcorners_"+filenameOnly, frame);
			
			
	
		}
		// Mat img1 = Highgui.imread();
	}
	
	System.out.println("Good pictures to calibration: " + allCorners.size());
	System.out.println("ImageList length: " + imgList.size());
	System.out.println("ObjectPoints length: " + objectPoints.size());
	
	//cameraMatrix.put(0,0,1);
	//cameraMatrix.put(1,1,1);
    cameraMatrix = Mat.eye(3,3, CvType.CV_64F);
    distCoefficient = Mat.zeros(8,1, CvType.CV_64F);
    double rms = Calib3d.calibrateCamera(objectPoints, allCorners, imageSize, cameraMatrix, 
							distCoefficient, rvecs, tvecs, //);
							Calib3d.CALIB_FIX_FOCAL_LENGTH | Calib3d.CALIB_FIX_PRINCIPAL_POINT |Calib3d.CALIB_ZERO_TANGENT_DIST | Calib3d.CALIB_FIX_ASPECT_RATIO); 
							//Calib3d.CALIB_USE_INTRINSIC_GUESS | Calib3d.CALIB_FIX_FOCAL_LENGTH|Calib3d.CALIB_FIX_K4|Calib3d.CALIB_FIX_K5);
				
    System.out.println(cameraMatrix.dump()); // camera matrix
    System.out.println("calibration rms : " + rms);
    
    Mat map1 = new Mat();
    Mat map2 = new Mat();
    Mat r = new Mat();
    Rect rect = new Rect();
    
    Imgproc.initUndistortRectifyMap(cameraMatrix, distCoefficient, r,
									Calib3d.getOptimalNewCameraMatrix(cameraMatrix, distCoefficient, imageSize, 1.0, imageSize, rect, false),
									imageSize, CvType.CV_16SC2, map1, map2);
	
	int totalPoints = 0;
	double totalError = 0.0, err;
	MatOfPoint2f imagepoints2 = new MatOfPoint2f();
	// compute reprojection error
	double avgError = 0.0;
	
	
	
	for (int i=0; i<allCorners.size(); i++) {
		Mat undistored = new Mat();
		//Mat img = Highgui.imread(files[i], Highgui.CV_LOAD_IMAGE_GRAYSCALE);
		Mat img = imgList.get(i);
		imagepoints2 = new MatOfPoint2f();
		
		// undistort images 
		Imgproc.remap(img, undistored, map1, map2, Imgproc.INTER_LINEAR);
		
		Imgproc.undistort(img, undistored, cameraMatrix, distCoefficient);
		
		
		//Highgui.imwrite("undistored_"+i+".jpg", undistored);
		
		MatOfPoint2f corners2 = new MatOfPoint2f();
		
		boolean foundPattern = Calib3d.findChessboardCorners(undistored, patternSize, corners2, Calib3d.CALIB_CB_ADAPTIVE_THRESH + Calib3d.CALIB_CB_NORMALIZE_IMAGE + Calib3d.CALIB_CB_FAST_CHECK );
		Calib3d.projectPoints((MatOfPoint3f) (objectPoints.get(i)), rvecs.get(i), tvecs.get(i), cameraMatrix, new MatOfDouble(distCoefficient), imagepoints2);
		err = Core.norm(allCorners.get(i), imagepoints2, Core.NORM_L2);
		
		/* // just for debugging
		System.out.println("imagepoints2: " + imagepoints2.dump());
		System.out.println("allCorners(i): " + allCorners.get(i).dump());
		System.out.println("rvecs(i):" + rvecs.get(i).dump());
		System.out.println("tvecs(i):" + tvecs.get(i).dump());
		*/
		
		int n = numSquares; // objectPoints.get(j).size();
		totalError += err*err;
		totalPoints += n; 
		avgError += err / n;
		
		Mat frame = new Mat();
		frame = undistored.clone();
		MatOfPoint2f undistored_points2 = new MatOfPoint2f();
		Imgproc.undistortPoints((MatOfPoint2f) (allCorners.get(i)), undistored_points2, cameraMatrix, distCoefficient);
		Imgproc.cvtColor(frame, frame, Imgproc.COLOR_GRAY2BGR);
		Calib3d.drawChessboardCorners(frame, patternSize, imagepoints2, true);
		Highgui.imwrite("projected_"+i+".jpg", frame);
			 
			

		
					
	}
	avgError = avgError / allCorners.size();
	System.out.println("Average error per images: " + avgError);
	System.out.println("Arithmetical error: " + Math.sqrt(totalError / totalPoints));
	
	try {
		
		// writing YAML
		FileOutputStream stream = new FileOutputStream("LG660.yml");
		OutputStreamWriter outputStreamWriter = new OutputStreamWriter(stream);
		outputStreamWriter.write("%YAML:1.0\n");
		outputStreamWriter.write("cameraMatrix: !!opencv-matrix\n");
		outputStreamWriter.write("\trows: 3\n" );
		outputStreamWriter.write("\tcols: 3\n" );
		outputStreamWriter.write("\tdt: d\n");
		outputStreamWriter.write("\tdata: " + cameraMatrix.dump());
		
		outputStreamWriter.write("distCoeffs: !!opencv-matrix\n");
		outputStreamWriter.write("\trows: 5\n" );
		outputStreamWriter.write("\tcols: 1\n" );
		outputStreamWriter.write("\tdt: d\n");
		outputStreamWriter.write("\tdata: " + distCoefficient.dump());
		
		outputStreamWriter.close();
		stream.close();
		
		// writing XML
		
		stream = new FileOutputStream("LG660.xml");
		outputStreamWriter = new OutputStreamWriter(stream);
		outputStreamWriter.write("<?xml version=\"1.0\"?>\n");
		outputStreamWriter.write("<opencv_storage>\n");
		outputStreamWriter.write("<camera_matrix type=\"opencv-matrix\">\n");
		outputStreamWriter.write("\t<rows>3</rows>\n" );
		outputStreamWriter.write("\t<cols>3</cols>\n" );
		outputStreamWriter.write("\t<dt>d</dt>\n");
		outputStreamWriter.write("\t<data>" + cameraMatrix.dump()+"</data>\n");
		outputStreamWriter.write("</camera_matrix>\n");
		
		outputStreamWriter.write("<distCoeffs type=\"opencv-matrix\">\n");
		outputStreamWriter.write("\t<rows>5</rows>\n" );
		outputStreamWriter.write("\t<cols>1</cols>\n" );
		outputStreamWriter.write("\t<dt>d</dt>\n");
		outputStreamWriter.write("\t<data>" + distCoefficient.dump()+"</data>\n");
		outputStreamWriter.write("</distCoeffs>\n");
		outputStreamWriter.write("</opencv_storage>\n");
		
		outputStreamWriter.close();
		stream.close();
		
		
		
	
	/*
		FileOutputStream stream = new FileOutputStream("FlannParam.yml");
		OutputStreamWriter outputStreamWriter = new OutputStreamWriter(stream);
		outputStreamWriter.write("%YAML:1.0);\nhessianThreshold:"+ _minHessian + "\n");
		outputStreamWriter.close();
		stream.close();
	*/ 
	} catch (IOException ex) {
		ex.printStackTrace();
	}
	
	
	// gettin real camera parameters:
	System.out.println("Camera parameters:");
	double apertureWidth=10.0;
	double apertureHeight = 10.0;
	double[] fovx = new double[3];
	double[] fovy = new double[3];
	double[] focalLength = new double[3];
	Point principalPoint = new Point();
	double[] aspectRatio = new double[3];
	
	Calib3d.calibrationMatrixValues(cameraMatrix, imageSize, apertureWidth, apertureHeight, fovx, fovy, focalLength, principalPoint, aspectRatio);
	System.out.println("aperture width: " + apertureWidth);
	System.out.println("aperture height: " + apertureHeight);
	System.out.println("fovx: " + fovx);
	System.out.println("fovy: " + fovy);
	System.out.println("focal length: " + focalLength);
	System.out.println("principal point: " + principalPoint);
	System.out.println("aspect ratio: " + aspectRatio);
	
	

  }


}