package com.ar.game;

    import com.badlogic.gdx.ApplicationAdapter;
    import com.badlogic.gdx.Gdx;
    import com.badlogic.gdx.graphics.Color;
    import com.badlogic.gdx.graphics.GL20;
    import com.badlogic.gdx.graphics.PerspectiveCamera;
    import com.badlogic.gdx.graphics.VertexAttributes;
    import com.badlogic.gdx.graphics.g3d.*;
    import com.badlogic.gdx.graphics.g3d.attributes.ColorAttribute;
    import com.badlogic.gdx.graphics.g3d.environment.DirectionalLight;
    import com.badlogic.gdx.graphics.g3d.utils.ModelBuilder;
    import dk.ar.opencv.UtilAR;
    import org.opencv.calib3d.Calib3d;
    import org.opencv.core.*;
    import org.opencv.highgui.Highgui;
    import org.opencv.highgui.VideoCapture;
    import org.opencv.imgproc.Imgproc;

    import javax.rmi.CORBA.Util;
    import java.io.File;
    import java.lang.Math.*;

    import java.util.ArrayList;
    import java.util.Arrays;
    import java.util.List;
    import java.util.Scanner;



    public class MyGdxGame extends ApplicationAdapter {

        VideoCapture webcam;
        Mat frame = new Mat();
        Mat frameDrawing = new Mat();
        Mat frameBW = new Mat();
        Mat frameMono = new Mat();
        MatOfPoint2f corners2d = new MatOfPoint2f();
        MatOfPoint3f corners3d = new MatOfPoint3f();

        PerspectiveCamera cam;
        ArrayList<ModelInstance> arrows;
        ModelBatch batch;
        private Environment environment;

        boolean useWebcam = true;

        @Override
        public void create () {
            if (useWebcam) {
                webcam = new VideoCapture(0);

            } else {


                String path = getClass().getResource("/webcam.png").getPath();

                System.out.println("Path " + path);
                if (path.startsWith("/")) {
                    path = path.substring(1);
                }
                frame = Highgui.imread(path);

                if (!frame.empty())
                {
                    System.out.println("Height: " + frame.height() + " Width: " + frame.width());
                }
                else
                {
                    System.out.println("Image is empty!" );
                }

            }


            cam = new PerspectiveCamera(67, Gdx.graphics.getWidth(), Gdx.graphics.getHeight());
            cam.position.set(3.6f, 2.4f, -15f);
            cam.lookAt(3,5,0);
            cam.near = 1f;
            cam.far = 300f;
            cam.update();

            environment = new Environment();
            environment.set(new ColorAttribute(ColorAttribute.AmbientLight, 0.2f, 0.8f, 0.4f, 1f));
            environment.add(new DirectionalLight().set(0.8f, 0.8f, 0.8f, -1f, 0.8f, 0.2f));

            batch = new ModelBatch();

            ModelBuilder mb = new ModelBuilder();

            arrows = new ArrayList<ModelInstance>();

            Model arrow_x = mb.createArrow(0, 0, 0, 1, 0, 0, 0.2f, 0.5f, 20, GL20.GL_TRIANGLES, new Material(ColorAttribute.createDiffuse(Color.BLUE)),
                    VertexAttributes.Usage.Position | VertexAttributes.Usage.Normal);

            arrows.add(new ModelInstance(arrow_x));

            Model arrow_y = mb.createArrow(0, 0, 0, 0, 1, 0, 0.2f, 0.5f, 20, GL20.GL_TRIANGLES, new Material(ColorAttribute.createDiffuse(Color.RED)),
                    VertexAttributes.Usage.Position | VertexAttributes.Usage.Normal);

            arrows.add(new ModelInstance(arrow_y));

            Model arrow_z = mb.createArrow(0, 0, 0, 0, 0, 1, 0.2f, 0.5f, 20, GL20.GL_TRIANGLES, new Material(ColorAttribute.createDiffuse(Color.GREEN)),
                    VertexAttributes.Usage.Position | VertexAttributes.Usage.Normal);

            arrows.add(new ModelInstance(arrow_z));
        }

        @Override
        public void render () {
            if(useWebcam) {

                if (!webcam.isOpened() || !webcam.grab() || !webcam.retrieve(frame)) {
                    System.out.println("Unable to get frame");
                    return;
                }


              Core.flip(frame, frame, 1);

            }



            Imgproc.cvtColor(frame, frameBW, Imgproc.COLOR_RGB2GRAY);

            double t = 0.5;
            Imgproc.threshold(frameBW, frameMono, t * 255, 255, Imgproc.THRESH_BINARY);

            frameDrawing = frame.clone();

            List<MatOfPoint> contours = new ArrayList<MatOfPoint>();
            Mat hierarchy = new Mat();


            Imgproc.findContours(frameMono, contours, hierarchy, Imgproc.RETR_LIST, Imgproc.CHAIN_APPROX_SIMPLE);

            Scalar green = new Scalar(0,255,0);

            MatOfPoint2f approx = new MatOfPoint2f();

            List<List<Point>> squares = new ArrayList<List<Point>>();

            for(int i = 0; i < contours.size(); i++) {
                MatOfPoint2f poly = new MatOfPoint2f( contours.get(i).toArray() );
                Imgproc.approxPolyDP( poly, approx, 3, true);

                double area = Imgproc.contourArea(approx,true);
                boolean clockwise = area > 0;
                if(approx.size().area() == 4 && Math.abs(area) > 500 && clockwise) {
                    Scalar color = new Scalar((clockwise ? 0 : 255), (clockwise ? 255 : 0 ), 0);

                    Point prev = null;
                    Point current = null;
                    System.out.println("approx length: " + approx.size());
                    System.out.println("Area is " + area );

                    List<Point> square = new ArrayList<Point>();

                    for(int j  =0; j < approx.size().height  ; j++ ) {
                        for (int k = 0; k < approx.size().width; k++) {
                            System.out.println(j + ", " + k);

                            double[] p = approx.get(j, k);
                            System.out.println(p[0] + ", " + p[1]);
                            prev = current;
                            current = new Point(p[0],p[1]);

                            square.add(current);

                            color = new Scalar(j * (255/3) , 128, 128);

                            Core.circle(frameDrawing, current, 10,color);

                            if(current != null && prev != null) {
                                System.out.println("Drawing line");
                                Core.line(frameDrawing, prev, current, color, 5);

                            }


                        }
                    }
                    // Add square to array
                    squares.add(square);

                    //Render line from last to first on image
                    double[] p = approx.get(0, 0);
                    System.out.println("First: " + p[0] + ", " + p[1]);
                    Point first = new Point(p[0], p[1]);
                    Core.line(frameDrawing, first, current, color, 5);
                    MatOfPoint points = new MatOfPoint(approx.toArray());


                  //  Core.rectangle(
                  //          frameMono,
                  //          new Point(rect.x+rect.width,rect.y+rect.height),
                  //          new Point(rect.x+rect.width,rect.y+rect.height) , new Scalar(255, 0, 0, 255), 3); // FIXME Coordinates
                    //Point(rect.x+rect.width,rect.y+rect.height) , new Scalar(255, 0, 0, 255), 3);

                }

            }

            System.out.println("Total of " + squares.size() + " detected");

            UtilAR.imDrawBackground(frameDrawing);

            if(squares.size() == 1) {

                List<Point> corners = new ArrayList<Point>();
                for (int i = 0; i < squares.size(); i++) {
                    List<Point> square = squares.get(i);
                    corners2d.fromList(square);
                }

                MatOfDouble distortionCoefficients = UtilAR.getDefaultDistortionCoefficients();
                Mat cameraMatrix = UtilAR.getDefaultIntrinsicMatrixMads(frame.width(), frame.height());
                corners3d = getCorner3f();

                Mat rotationVector = new Mat();
                Mat transformationVector =  new Mat();

                System.out.println(corners2d.toList().toString());
                System.out.println(corners3d.toList().toString());

                Calib3d.solvePnP(corners3d,
                        corners2d,
                        cameraMatrix,
                        distortionCoefficients,
                        rotationVector,
                        transformationVector);

                System.out.println("Rotation: " + rotationVector.dump());
                System.out.println("Transform: " + transformationVector.dump());

                UtilAR.setCameraByRT(rotationVector, transformationVector, cam);

                System.out.println("Camera position: " + cam.position.toString());

                int cols = frame.cols()/5;
                int rows = frame.rows()/5;
                List<Point> cornerList = new ArrayList<Point>();
                cornerList.add(new Point(0,0));
                cornerList.add(new Point(cols,0));
                cornerList.add(new Point(cols,rows));
                cornerList.add(new Point(0,rows));

                MatOfPoint2f obj_corners = new MatOfPoint2f();
                obj_corners.fromList(cornerList);

                Mat trans = Imgproc.getPerspectiveTransform(corners2d, obj_corners);
                Mat transformedMat = new Mat();

                Imgproc.warpPerspective(
                        frame,
                        transformedMat,
                        trans,
                        new Size(200,200),
                        Imgproc.INTER_LINEAR,
                        Imgproc.BORDER_CONSTANT,
                        green);

                Core.flip(transformedMat,transformedMat, 1);

                Mat preview = frame.colRange(0,200).rowRange(0,200);
                transformedMat.copyTo(preview);

                UtilAR.imDrawBackground(frame);
                batch.begin(cam);
                for(ModelInstance i : arrows) {
                    batch.render(i, environment);
                }
                batch.end();
            }

        }

        MatOfPoint3f getCorner3f() {
            MatOfPoint3f corners3f = new MatOfPoint3f();
            Point3[] vp = new Point3[4];
            vp[0] = new Point3(0,0,0);
            vp[1] = new Point3(0,1,0);
            vp[2] = new Point3(1,1,0);
            vp[3] = new Point3(1,0,0);

            corners3f.fromArray(vp);
            return corners3f;
        }

    }