package com.android.draw3d;

import java.nio.FloatBuffer;
import java.util.ArrayList;
import java.util.List;

import javax.microedition.khronos.egl.EGLConfig;
import javax.microedition.khronos.opengles.GL10;

import org.opencv.android.Utils;
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.utils.Converters;

import android.content.Context;
import android.graphics.Bitmap;
import android.opengl.GLSurfaceView;
import android.opengl.GLU;
import android.util.Log;

public class DrawModelRenderer implements GLSurfaceView.Renderer {
	private Context mContext;

	private DrawModel mModel;
	
	private int view_width = 640;
	private int view_height = 720;
	private float ratio = 0;

	private static float speedPyramid = 2.0f;
	private static float speedCube = -0.5f;
	private float angleModel;
	
	private boolean has_update = false;
	private int markers_amount = 0;
	public List<Marker> markers = new ArrayList<Marker>();
	
	
	public DrawModelRenderer(Context context) {
		mContext = context;
		mModel = new DrawModel();	
	}

	@Override
	public void onSurfaceCreated(GL10 gl, EGLConfig config) {
		gl.glClearColor(0.0f, 0.0f, 0.0f, 0.0f); // Set color's clear-value to
		/*											// black
		gl.glClearDepthf(1.0f); // Set depth's clear-value to farthest
		gl.glEnable(GL10.GL_DEPTH_TEST); // Enables depth-buffer for hidden
											// surface removal
		gl.glDepthFunc(GL10.GL_LEQUAL); // The type of depth testing to do
		gl.glHint(GL10.GL_PERSPECTIVE_CORRECTION_HINT, GL10.GL_NICEST); // nice
																		// perspective
																		// view
		gl.glShadeModel(GL10.GL_SMOOTH); // Enable smooth shading of color
		gl.glDisable(GL10.GL_DITHER); // Disable dithering for better
		*/								// performance

		// Setup Texture, each time the surface is created (NEW)
		mModel.loadTexture(gl, mContext); // Load image into Texture (NEW)
		gl.glEnable(GL10.GL_TEXTURE_2D); // Enable texture (NEW)
		
		sizeChanged(gl, view_width, view_height);
	}

	@Override
	public void onSurfaceChanged(GL10 gl, int width, int height) {
		if (height == 0)
			height = 1; // To prevent divide by zero
		float aspect = (float) width / height;

		// Set the viewport (display area) to cover the entire window
		gl.glViewport(0, 0, width, height);

		// Setup perspective projection, with aspect ratio matches viewport
		gl.glMatrixMode(GL10.GL_PROJECTION); // Select projection matrix
		gl.glLoadIdentity(); // Reset projection matrix
		// Use perspective projection
		GLU.gluPerspective(gl, 45, aspect, 0.1f, 100.f);

		gl.glMatrixMode(GL10.GL_MODELVIEW); // Select model-view matrix
		gl.glLoadIdentity(); // Reset
	}

	@Override
	public void onDrawFrame(GL10 gl) {
		/*gl.glClear(GL10.GL_COLOR_BUFFER_BIT | GL10.GL_DEPTH_BUFFER_BIT);
		
		gl.glLoadIdentity();
		gl.glTranslatef(0.0f, 0.0f, 0.0f);
		gl.glScalef(1.0f, 1.0f, 1.0f);
		gl.glRotatef(4, 0.2f, 0.2f, 0.1f);
		//mModel.cube_draw(gl);
		
		if(markers_amount>0 & markers.size()>0){
			for(int i=0; i<markers.size(); i++){
				if(markers.get(i).marker_id == 198 || markers.get(i).marker_id == 588){
					float mitte_x = (float) (markers.get(i).marker_corner0.x+markers.get(i).marker_corner2.x)/2;
					float mitte_y = (float) (markers.get(i).marker_corner0.y+markers.get(i).marker_corner2.y)/2;
					float grob_size = (float) (Math.abs(markers.get(i).marker_corner0.x-markers.get(i).marker_corner2.x) + 
							                   Math.abs(markers.get(i).marker_corner1.x-markers.get(i).marker_corner3.x))/2;
					float factor = grob_size/640;
					
					float x = ((mitte_x-200)/320*ratio)/factor;
					float y = ((260-mitte_y)/360)/factor;
					float z = -1/factor;
					//mModel.cub_translation(x,y,z);
					gl.glTranslatef(x, y, z);
					mModel.cube_draw(gl);
				}
			}
		}
		
		if(has_update){
			angleModel += speedCube; // (NEW)
		}*/
		
		draw(markers, gl);
		System.gc();
	}

	public void sizeChanged(GL10 gl, int width, int height) {
		gl.glViewport(0, 0, width, height);
		ratio = (float) width / height;
		gl.glMatrixMode(GL10.GL_PROJECTION);
		gl.glLoadIdentity();
		gl.glFrustumf(-ratio, ratio, -1, 1, 1, 25);
	}
	
	public void set_has_update(boolean set_cmd){
		has_update = set_cmd;
	}
	
	public void set_markers(int size, List<Marker> rMarkers){
		markers_amount = size;
		markers = new ArrayList<Marker>();
		markers = rMarkers;
	}
	/////////////////////////////////////////////////
	public void draw(List<Marker> rMarkers, GL10 gl){
		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};
		
		Mat mCameraMatrix;
		mCameraMatrix= new Mat(3, 3, CvType.CV_64FC1);
		mCameraMatrix.put(0, 0, camera_matrix);
		
        MatOfDouble mDistortionCoefficients;
        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);
        
        Mat Rvec = new Mat(3, 1, CvType.CV_32FC1);
		Mat Tvec = new Mat(3, 1, CvType.CV_32FC1);
		
        
		gl.glClear(GL10.GL_COLOR_BUFFER_BIT | GL10.GL_DEPTH_BUFFER_BIT);
		
		float m_projection_matrix[] = new float[16];
		intrinsicMatrix2ProjectionMatrix(camera_matrix, 640, 720, 1.0f, 25.0f, m_projection_matrix);
		gl.glMatrixMode(GL10.GL_PROJECTION);
		float reflect[] = {
				1,  0, 0, 0,
				0, -1, 0, 0,
				0,  0, 1, 0,
				0,  0, 0, 1
			};
		gl.glLoadMatrixf(reflect, 0);
		gl.glMultMatrixf(m_projection_matrix, 0);
		
		gl.glMatrixMode(GL10.GL_MODELVIEW);
		gl.glLoadIdentity();
		gl.glEnable(GL10.GL_BLEND);
		gl.glEnable(GL10.GL_DEPTH_TEST);
		gl.glShadeModel(GL10.GL_SMOOTH);
		
		if(markers_amount>0 & rMarkers.size()>0){
			for(int i=0; i<markers.size(); i++){
				if(markers.get(i).marker_id == 198 || markers.get(i).marker_id == 588){
					float m_model_view_matrix[] = new float[16];
					
					double size = 1;
					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(markers.get(i).marker_corner0, markers.get(i).marker_corner1, markers.get(i).marker_corner2, markers.get(i).marker_corner3);

					Calib3d.solvePnP(objectPoints, imagePoints, mCameraMatrix, mDistortionCoefficients, Rvec, Tvec);
					
					extrinsicMatrix2ModelViewMatrix(Rvec, Tvec, m_model_view_matrix);
					gl.glLoadMatrixf(m_model_view_matrix, 0);
					mModel.cube_draw(gl);
				}
			}
		}
		
		gl.glDisable(GL10.GL_DEPTH_TEST);
		gl.glMatrixMode(GL10.GL_MODELVIEW);
		gl.glMatrixMode(GL10.GL_PROJECTION);
		gl.glLoadIdentity();
	}
	
	public void intrinsicMatrix2ProjectionMatrix(double[] camera_matrix, float width, float height, float near_plane, float far_plane, float[] projection_matrix)
	{
		float f_x = (float) camera_matrix[0];
		float f_y = (float) camera_matrix[4];

		float c_x = (float) camera_matrix[2];
		float c_y = (float) camera_matrix[5];
		
		projection_matrix[0] = 2*f_x/width;
		projection_matrix[1] = 0.0f;
		projection_matrix[2] = 0.0f;
		projection_matrix[3] = 0.0f;

		projection_matrix[4] = 0.0f;
		projection_matrix[5] = 2*f_y/height;
		projection_matrix[6] = 0.0f;
		projection_matrix[7] = 0.0f;

		projection_matrix[8] = 1.0f - 2*c_x/width;
		projection_matrix[9] = 2*c_y/height - 1.0f;
		projection_matrix[10] = -(far_plane + near_plane)/(far_plane - near_plane);
		projection_matrix[11] = -1.0f;

		projection_matrix[12] = 0.0f;
		projection_matrix[13] = 0.0f;
		projection_matrix[14] = -2.0f*far_plane*near_plane/(far_plane - near_plane);
		projection_matrix[15] = 0.0f;
	}
	
	public void extrinsicMatrix2ModelViewMatrix(Mat rotation, Mat translation, float[] model_view_matrix){
		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));
		
		double d[] = 
		{
			1, 0, 0,
			0, -1, 0,
			0, 0, -1
		};
		Mat rx = new Mat(3, 3, CvType.CV_64FC1);
		rx.put(0, 0, d);
		
		Mat r = new Mat(3, 3, CvType.CV_64FC1);
		Calib3d.Rodrigues(rotation, r);
		Core.multiply(rx, r, r);
		
		model_view_matrix[0] = (float) r.get(0, 0)[0];
		model_view_matrix[1] = (float) r.get(1, 0)[0];
		model_view_matrix[2] = (float) r.get(2, 0)[0];
		model_view_matrix[3] = 0.0f;

		model_view_matrix[4] = (float) r.get(0, 1)[0];
		model_view_matrix[5] = (float) r.get(1, 1)[0];
		model_view_matrix[6] = (float) r.get(2, 1)[0];
		model_view_matrix[7] = 0.0f;

		model_view_matrix[8] = (float) r.get(0, 2)[0];
		model_view_matrix[9] = (float) r.get(1, 2)[0];
		model_view_matrix[10] = (float) r.get(2, 2)[0];
		model_view_matrix[11] = 0.0f;
		
		model_view_matrix[12] = (float) translation.get(0, 0)[0];
		model_view_matrix[13] = (float) translation.get(1, 0)[0];
		model_view_matrix[14] = (float) -translation.get(2, 0)[0];
		model_view_matrix[15] = 1.0f;
	}
}
