/*
 * Copyright 2014 Google Inc. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

package com.kitware.tangoutils.renderables;

import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.nio.FloatBuffer;

import android.opengl.GLES20;
import android.opengl.Matrix;
import android.util.Log;

/**
 * {@link Renderable} OpenGL object showing the Trajectory of the Project Tango
 * device in 3D space. Points are added when the trajectory is updated by
 * passing translation data obtained from Tango Pose Data.
 */
public class Trajectory extends Renderable {

    private static final int COORDS_PER_VERTEX = 3;
    private static final float MIN_DISTANCE_CHECK = 0.025f;

    /** Note: due to resetPath() logic, keep this as a multiple of 9 **/
    private static final int MAX_VERTICES = 9000;
    private static final int BYTES_PER_FLOAT = 4;
    private static int mTrajectoryCount = 0;

    private static final String TAG = Trajectory.class.getSimpleName();
    private String mVertexShaderCode = "uniform mat4 uMVPMatrix;"
            + "attribute vec4 vPosition;" + "uniform vec4 aColor;"
            + "varying vec4 vColor;" + "void main() {" + "gl_PointSize = 5.0;"
            + "vColor=aColor;" + "gl_Position = uMVPMatrix * vPosition;" + "}";
    private String mFragmentShaderCode = "precision mediump float;"
            + "varying vec4 vColor;" + "void main() {"
            + "gl_FragColor = vColor;" + "}";
    private FloatBuffer mVertexBuffer;
    private float[] mColor = { 0.22f, 0.28f, 0.67f, 1.0f };
    private final int mProgram;
    private int mPosHandle;
    private int mMVPMatrixHandle;
    private int mColorHandle;
    private int mLineWidth;

    public Trajectory(int lineWidth) {
        mLineWidth = lineWidth;
        // Reset the model matrix to the identity
        Matrix.setIdentityM(getModelMatrix(), 0);

        // Allocate a vertex buffer
        ByteBuffer vertexByteBuffer = ByteBuffer.allocateDirect(MAX_VERTICES
                * BYTES_PER_FLOAT);
        vertexByteBuffer.order(ByteOrder.nativeOrder());
        mVertexBuffer = vertexByteBuffer.asFloatBuffer();

        // Load the vertex and fragment shaders, then link the program
        int vertexShader = RenderUtils.loadShader(GLES20.GL_VERTEX_SHADER,
                mVertexShaderCode);
        int fragShader = RenderUtils.loadShader(GLES20.GL_FRAGMENT_SHADER,
                mFragmentShaderCode);
        mProgram = GLES20.glCreateProgram();
        GLES20.glAttachShader(mProgram, vertexShader);
        GLES20.glAttachShader(mProgram, fragShader);
        GLES20.glLinkProgram(mProgram);
    }

    // float[] color should contain only 4 elements.
    public Trajectory(int lineWidth, float[] color) {
        mLineWidth = lineWidth;
        mColor = color;
        // Reset the model matrix to the identity
        Matrix.setIdentityM(getModelMatrix(), 0);

        // Allocate a vertex buffer
        ByteBuffer vertexByteBuffer = ByteBuffer.allocateDirect(MAX_VERTICES
                * BYTES_PER_FLOAT);
        vertexByteBuffer.order(ByteOrder.nativeOrder());
        mVertexBuffer = vertexByteBuffer.asFloatBuffer();

        // Load the vertex and fragment shaders, then link the program
        int vertexShader = RenderUtils.loadShader(GLES20.GL_VERTEX_SHADER,
                mVertexShaderCode);
        int fragShader = RenderUtils.loadShader(GLES20.GL_FRAGMENT_SHADER,
                mFragmentShaderCode);
        mProgram = GLES20.glCreateProgram();
        GLES20.glAttachShader(mProgram, vertexShader);
        GLES20.glAttachShader(mProgram, fragShader);
        GLES20.glLinkProgram(mProgram);
    }

    public void updateTrajectory(float[] translation) {
        mVertexBuffer.position(mTrajectoryCount * 3);
        if (((mTrajectoryCount + 1) * 3) >= MAX_VERTICES) {
            Log.w(TAG, "Clearing float buffer");
            resetPath();
        }
        float dx = 0, dy = 0, dz = 0;
        try {
            dx = mVertexBuffer.get(mVertexBuffer.position() - 3)
                    - translation[0];
            dy = mVertexBuffer.get(mVertexBuffer.position() - 2)
                    - translation[2];
            dz = mVertexBuffer.get(mVertexBuffer.position() - 1)
                    - (-translation[1]);
        } catch (IndexOutOfBoundsException e) {
            mVertexBuffer.put(new float[] { translation[0], translation[2],
                    -translation[1] });
            mTrajectoryCount++;
        }
        float distance = (float) Math.sqrt(dx * dx + dy * dy + dz * dz);
        if (distance > MIN_DISTANCE_CHECK) {
            mVertexBuffer.put(new float[] { translation[0], translation[2],
                    -translation[1] });
            mTrajectoryCount++;
        }
    }

    public void resetPath() {
        int currentPosition = mVertexBuffer.position();
        int pointsToGet = (MAX_VERTICES / 3);
        mVertexBuffer.position(currentPosition - pointsToGet);

        float[] tail = new float[pointsToGet];
        mVertexBuffer.get(tail, 0, pointsToGet);

        mVertexBuffer.clear();
        mVertexBuffer.put(tail);

        mTrajectoryCount = pointsToGet / 3;
    }

    public void clearPath() {
        ByteBuffer vertexByteBuffer = ByteBuffer.allocateDirect(MAX_VERTICES
                * BYTES_PER_FLOAT);
        vertexByteBuffer.order(ByteOrder.nativeOrder());
        mVertexBuffer = vertexByteBuffer.asFloatBuffer();
    }

    @Override
    public void draw(float[] viewMatrix, float[] projectionMatrix) {
        GLES20.glUseProgram(mProgram);
        mVertexBuffer.position(0);

        // Compose the model, view, and projection matrices into a single m-v-p
        // matrix
        updateMvpMatrix(viewMatrix, projectionMatrix);

        // Load vertex attribute data
        mPosHandle = GLES20.glGetAttribLocation(mProgram, "vPosition");
        GLES20.glVertexAttribPointer(mPosHandle, COORDS_PER_VERTEX,
                GLES20.GL_FLOAT, false, 0, mVertexBuffer);
        GLES20.glEnableVertexAttribArray(mPosHandle);

        mMVPMatrixHandle = GLES20.glGetUniformLocation(mProgram, "uMVPMatrix");
        GLES20.glUniformMatrix4fv(mMVPMatrixHandle, 1, false, getMvpMatrix(), 0);

        mColorHandle = GLES20.glGetUniformLocation(mProgram, "aColor");
        GLES20.glUniform4f(mColorHandle, mColor[0], mColor[1], mColor[2],
                mColor[3]);
        GLES20.glLineWidth(mLineWidth);
        GLES20.glDrawArrays(GLES20.GL_LINE_STRIP, 0, mTrajectoryCount);
    }

    public void setColor(float[] color) {
        mColor = color;
    }
}