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.utils.Converters;

import android.util.Log;

public class Marker {
	// c0------c3
	// | |
	// | |
	// c1------c2

	int marker_id;
	Point marker_corner0;
	Point marker_corner1;
	Point marker_corner2;
	Point marker_corner3;
	Mat Rvec;
	Mat Tvec;

	public Marker() {
		marker_id = -1;
		marker_corner0 = new Point(0, 0);
		marker_corner1 = new Point(0, 0);
		marker_corner2 = new Point(0, 0);
		marker_corner3 = new Point(0, 0);

		Rvec = new Mat(3, 1, CvType.CV_32FC1);
		Tvec = new Mat(3, 1, CvType.CV_32FC1);//old CV_32FC1
	}

	public Marker(int id, Point corner0, Point corner1, Point corner2, Point corner3) {
		marker_id = id;

		marker_corner0 = corner0;
		marker_corner1 = corner1;
		marker_corner2 = corner2;
		marker_corner3 = corner3;

		Rvec = new Mat(3, 1, CvType.CV_32FC1);
		Tvec = new Mat(3, 1, CvType.CV_32FC1);
	}

	public void drawToMat(Mat mat, Scalar color, int thickness) {
		Core.circle(mat, marker_corner0, 5, color, thickness);
		Core.circle(mat, marker_corner1, 5, color, thickness);
		Core.line(mat, marker_corner0, marker_corner1, color);
		Core.line(mat, marker_corner1, marker_corner2, color);
		Core.line(mat, marker_corner2, marker_corner3, color);
		Core.line(mat, marker_corner3, marker_corner0, color);

		Point text_point = new Point((marker_corner0.x + marker_corner2.x) / 2,
				(marker_corner0.y + marker_corner2.y) / 2);
		String text = "ID: " + marker_id;
		Core.putText(mat, text, text_point, 1, 2, color);
	}

	public void calculateExtrinsics(double size, Mat camMatrix, MatOfDouble distCoeff, boolean setYPerpendicular) {
		double halfsize = size / 2;

		List<Point3> points3 = new ArrayList<Point3>();
		points3.add(new Point3(-halfsize, -halfsize, 0));
		points3.add(new Point3(-halfsize, halfsize, 0));
		points3.add(new Point3(halfsize, halfsize, 0));
		points3.add(new Point3(halfsize, -halfsize, 0));

		MatOfPoint3f objectPoints = new MatOfPoint3f();
		objectPoints.fromList(points3);

		MatOfPoint2f imagePoints = new MatOfPoint2f(marker_corner0, marker_corner1, marker_corner2, marker_corner3);

		Calib3d.solvePnP(objectPoints, imagePoints, camMatrix, distCoeff, Rvec, Tvec);

		/*
		 * if(setYPerpendicular){ rotateXAxis(Rvec); }
		 */

		System.gc();
	}

	private void rotateXAxis(Mat rotation) {
		Mat R = new Mat(3, 3, CvType.CV_32F);
		Calib3d.Rodrigues(rotation, R);

		Mat RX = new Mat();
		Mat.eye(3, 3, CvType.CV_64FC1).copyTo(RX);

		double angleRad = 1 / 2;

		RX.put(1, 1, Math.cos(angleRad));
		RX.put(1, 2, -Math.sin(angleRad));
		RX.put(2, 1, Math.sin(angleRad));
		RX.put(2, 2, Math.cos(angleRad));

		Mat result = new Mat();
		Core.multiply(R, RX, result);

		Calib3d.Rodrigues(R, rotation);
	}
}
