package com.android.draw3d;

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

import org.opencv.core.Core;
import org.opencv.core.CvType;
import org.opencv.core.Mat;
import org.opencv.core.MatOfPoint;
import org.opencv.core.MatOfPoint2f;
import org.opencv.core.Point;
import org.opencv.core.Rect;
import org.opencv.core.Size;
import org.opencv.core.TermCriteria;
import org.opencv.imgproc.Imgproc;
import org.opencv.utils.Converters;

import android.util.Log;

public class MarkerRecognizer {
	
	double APPROX_POLY_EPS = 0.08;
	int MARKER_CELL_SIZE = 10;
	int MARKER_SIZE = 70;// 7*MARKER_CELL_SIZE
	
	public MarkerRecognizer(){
		
	}
	
	public void MarkerDetect(Mat matGray, List<Marker> markers, int min_size, int min_side_length){
		Mat matBin = new Mat();
		
		int thresh_size = (min_size/4)*2+1;
		
		Imgproc.adaptiveThreshold(matGray, matBin, 255, Imgproc.ADAPTIVE_THRESH_GAUSSIAN_C, Imgproc.THRESH_BINARY_INV, thresh_size, thresh_size/3);
		Imgproc.morphologyEx(matBin, matBin, Imgproc.MORPH_OPEN, new Mat());
		List<MatOfPoint> all_contours = new ArrayList<MatOfPoint>();
		List<MatOfPoint> contours = new ArrayList<MatOfPoint>();
		Imgproc.findContours(matBin, all_contours, new Mat(), Imgproc.RETR_LIST, Imgproc.CHAIN_APPROX_NONE);
        
        for(int i=0; i<all_contours.size(); i++){
        	List<Point> ac_Points = new ArrayList<Point>();
			Converters.Mat_to_vector_Point(all_contours.get(i), ac_Points);
			
			if(ac_Points.size() > min_size){
				contours.add(all_contours.get(i));
			}
		}
        
        //String num2 = "num2: " + contours.size();
        //Log.e("Num", num2);
		
		MatOfPoint approx_poly = new MatOfPoint();
		MatOfPoint2f approx_poly2f = new MatOfPoint2f();
		for (int i = 0; i < contours.size(); ++i){
			MatOfPoint contour = contours.get(i);
			
			MatOfPoint2f contour2f = new MatOfPoint2f();
			contour.convertTo(contour2f, CvType.CV_32FC2);
			
			List<Point> contourPoints = new ArrayList<Point>();
			Converters.Mat_to_vector_Point(contour, contourPoints);
			
			double eps = contourPoints.size()*APPROX_POLY_EPS;
			Imgproc.approxPolyDP(contour2f, approx_poly2f, eps, true);
			approx_poly2f.convertTo(approx_poly, CvType.CV_32S);
			List<Point> approx_poly2f_Points = new ArrayList<Point>();
			List<Point> approx_poly_Points = new ArrayList<Point>();
			Converters.Mat_to_vector_Point(approx_poly2f, approx_poly2f_Points);
			Converters.Mat_to_vector_Point(approx_poly, approx_poly_Points);
			
			if(approx_poly2f_Points.size()==4  &  Imgproc.isContourConvex(approx_poly)){
				float min_side = Float.MAX_VALUE;
				for(int j=0; j<4; j++){
					int k = (j+1)%4;
					Point side = new Point(approx_poly2f_Points.get(j).x-approx_poly2f_Points.get(k).x, approx_poly2f_Points.get(j).y-approx_poly2f_Points.get(k).y);
					min_side = Math.min(min_size, (float) side.dot(side));
				}
				
				if(min_side > min_side_length*min_side_length){
					Marker marker = new Marker(1, approx_poly2f_Points.get(0), approx_poly2f_Points.get(1), approx_poly2f_Points.get(2), approx_poly2f_Points.get(3));
					Point v1 = new Point(marker.marker_corner1.x-marker.marker_corner0.x, marker.marker_corner1.y-marker.marker_corner0.y);
					Point v2 = new Point(marker.marker_corner2.x-marker.marker_corner0.x, marker.marker_corner2.y-marker.marker_corner0.y);
					if(v1.dot(v2) > 0){
						marker = new Marker(1, approx_poly2f_Points.get(0), approx_poly2f_Points.get(3), approx_poly2f_Points.get(2), approx_poly2f_Points.get(1));
					}
					
					markers.add(marker);
				}
			}
		}
		
		matBin.release();
		System.gc();
	}
	
	public void MarkerRecognize(Mat matGray, List<Marker> detectedMarkers, List<Marker> finalMarkers){
		finalMarkers.clear();
		
		Mat matMarker = new Mat();
		int[][] bit_matrix = new int[5][5];
		
		for(int i=0; i<detectedMarkers.size(); i++){
			Marker dm = detectedMarkers.get(i);
			
			double[] src_data = new double[]{dm.marker_corner0.x, dm.marker_corner0.y, dm.marker_corner1.x, dm.marker_corner1.y, dm.marker_corner2.x, dm.marker_corner2.y, dm.marker_corner3.x, dm.marker_corner3.y};
			double[] dst_data = new double[]{0,0,0,MARKER_SIZE-1,MARKER_SIZE-1,MARKER_SIZE-1,MARKER_SIZE-1,0};
			Mat src=new Mat(4,1,CvType.CV_32FC2);
		    Mat dst=new Mat(4,1,CvType.CV_32FC2);
		    src.put(0, 0, src_data);
		    dst.put(0, 0, dst_data);
		    
			Mat M = Imgproc.getPerspectiveTransform(src, dst);
			
			Size dsize = new Size(MARKER_SIZE,MARKER_SIZE);
			Imgproc.warpPerspective(matGray, matMarker, M, dsize);
			Imgproc.threshold(matMarker, matMarker, 125, 255, Imgproc.THRESH_BINARY|Imgproc.THRESH_OTSU);
			
			label:{
				//Valid marker must have a whole black border
				for(int y=0 ; y<7 ; y++){
					int inc = 0;
					if(y==0 || y==6)
						inc = 1;
					else
						inc = 6;
					
					int cell_y = y*MARKER_CELL_SIZE;
					
					for(int x=0 ; x<7 ; x=x+inc){
						int cell_x = x*MARKER_CELL_SIZE;
						Rect roi = new Rect(cell_x, cell_y, MARKER_CELL_SIZE, MARKER_CELL_SIZE);
						int none_zero_count = Core.countNonZero(matMarker.submat(roi));
						if(none_zero_count > MARKER_CELL_SIZE*MARKER_CELL_SIZE/4){
							break label;
						}
					}
				}
				
		        
				//Decode the marker
				for(int y=0 ; y<5 ; y++){
					int cell_y = (y+1)*MARKER_CELL_SIZE;
					for(int x=0 ; x<5; x++){
						int cell_x = (x+1)*MARKER_CELL_SIZE;
						Rect roi = new Rect(cell_x, cell_y, MARKER_CELL_SIZE, MARKER_CELL_SIZE);
						int none_zero_count = Core.countNonZero(matMarker.submat(roi));
						
						if(none_zero_count > MARKER_CELL_SIZE*MARKER_CELL_SIZE/2)
							bit_matrix[y][x] = 1;
						else
							bit_matrix[y][x] = 0;
					}
				}
				
				//String num = "num: " + id;
		        //Log.e("Num", num);
				
				
				//Find the right orientation of the marker
				boolean good_marker = false;
				int rotation_idx = 0;
				for(rotation_idx=0 ; rotation_idx<4; rotation_idx++){
					if(hammingDistance(bit_matrix)==0){
						good_marker = true;
						break;
					}
					bitMatrixRotate(bit_matrix);
				}
				
				if(good_marker){
					Marker final_marker = detectedMarkers.get(i);
					final_marker.marker_id = bitMatrixToId(bit_matrix);
					markerRotate(final_marker, rotation_idx);
					finalMarkers.add(final_marker);
				}
			}
		}
		
		System.gc();
	}
	
	public void MarkerRefine(Mat matGray,List<Marker> finalMarkers){
		for(int i=0 ; i<finalMarkers.size() ; i++){
			Marker marker = finalMarkers.get(i);
			
			TermCriteria criteria = new TermCriteria(TermCriteria.MAX_ITER, 30, 0.1);
			Size winSize = new Size(5,5);
			Size zeroZone = new Size(-1,-1);
			
			MatOfPoint2f corners = new MatOfPoint2f(marker.marker_corner0, marker.marker_corner1, marker.marker_corner2, marker.marker_corner3);
			
			Imgproc.cornerSubPix(matGray, corners, winSize, zeroZone, criteria);
		}
	}
	
////////////////////////////////////////////////////////////////////////	
	private int hammingDistance(int[][] bit_matrix){
		int[][] ids = new int[][]{
			{1,0,0,0,0},	// 00
			{1,0,1,1,1},	// 01
			{0,1,0,0,1},	// 10
			{0,1,1,1,0}		// 11
		};
		
		int dist = 0;
		
		for (int y=0 ; y<5 ; ++y){
			int minSum = 3; //hamming distance to each possible word
	    
			for (int p=0; p<4; ++p){
				int sum=0;
				
				//now, count
				for (int x=0; x<5; ++x){
					if(!(bit_matrix[y][x] == ids[p][x]))
						sum += 1;
				}
				minSum = Math.min(minSum, sum);
			}
	    
			//do the and
			dist += minSum;
		}
		
		return dist;
	}
	
	private void bitMatrixRotate(int[][] bit_matrix){
		int[][] out_bit_matrix = bit_matrix;
		int rows = bit_matrix.length;
		int cols = bit_matrix[0].length;
		
		for (int i=0; i<rows; ++i)
		{
			for (int j=0; j<cols; j++)
			{
				bit_matrix[i][j] = out_bit_matrix[cols-j-1][i];
			}
		}
		
	}
	
	private int bitMatrixToId(int[][] bit_matrix){
		int id = 0;
		
		for (int y=0; y<5; ++y){
			id <<= 1;
			id |= bit_matrix[y][1];

			id <<= 1;
			id |= bit_matrix[y][3];
		}
		
		return id;
	}
	
	private void markerRotate(Marker marker,int rotate){
		Marker m = marker;
		switch(rotate){
		case 1:
			marker.marker_corner0 = m.marker_corner1;
			marker.marker_corner1 = m.marker_corner2;
			marker.marker_corner2 = m.marker_corner3;
			marker.marker_corner3 = m.marker_corner0;
			break;
		case 2:
			marker.marker_corner0 = m.marker_corner2;
			marker.marker_corner1 = m.marker_corner3;
			marker.marker_corner2 = m.marker_corner0;
			marker.marker_corner3 = m.marker_corner1;
			break;
		case 3:
			marker.marker_corner0 = m.marker_corner3;
			marker.marker_corner1 = m.marker_corner0;
			marker.marker_corner2 = m.marker_corner1;
			marker.marker_corner3 = m.marker_corner2;
			break;
		}
	}
}

