package com.android.draw3d;

import java.util.ArrayList;
import java.util.List;

import org.opencv.calib3d.Calib3d;
import org.opencv.core.Core;
import org.opencv.core.CvType;
import org.opencv.core.Mat;
import org.opencv.core.MatOfDouble;
import org.opencv.core.MatOfPoint2f;
import org.opencv.core.MatOfPoint3f;
import org.opencv.core.Point;
import org.opencv.core.Point3;
import org.opencv.core.Scalar;
import org.opencv.imgproc.Imgproc;
import org.opencv.utils.Converters;

import android.util.Log;

public class DrawAboveMarker {
	Mat mCameraMatrix;
	MatOfDouble mDistortionCoefficients;
    
	public DrawAboveMarker(){
		double camera_matrix[] = {
				848.069f, 0.0f, 268.697f,
				0.0f, 847.687f, 264.266f,
				0.0f, 0.0f, 1.0f
		};
		double dist_coeff[] = {-0.445957f, 0.278828f, -0.002213f, -0.000656f};
		
		mCameraMatrix= new Mat(3, 3, CvType.CV_64FC1);
		//Core.setIdentity(mCameraMatrix);
		mCameraMatrix.put(0, 0, camera_matrix);
		
        mDistortionCoefficients = new MatOfDouble();
        Mat.zeros(4, 1, CvType.CV_64FC1).copyTo(mDistortionCoefficients);
        Mat d = new Mat();
        d.put(0, 0, dist_coeff);
        d.copyTo(mDistortionCoefficients);
	}
	
	public void drawCube(Mat mat, Marker marker, boolean setYperpendicular){
		MatOfPoint3f objectPoints = new MatOfPoint3f();
		double size = 1;
		double halfsize = size/2;
		
		if (setYperpendicular) {
			Point3 p0 = new Point3(-halfsize, 0, -halfsize);
			Point3 p1 = new Point3( halfsize, 0, -halfsize);
			Point3 p2 = new Point3( halfsize, 0,  halfsize);
			Point3 p3 = new Point3(-halfsize, 0,  halfsize);
			
			Point3 p4 = new Point3(-halfsize, size, -halfsize);
			Point3 p5 = new Point3( halfsize, size, -halfsize);
			Point3 p6 = new Point3( halfsize, size,  halfsize);
			Point3 p7 = new Point3(-halfsize, size,  halfsize);
			objectPoints.fromArray(p0,p1,p2,p3,p4,p5,p6,p7);
		} else{
			Point3 p0 = new Point3(-halfsize, -halfsize, 0);
			Point3 p1 = new Point3( halfsize, -halfsize, 0);
			Point3 p2 = new Point3( halfsize,  halfsize, 0);
			Point3 p3 = new Point3(-halfsize,  halfsize, 0);
			
			Point3 p4 = new Point3(-halfsize, -halfsize, size);
			Point3 p5 = new Point3( halfsize, -halfsize, size);
			Point3 p6 = new Point3( halfsize,  halfsize, size);
			Point3 p7 = new Point3(-halfsize,  halfsize, size);
			objectPoints.fromArray(p0,p1,p2,p3,p4,p5,p6,p7);
		}
		
		marker.calculateExtrinsics(size, mCameraMatrix, mDistortionCoefficients, setYperpendicular);
		
		MatOfPoint2f imagePointsMat = new MatOfPoint2f();
		Calib3d.projectPoints(objectPoints, marker.Rvec, marker.Tvec, mCameraMatrix, mDistortionCoefficients, imagePointsMat);
		List<Point> imagePoints = new ArrayList<Point>();
		Converters.Mat_to_vector_Point(imagePointsMat,imagePoints);
		
		Scalar color= new Scalar(255, 0, 0);
		for (int i = 0; i < 4; i++){
			Core.line(mat, imagePoints.get(i), imagePoints.get((i + 1) % 4), color, 2);
		}
	    for (int i = 0; i < 4; i++){
	    	Core.line(mat, imagePoints.get(i+4), imagePoints.get(4+(i + 1) % 4), color, 2);
	    }

	    for (int i = 0; i < 4; i++){
	    	Core.line(mat, imagePoints.get(i), imagePoints.get(i+4), color, 2);
	    }
	    
	    Scalar cc= new Scalar(255, 255, 0);
	    Core.circle(mat, imagePoints.get(4), 4, cc, 2);
	}
}

