package com.android.cts.verifier.sensors;

/*
 * Copyright (C) 2014 The Android Open Source Project
 *
 * 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.
 */
import android.media.MediaCodec;
import android.media.MediaExtractor;
import android.media.MediaFormat;
import android.os.Debug;
import android.os.Environment;
import android.os.PowerManager;
import android.util.JsonWriter;
import android.util.Log;

import org.opencv.core.Mat;
import org.opencv.core.CvType;
import org.opencv.core.MatOfDouble;
import org.opencv.core.MatOfFloat;
import org.opencv.core.MatOfPoint2f;
import org.opencv.core.MatOfPoint3f;
import org.opencv.core.Point;
import org.opencv.core.Size;
import org.opencv.imgcodecs.Imgcodecs;
import org.opencv.imgproc.Imgproc;
import org.opencv.calib3d.Calib3d;
import org.opencv.core.Core;

import org.json.JSONObject;
import org.json.JSONException;

import java.io.BufferedReader;
import java.io.ByteArrayOutputStream;
import java.io.File;
import java.io.FileNotFoundException;
import java.io.FileOutputStream;
import java.io.FileReader;
import java.io.IOException;
import java.io.OutputStream;
import java.io.OutputStreamWriter;
import java.nio.ByteBuffer;
import java.util.ArrayList;

import android.opengl.GLES20;
import javax.microedition.khronos.opengles.GL10;

/**
 *  This class does analysis on the recorded RVCVCXCheck data sets.
 */
public class RVCVXCheckAnalyzer {
    private static final String TAG = "RVCVXAnalysis";
    private static final boolean LOCAL_LOGV = false;
    private static final boolean LOCAL_LOGD = true;
    private final String mPath;

    private static final boolean OUTPUT_DEBUG_IMAGE = false;
    private static final double VALID_FRAME_THRESHOLD = 0.8;
    private static final double REPROJECTION_THRESHOLD_RATIO = 0.03;
    private static final boolean FORCE_CV_ANALYSIS  = false;
    private static final boolean TRACE_VIDEO_ANALYSIS = false;
    private static final double DECIMATION_FPS_TARGET = 15.0;
    private static final double MIN_VIDEO_LENGTH_SEC = 10;

    RVCVXCheckAnalyzer(String path)
    {
        mPath = path;
    }

    /**
     * A class that contains  the analysis results
     *
     */
    class AnalyzeReport {
        public boolean error=true;
        public String reason = "incomplete";

        // roll pitch yaw RMS error ( \sqrt{\frac{1}{n} \sum e_i^2 })
        // unit in rad
        public double roll_rms_error;
        public double pitch_rms_error;
        public double yaw_rms_error;

        // roll pitch yaw max error
        // unit in rad
        public double roll_max_error;
        public double pitch_max_error;
        public double yaw_max_error;

        // optimal t delta between sensor and camera data set to make best match
        public double optimal_delta_t;
        // the associate yaw offset based on initial values
        public double yaw_offset;

        public int n_of_frame;
        public int n_of_valid_frame;

        // both data below are in [sec]
        public double sensor_period_avg;
        public double sensor_period_stdev;

        /**
         * write Json format serialization to a file in case future processing need the data
         */
        public void writeToFile(File file) {
            try {
                writeJSONToStream(new FileOutputStream(file));
            } catch (FileNotFoundException e) {
                e.printStackTrace();
                Log.e(TAG, "Cannot create analyze report file.");
            }
        }

        /**
         * Get the JSON format serialization
         *@return Json format serialization as String
         */
        @Override
        public String toString() {
            ByteArrayOutputStream s = new ByteArrayOutputStream();
            writeJSONToStream(s);
            return new String(s.toByteArray(),  java.nio.charset.StandardCharsets.UTF_8);
        }

        private void writeJSONToStream(OutputStream s) {
            try{
                JsonWriter writer =
                        new JsonWriter(
                                new OutputStreamWriter( s )
                        );
                writer.beginObject();
                writer.setLenient(true);

                writer.name("roll_rms_error").value(roll_rms_error);
                writer.name("pitch_rms_error").value(pitch_rms_error);
                writer.name("yaw_rms_error").value(yaw_rms_error);
                writer.name("roll_max_error").value(roll_max_error);
                writer.name("pitch_max_error").value(pitch_max_error);
                writer.name("yaw_max_error").value(yaw_max_error);
                writer.name("optimal_delta_t").value(optimal_delta_t);
                writer.name("yaw_offset").value(yaw_offset);
                writer.name("n_of_frame").value(n_of_frame);
                writer.name("n_of_valid_frame").value(n_of_valid_frame);
                writer.name("sensor_period_avg").value(sensor_period_avg);
                writer.name("sensor_period_stdev").value(sensor_period_stdev);

                writer.endObject();

                writer.close();
            } catch (IOException e) {
                // do nothing
                Log.e(TAG, "Error in serialize analyze report to JSON");
            } catch (IllegalArgumentException e) {
                e.printStackTrace();
                Log.e(TAG, "Invalid parameter to write into JSON format");
            }
        }
    }

    /**
     *  Process data set stored in the path specified in constructor
     *  and return an analyze report to caller
     *
     *  @return An AnalyzeReport that contains detailed information about analysis
     */
    public AnalyzeReport processDataSet() {
        int nframe;// number of frames in video
        int nslog; // number of sensor log
        int nvlog; // number of video generated log


        AnalyzeReport report = new AnalyzeReport();

        ArrayList<AttitudeRec> srecs = new ArrayList<>();
        ArrayList<AttitudeRec> vrecs = new ArrayList<>();
        ArrayList<AttitudeRec> srecs2 = new ArrayList<>();


        final boolean use_solved = new File(mPath, "vision_rpy.log").exists() && !FORCE_CV_ANALYSIS;

        if (use_solved) {
            nframe = nvlog = loadAttitudeRecs(new File(mPath, "vision_rpy.log"), vrecs);
            nslog = loadAttitudeRecs(new File(mPath, "sensor_rpy.log"),srecs);
        }else {
            nframe = analyzeVideo(vrecs);
            nvlog = vrecs.size();

            if (LOCAL_LOGV) {
                Log.v(TAG, "Post video analysis nvlog = " + nvlog + " nframe=" + nframe);
            }
            if (nvlog <= 0 || nframe <= 0) {
                // invalid results
                report.reason = "Unable to to load recorded video.";
                return report;
            }
            if (nframe < MIN_VIDEO_LENGTH_SEC*VALID_FRAME_THRESHOLD) {
                // video is too short
                Log.w(TAG, "Video record is to short, n frame = " + nframe);
                report.reason = "Video too short.";
                return report;
            }
            if ((double) nvlog / nframe < VALID_FRAME_THRESHOLD) {
                // too many invalid frames
                Log.w(TAG, "Too many invalid frames, n valid frame = " + nvlog +
                        ", n total frame = " + nframe);
                report.reason = "Too many invalid frames.";
                return report;
            }

            fixFlippedAxis(vrecs);

            nslog = loadSensorLog(srecs);
        }

        // Gradient descent will have faster performance than this simple search,
        // but the performance is dominated by the vision part, so it is not very necessary.
        double delta_t;
        double min_rms = Double.MAX_VALUE;
        double min_delta_t =0.;
        double min_yaw_offset =0.;

        // pre-allocation
        for (AttitudeRec i: vrecs) {
            srecs2.add(new AttitudeRec(0,0,0,0));
        }

        // find optimal offset
        for (delta_t = -2.0; delta_t<2.0; delta_t +=0.01) {
            double rms;
            resampleSensorLog(srecs, vrecs, delta_t, 0.0, srecs2);
            rms = Math.sqrt(calcSqrErr(vrecs, srecs2, 0)+ calcSqrErr(vrecs, srecs2, 1));
            if (rms < min_rms) {
                min_rms = rms;
                min_delta_t = delta_t;
                min_yaw_offset = vrecs.get(0).yaw - srecs2.get(0).yaw;
            }
        }
        // sample at optimal offset
        resampleSensorLog(srecs, vrecs, min_delta_t, min_yaw_offset, srecs2);

        if (!use_solved) {
            dumpAttitudeRecs(new File(mPath, "vision_rpy.log"), vrecs);
            dumpAttitudeRecs(new File(mPath, "sensor_rpy.log"), srecs);
        }
        dumpAttitudeRecs(new File(mPath, "sensor_rpy_resampled.log"), srecs2);
        dumpAttitudeError(new File(mPath, "attitude_error.log"), vrecs, srecs2);

        // fill report fields
        report.roll_rms_error = Math.sqrt(calcSqrErr(vrecs, srecs2, 0));
        report.pitch_rms_error = Math.sqrt(calcSqrErr(vrecs, srecs2, 1));
        report.yaw_rms_error = Math.sqrt(calcSqrErr(vrecs, srecs2, 2));

        report.roll_max_error = calcMaxErr(vrecs, srecs2, 0);
        report.pitch_max_error = calcMaxErr(vrecs, srecs2, 1);
        report.yaw_max_error = calcMaxErr(vrecs, srecs2, 2);

        report.optimal_delta_t = min_delta_t;
        report.yaw_offset = (min_yaw_offset);

        report.n_of_frame = nframe;
        report.n_of_valid_frame = nvlog;

        double [] sensor_period_stat = calcSensorPeriodStat(srecs);
        report.sensor_period_avg = sensor_period_stat[0];
        report.sensor_period_stdev = sensor_period_stat[1];

        // output report to file and log in JSON format as well
        report.writeToFile(new File(mPath, "report.json"));
        if (LOCAL_LOGV)    Log.v(TAG, "Report in JSON:" + report.toString());

        report.reason = "Completed";
        report.error = false;
        return report;
    }

    /**
     * Generate pattern geometry like this one
     * http://docs.opencv.org/trunk/_downloads/acircles_pattern.png
     *
     * @return Array of 3D points
     */
    private MatOfPoint3f asymmetricalCircleGrid(Size size) {
        final int cn = 3;

        int n = (int)(size.width * size.height);
        float positions[] = new float[n * cn];
        float unit=0.02f;
        MatOfPoint3f grid = new MatOfPoint3f();

        for (int i = 0; i < size.height; i++) {
            for (int j = 0; j < size.width * cn; j += cn) {
                positions[(int) (i * size.width * cn + j + 0)] =
                        (2 * (j / cn) + i % 2) * (float) unit;
                positions[(int) (i * size.width * cn + j + 1)] =
                        i * unit;
                positions[(int) (i * size.width * cn + j + 2)] = 0;
            }
        }
        grid.create(n, 1, CvType.CV_32FC3);
        grid.put(0, 0, positions);
        return grid;
    }

    /**
     *  Create a camera intrinsic matrix using input parameters
     *
     *  The camera intrinsic matrix will be like:
     *
     *       +-                       -+
     *       |  f   0    center.width  |
     *   A = |  0   f    center.height |
     *       |  0   0         1        |
     *       +-                       -+
     *
     *  @return An approximated (not actually calibrated) camera matrix
     */
    private static Mat cameraMatrix(float f, Size center) {
        final double [] data = {f, 0, center.width, 0, f, center.height, 0, 0, 1f};
        Mat m = new Mat(3,3, CvType.CV_64F);
        m.put(0, 0, data);
        return m;
    }

    /**
     *  Attitude record in time roll pitch yaw format.
     *
     */
    private class AttitudeRec {
        public double time;
        public double roll;
        public double pitch;
        public double yaw;

        // ctor
        AttitudeRec(double atime, double aroll, double apitch, double ayaw) {
            time = atime;
            roll = aroll;
            pitch = apitch;
            yaw = ayaw;
        }

        // ctor
        AttitudeRec(double atime, double [] rpy) {
            time = atime;
            roll = rpy[0];
            pitch = rpy[1];
            yaw = rpy[2];
        }

        // copy value of another to this
        void assign(AttitudeRec rec) {
            time = rec.time;
            roll = rec.time;
            pitch = rec.pitch;
            yaw = rec.yaw;
        }

        // copy roll-pitch-yaw value but leave the time specified by atime
        void assign(AttitudeRec rec, double atime) {
            time = atime;
            roll = rec.time;
            pitch = rec.pitch;
            yaw = rec.yaw;
        }

        // set each field separately
        void set(double atime, double aroll, double apitch, double ayaw) {
            time = atime;
            roll = aroll;
            pitch = apitch;
            yaw = ayaw;
        }
    }


    /**
     *  Load the sensor log in (time Roll-pitch-yaw) format to a ArrayList<AttitudeRec>
     *
     *  @return the number of sensor log items
     */
    private int loadSensorLog(ArrayList<AttitudeRec> recs) {
        //ArrayList<AttitudeRec> recs = new ArrayList<AttitudeRec>();
        File csvFile = new File(mPath, "sensor.log");
        BufferedReader br=null;
        String line;

        // preallocate and reuse
        double [] quat = new double[4];
        double [] rpy = new double[3];

        double t0 = -1;

        try {
            br = new BufferedReader(new FileReader(csvFile));
            while ((line = br.readLine()) != null) {
                //space separator
                String[] items = line.split(" ");

                if (items.length != 5) {
                    recs.clear();
                    return -1;
                }

                quat[0] = Double.parseDouble(items[1]);
                quat[1] = Double.parseDouble(items[2]);
                quat[2] = Double.parseDouble(items[3]);
                quat[3] = Double.parseDouble(items[4]);

                //
                quat2rpy(quat, rpy);

                if (t0 < 0) {
                    t0 = Long.parseLong(items[0])/1e9;
                }
                recs.add(new AttitudeRec(Long.parseLong(items[0])/1e9-t0, rpy));
            }

        } catch (FileNotFoundException e) {
            e.printStackTrace();
            Log.e(TAG, "Cannot find sensor logging data");
        } catch (IOException e) {
            e.printStackTrace();
            Log.e(TAG, "Cannot read sensor logging data");
        } finally {
            if (br != null) {
                try {
                    br.close();
                } catch (IOException e) {
                    e.printStackTrace();
                }
            }
        }

        return recs.size();
    }

    /**
     * Read video meta info
     */
    private class VideoMetaInfo {
        public double fps;
        public int frameWidth;
        public int frameHeight;
        public double fovWidth;
        public double fovHeight;
        public boolean valid = false;

        VideoMetaInfo(File file) {

            BufferedReader br=null;
            String line;
            String content="";
            try {
                br = new BufferedReader(new FileReader(file));
                while ((line = br.readLine()) != null) {
                    content = content +line;
                }

            } catch (FileNotFoundException e) {
                e.printStackTrace();
                Log.e(TAG, "Cannot find video meta info file");
            } catch (IOException e) {
                e.printStackTrace();
                Log.e(TAG, "Cannot read video meta info file");
            } finally {
                if (br != null) {
                    try {
                        br.close();
                    } catch (IOException e) {
                        e.printStackTrace();
                    }
                }
            }

            if (content.isEmpty()) {
                return;
            }

            try {
                JSONObject json = new JSONObject(content);
                frameWidth = json.getInt("width");
                frameHeight = json.getInt("height");
                fps = json.getDouble("frameRate");
                fovWidth = json.getDouble("fovW")*Math.PI/180.0;
                fovHeight = json.getDouble("fovH")*Math.PI/180.0;
            } catch (JSONException e) {
                return;
            }

            valid = true;

        }
    }


    /**
     * Debugging helper function, load ArrayList<AttitudeRec> from a file dumped out by
     * dumpAttitudeRecs
     */
    private int loadAttitudeRecs(File file, ArrayList<AttitudeRec> recs) {
        BufferedReader br=null;
        String line;
        double time;
        double [] rpy = new double[3];

        try {
            br = new BufferedReader(new FileReader(file));
            while ((line = br.readLine()) != null) {
                //space separator
                String[] items = line.split(" ");

                if (items.length != 4) {
                    recs.clear();
                    return -1;
                }

                time = Double.parseDouble(items[0]);
                rpy[0] = Double.parseDouble(items[1]);
                rpy[1] = Double.parseDouble(items[2]);
                rpy[2] = Double.parseDouble(items[3]);

                recs.add(new AttitudeRec(time, rpy));
            }

        } catch (FileNotFoundException e) {
            e.printStackTrace();
            Log.e(TAG, "Cannot find AttitudeRecs file specified.");
        } catch (IOException e) {
            e.printStackTrace();
            Log.e(TAG, "Read AttitudeRecs file failure");
        } finally {
            if (br != null) {
                try {
                    br.close();
                } catch (IOException e) {
                    e.printStackTrace();
                }
            }
        }

        return recs.size();
    }
    /**
     * Debugging helper function, Dump an ArrayList<AttitudeRec> to a file
     */
    private void dumpAttitudeRecs(File file, ArrayList<AttitudeRec> recs) {
        OutputStreamWriter w=null;
        try {
            w = new OutputStreamWriter(new FileOutputStream(file));

            for (AttitudeRec r : recs) {
                w.write(String.format("%f %f %f %f\r\n", r.time, r.roll, r.pitch, r.yaw));
            }
            w.close();
        } catch(FileNotFoundException e) {
            e.printStackTrace();
            Log.e(TAG, "Cannot create AttitudeRecs file.");
        } catch (IOException e) {
            Log.e(TAG, "Write AttitudeRecs file failure");
        } finally {
            if (w!=null) {
                try {
                    w.close();
                } catch (IOException e) {
                    e.printStackTrace();
                }
            }
        }
    }

    /**
     *  Read the sensor log in ArrayList<AttitudeRec> format and find out the sensor sample time
     *  statistics: mean and standard deviation.
     *
     *  @return The returned value will be a double array with exact 2 items, first [0] will be
     *  mean and the second [1]  will be the standard deviation.
     *
     */
    private double [] calcSensorPeriodStat(ArrayList<AttitudeRec> srec)   {
        double tp = srec.get(0).time;
        int i;
        double sum = 0.0;
        double sumsq = 0.0;
        for(i=1; i<srec.size(); ++i) {
            double dt;
            dt = srec.get(i).time - tp;
            sum += dt;
            sumsq += dt*dt;
            tp += dt;
        }
        double [] ret = new double[2];
        ret[0] = sum/srec.size();
        ret[1] = Math.sqrt(sumsq/srec.size() - ret[0]*ret[0]);
        return ret;
    }

    /**
     * Flipping the axis as the image are flipped upside down in OpenGL frames
     */
    private void fixFlippedAxis(ArrayList<AttitudeRec> vrecs)   {
        for (AttitudeRec i: vrecs) {
            i.yaw = -i.yaw;
        }
    }

    /**
     *  Calculate the maximum error on the specified axis between two time aligned (resampled)
     *  ArrayList<AttitudeRec>. Yaw axis needs special treatment as 0 and 2pi error are same thing
     *
     * @param ra  one ArrayList of AttitudeRec
     * @param rb  the other ArrayList of AttitudeRec
     * @param axis axis id for the comparison (0 = roll, 1 = pitch, 2 = yaw)
     * @return Maximum error
     */
    private double calcMaxErr(ArrayList<AttitudeRec> ra, ArrayList<AttitudeRec> rb, int axis)  {
        // check if they are valid and comparable data
        if (ra.size() != rb.size()) {
            throw new ArrayIndexOutOfBoundsException("Two array has to be the same");
        }
        // check input parameter validity
        if (axis<0 || axis > 2) {
            throw new IllegalArgumentException("Invalid data axis.");
        }

        int i;
        double max = 0.0;
        double diff = 0.0;
        for(i=0; i<ra.size(); ++i) {
            // make sure they are aligned data
            if (ra.get(i).time != rb.get(i).time) {
                throw new IllegalArgumentException("Element "+i+
                        " of two inputs has different time.");
            }
            switch(axis) {
                case 0:
                    diff = ra.get(i).roll - rb.get(i).roll; // they always opposite of each other..
                    break;
                case 1:
                    diff = ra.get(i).pitch - rb.get(i).pitch;
                    break;
                case 2:
                    diff = Math.abs(((4*Math.PI + ra.get(i).yaw - rb.get(i).yaw)%(2*Math.PI))
                            -Math.PI)-Math.PI;
                    break;
            }
            diff = Math.abs(diff);
            if (diff>max) {
                max = diff;
            }
        }
        return max;
    }

    /**
     *  Calculate the RMS error on the specified axis between two time aligned (resampled)
     *  ArrayList<AttitudeRec>. Yaw axis needs special treatment as 0 and 2pi error are same thing
     *
     * @param ra  one ArrayList of AttitudeRec
     * @param rb  the other ArrayList of AttitudeRec
     * @param axis axis id for the comparison (0 = roll, 1 = pitch, 2 = yaw)
     * @return Mean square error
     */
    private double calcSqrErr(ArrayList<AttitudeRec> ra, ArrayList<AttitudeRec> rb, int axis) {
        // check if they are valid and comparable data
        if (ra.size() != rb.size()) {
            throw new ArrayIndexOutOfBoundsException("Two array has to be the same");
        }
        // check input parameter validity
        if (axis<0 || axis > 2) {
            throw new IllegalArgumentException("Invalid data axis.");
        }

        int i;
        double sum = 0.0;
        double diff = 0.0;
        for(i=0; i<ra.size(); ++i) {
            // check input data validity
            if (ra.get(i).time != rb.get(i).time) {
                throw new IllegalArgumentException("Element "+i+
                        " of two inputs has different time.");
            }

            switch(axis) {
                case 0:
                    diff = ra.get(i).roll - rb.get(i).roll;
                    break;
                case 1:
                    diff = ra.get(i).pitch - rb.get(i).pitch;
                    break;
                case 2:
                    diff = Math.abs(((4*Math.PI + ra.get(i).yaw - rb.get(i).yaw)%(2*Math.PI))-
                            Math.PI)-Math.PI;
                    break;
            }

            sum += diff*diff;
        }
        return sum/ra.size();
    }

    /**
     * Debugging helper function. Dump the error between two time aligned ArrayList<AttitudeRec>'s
     *
     * @param file File to write to
     * @param ra  one ArrayList of AttitudeRec
     * @param rb  the other ArrayList of AttitudeRec
     */
    private void dumpAttitudeError(File file, ArrayList<AttitudeRec> ra, ArrayList<AttitudeRec> rb){
        if (ra.size() != rb.size()) {
            throw new ArrayIndexOutOfBoundsException("Two array has to be the same");
        }

        int i;

        ArrayList<AttitudeRec> rerr = new ArrayList<>();
        for(i=0; i<ra.size(); ++i) {
            if (ra.get(i).time != rb.get(i).time) {
                throw new IllegalArgumentException("Element "+ i
                        + " of two inputs has different time.");
            }

            rerr.add(new AttitudeRec(ra.get(i).time, ra.get(i).roll - rb.get(i).roll,
                    ra.get(i).pitch - rb.get(i).pitch,
                    (Math.abs(((4*Math.PI + ra.get(i).yaw - rb.get(i).yaw)%(2*Math.PI))
                            -Math.PI)-Math.PI)));

        }
        dumpAttitudeRecs(file, rerr);
    }

    /**
     * Resample one ArrayList<AttitudeRec> with respect to another ArrayList<AttitudeRec>
     *
     * @param rec           the ArrayList of AttitudeRec to be sampled
     * @param timebase      the other ArrayList of AttitudeRec that serves as time base
     * @param delta_t       offset in time before resample
     * @param yaw_offset    offset in yaw axis
     * @param resampled     output ArrayList of AttitudeRec
     */

    private void resampleSensorLog(ArrayList<AttitudeRec> rec, ArrayList<AttitudeRec> timebase,
            double delta_t, double yaw_offset, ArrayList<AttitudeRec> resampled)    {
        int i;
        int j = -1;
        for(i=0; i<timebase.size(); i++) {
            double time = timebase.get(i).time + delta_t;

            while(j<rec.size()-1 && rec.get(j+1).time < time) j++;

            if (j == -1) {
                //use first
                resampled.get(i).assign(rec.get(0), timebase.get(i).time);
            } else if (j == rec.size()-1) {
                // use last
                resampled.get(i).assign(rec.get(j), timebase.get(i).time);
            } else {
                // do linear resample
                double alpha = (time - rec.get(j).time)/((rec.get(j+1).time - rec.get(j).time));
                double roll = (1-alpha) * rec.get(j).roll + alpha * rec.get(j+1).roll;
                double pitch = (1-alpha) * rec.get(j).pitch + alpha * rec.get(j+1).pitch;
                double yaw = (1-alpha) * rec.get(j).yaw + alpha * rec.get(j+1).yaw + yaw_offset;
                resampled.get(i).set(timebase.get(i).time, roll, pitch, yaw);
            }
        }
    }

    /**
     * Analyze video frames using computer vision approach and generate a ArrayList<AttitudeRec>
     *
     * @param recs  output ArrayList of AttitudeRec
     * @return total number of frame of the video
     */
    private int analyzeVideo(ArrayList<AttitudeRec> recs) {
        VideoMetaInfo meta = new VideoMetaInfo(new File(mPath, "videometa.json"));

        int decimation = 1;
        boolean use_timestamp = true;

        // roughly determine if decimation is necessary
        if (meta.fps > DECIMATION_FPS_TARGET) {
            decimation = (int)(meta.fps / DECIMATION_FPS_TARGET);
            meta.fps /=decimation;
        }

        VideoDecoderForOpenCV videoDecoder = new VideoDecoderForOpenCV(
                new File(mPath, "video.mp4"), decimation);


        Mat frame;
        Mat gray = new Mat();
        int i = -1;

        Size frameSize = videoDecoder.getSize();

        if (frameSize.width != meta.frameWidth || frameSize.height != meta.frameHeight) {
            // this is very unlikely
            return -1;
        }

        if (TRACE_VIDEO_ANALYSIS) {
            Debug.startMethodTracing("cvprocess");
        }

        final int patternWidth = 4;
        final int patternHeight = 11;
        Size patternSize = new Size(patternWidth, patternHeight);

        float fc = (float)(meta.frameWidth/2.0/Math.tan(meta.fovWidth/2.0));
        Mat camMat = cameraMatrix(fc, new Size(frameSize.width/2, frameSize.height/2));
        MatOfDouble coeff = new MatOfDouble(); // dummy

        MatOfPoint2f centers = new MatOfPoint2f();
        MatOfPoint3f grid = asymmetricalCircleGrid(patternSize);
        Mat rvec = new MatOfFloat();
        Mat tvec = new MatOfFloat();

        MatOfPoint2f reprojCenters = new MatOfPoint2f();

        if (LOCAL_LOGV) {
            Log.v(TAG, "Camera Mat = \n" + camMat.dump());
        }

        long startTime = System.nanoTime();
        long [] ts = new long[1];

        while ((frame = videoDecoder.getFrame(ts)) !=null) {
            if (LOCAL_LOGV) {
                Log.v(TAG, "got a frame " + i);
            }

            if (use_timestamp && ts[0] == -1) {
                use_timestamp = false;
            }

            // has to be in front, as there are cases where execution
            // will skip the later part of this while
            i++;

            // convert to gray manually as by default findCirclesGridDefault uses COLOR_BGR2GRAY
            Imgproc.cvtColor(frame, gray, Imgproc.COLOR_RGB2GRAY);

            boolean foundPattern = Calib3d.findCirclesGrid(
                    gray,  patternSize, centers, Calib3d.CALIB_CB_ASYMMETRIC_GRID);

            if (!foundPattern) {
                // skip to next frame
                continue;
            }

            if (OUTPUT_DEBUG_IMAGE) {
                Calib3d.drawChessboardCorners(frame, patternSize, centers, true);
            }

            // figure out the extrinsic parameters using real ground truth 3D points and the pixel
            // position of blobs found in findCircleGrid, an estimated camera matrix and
            // no-distortion are assumed.
            boolean foundSolution =
                    Calib3d.solvePnP(grid, centers, camMat, coeff, rvec, tvec,
                            false, Calib3d.CV_ITERATIVE);

            if (!foundSolution) {
                // skip to next frame
                if (LOCAL_LOGV) {
                    Log.v(TAG, "cannot find pnp solution in frame " + i + ", skipped.");
                }
                continue;
            }

            // reproject points to for evaluation of result accuracy of solvePnP
            Calib3d.projectPoints(grid, rvec, tvec, camMat, coeff, reprojCenters);

            // error is evaluated in norm2, which is real error in pixel distance / sqrt(2)
            double error = Core.norm(centers, reprojCenters, Core.NORM_L2);

            if (LOCAL_LOGV) {
                Log.v(TAG, "Found attitude, re-projection error = " + error);
            }

            // Calculate the average distance between opposite corners of the pattern in pixels
            Point[] centerPoints = centers.toArray();
            Point bottomLeftPos = centerPoints[0];
            Point bottomRightPos = centerPoints[patternWidth - 1];
            Point topLeftPos = centerPoints[(patternHeight * patternWidth) - patternWidth];
            Point topRightPos = centerPoints[(patternHeight * patternWidth) - 1];
            double avgPixelDist = (getDistanceBetweenPoints(bottomLeftPos, topRightPos)
                    + getDistanceBetweenPoints(bottomRightPos, topLeftPos)) / 2;

            // if error is reasonable, add it into the results. Use a dynamic threshold based on
            // the pixel distance of opposite corners of the pattern to prevent higher resolution
            // video or the distance between the camera and the test pattern from impacting the test
            if (error < REPROJECTION_THRESHOLD_RATIO * avgPixelDist) {
                double [] rv = new double[3];
                double timestamp;

                rvec.get(0,0, rv);
                if (use_timestamp) {
                    timestamp = (double)ts[0] / 1e6;
                } else {
                    timestamp = (double) i / meta.fps;
                }
                if (LOCAL_LOGV) Log.v(TAG, String.format("Added frame %d  ts = %f", i, timestamp));
                recs.add(new AttitudeRec(timestamp, rodr2rpy(rv)));
            }

            if (OUTPUT_DEBUG_IMAGE) {
                Calib3d.drawChessboardCorners(frame, patternSize, reprojCenters, true);
                Imgcodecs.imwrite(Environment.getExternalStorageDirectory().getPath()
                        + "/RVCVRecData/DebugCV/img" + i + ".png", frame);
            }
        }

        if (LOCAL_LOGV) {
            Log.v(TAG, "Finished decoding");
        }

        if (TRACE_VIDEO_ANALYSIS) {
            Debug.stopMethodTracing();
        }

        if (LOCAL_LOGV) {
            // time analysis
            double totalTime = (System.nanoTime()-startTime)/1e9;
            Log.i(TAG, "Total time: "+totalTime +"s, Per frame time: "+totalTime/i );
        }
        return i;
    }

    /**
     * OpenCV for Android have not support the VideoCapture from file
     * This is a make shift solution before it is supported.
     * One issue right now is that the glReadPixels is quite slow .. around 6.5ms for a 720p frame
     */
    private class VideoDecoderForOpenCV implements Runnable {
        static final String TAG = "VideoDecoderForOpenCV";

        private MediaExtractor extractor=null;
        private MediaCodec decoder=null;
        private CtsMediaOutputSurface surface=null;

        private MatBuffer mMatBuffer;

        private final File mVideoFile;

        private boolean valid;
        private Object setupSignal;

        private Thread mThread;
        private int mDecimation;

        /**
         * Constructor
         * @param file video file
         * @param decimation process every "decimation" number of frame
         */
        VideoDecoderForOpenCV(File file, int decimation) {
            mVideoFile = file;
            mDecimation = decimation;
            valid = false;

            start();
        }

        /**
         * Constructor
         * @param file video file
         */
        VideoDecoderForOpenCV(File file)   {
            this(file, 1);
        }

        /**
         * Test if video decoder is in valid states ready to output video.
         * @return true of force.
         */
        public boolean isValid() {
            return valid;
        }

        private void start() {
            setupSignal = new Object();
            mThread = new Thread(this);
            mThread.start();

            synchronized (setupSignal) {
                try {
                    setupSignal.wait();
                } catch (InterruptedException e) {
                    Log.e(TAG, "Interrupted when waiting for video decoder setup ready");
                }
            }
        }
        private void stop() {
            if (mThread != null) {
                mThread.interrupt();
                try {
                    mThread.join();
                } catch (InterruptedException e) {
                    Log.e(TAG, "Interrupted when waiting for video decoder thread to stop");
                }
                try {
                    decoder.stop();
                }catch (IllegalStateException e) {
                    Log.e(TAG, "Video decoder is not in a state that can be stopped");
                }
            }
            mThread = null;
        }

        void teardown() {
            if (decoder!=null) {
                decoder.release();
                decoder = null;
            }
            if (surface!=null) {
                surface.release();
                surface = null;
            }
            if (extractor!=null) {
                extractor.release();
                extractor = null;
            }
        }

        void setup() {
            int width=0, height=0;

            extractor = new MediaExtractor();

            try {
                extractor.setDataSource(mVideoFile.getPath());
            } catch (IOException e) {
                return;
            }

            for (int i = 0; i < extractor.getTrackCount(); i++) {
                MediaFormat format = extractor.getTrackFormat(i);
                String mime = format.getString(MediaFormat.KEY_MIME);
                width = format.getInteger(MediaFormat.KEY_WIDTH);
                height = format.getInteger(MediaFormat.KEY_HEIGHT);

                if (mime.startsWith("video/")) {
                    extractor.selectTrack(i);
                    try {
                        decoder = MediaCodec.createDecoderByType(mime);
                    }catch (IOException e) {
                        continue;
                    }
                    // Decode to surface
                    //decoder.configure(format, surface, null, 0);

                    // Decode to offscreen surface
                    surface = new CtsMediaOutputSurface(width, height);
                    mMatBuffer = new MatBuffer(width, height);

                    decoder.configure(format, surface.getSurface(), null, 0);
                    break;
                }
            }

            if (decoder == null) {
                Log.e(TAG, "Can't find video info!");
                return;
            }
            valid = true;
        }

        @Override
        public void run() {
            setup();

            synchronized (setupSignal) {
                setupSignal.notify();
            }

            if (!valid) {
                return;
            }

            decoder.start();

            ByteBuffer[] inputBuffers = decoder.getInputBuffers();
            ByteBuffer[] outputBuffers = decoder.getOutputBuffers();
            MediaCodec.BufferInfo info = new MediaCodec.BufferInfo();

            boolean isEOS = false;
            long startMs = System.currentTimeMillis();
            long timeoutUs = 10000;

            int iframe = 0;
            long frameTimestamp = 0;

            while (!Thread.interrupted()) {
                if (!isEOS) {
                    int inIndex = decoder.dequeueInputBuffer(10000);
                    if (inIndex >= 0) {
                        ByteBuffer buffer = inputBuffers[inIndex];
                        int sampleSize = extractor.readSampleData(buffer, 0);
                        if (sampleSize < 0) {
                            if (LOCAL_LOGD) {
                                Log.d("VideoDecoderForOpenCV",
                                        "InputBuffer BUFFER_FLAG_END_OF_STREAM");
                            }
                            decoder.queueInputBuffer(inIndex, 0, 0, 0,
                                    MediaCodec.BUFFER_FLAG_END_OF_STREAM);
                            isEOS = true;
                        } else {
                            frameTimestamp = extractor.getSampleTime();
                            decoder.queueInputBuffer(inIndex, 0, sampleSize, frameTimestamp, 0);
                            if (LOCAL_LOGD) {
                                Log.d(TAG, String.format("Frame %d sample time %f s",
                                            iframe, (double)frameTimestamp/1e6));
                            }
                            extractor.advance();
                        }
                    }
                }

                int outIndex = decoder.dequeueOutputBuffer(info, 10000);
                MediaFormat outFormat;
                switch (outIndex) {
                    case MediaCodec.INFO_OUTPUT_BUFFERS_CHANGED:
                        if (LOCAL_LOGD) {
                            Log.d(TAG, "INFO_OUTPUT_BUFFERS_CHANGED");
                        }
                        outputBuffers = decoder.getOutputBuffers();
                        break;
                    case MediaCodec.INFO_OUTPUT_FORMAT_CHANGED:
                        outFormat = decoder.getOutputFormat();
                        if (LOCAL_LOGD) {
                            Log.d(TAG, "New format " + outFormat);
                        }
                        break;
                    case MediaCodec.INFO_TRY_AGAIN_LATER:
                        if (LOCAL_LOGD) {
                            Log.d(TAG, "dequeueOutputBuffer timed out!");
                        }
                        break;
                    default:

                        ByteBuffer buffer = outputBuffers[outIndex];
                        boolean doRender = (info.size != 0);

                        // As soon as we call releaseOutputBuffer, the buffer will be forwarded
                        // to SurfaceTexture to convert to a texture.  The API doesn't
                        // guarantee that the texture will be available before the call
                        // returns, so we need to wait for the onFrameAvailable callback to
                        // fire.  If we don't wait, we risk rendering from the previous frame.
                        decoder.releaseOutputBuffer(outIndex, doRender);

                        if (doRender) {
                            surface.awaitNewImage();
                            surface.drawImage();
                            if (LOCAL_LOGV) {
                                Log.v(TAG, "Finish drawing a frame!");
                            }
                            if ((iframe++ % mDecimation) == 0) {
                                //Send the frame for processing
                                mMatBuffer.put(frameTimestamp);
                            }
                        }
                        break;
                }

                if ((info.flags & MediaCodec.BUFFER_FLAG_END_OF_STREAM) != 0) {
                    if (LOCAL_LOGD) {
                        Log.d("VideoDecoderForOpenCV", "OutputBuffer BUFFER_FLAG_END_OF_STREAM");
                    }
                    break;
                }
            }
            mMatBuffer.invalidate();

            decoder.stop();

            teardown();
            mThread = null;
        }


        /**
         * Get next valid frame
         * @return Frame in OpenCV mat
         */
        public Mat getFrame(long ts[]) {
            return mMatBuffer.get(ts);
        }

        /**
         * Get the size of the frame
         * @return size of the frame
         */
        Size getSize() {
            return mMatBuffer.getSize();
        }

        /**
         * A synchronized buffer
         */
        class MatBuffer {
            private Mat mat;
            private byte[] bytes;
            private ByteBuffer buf;
            private long timestamp;
            private boolean full;

            private int mWidth, mHeight;
            private boolean mValid = false;

            MatBuffer(int width, int height) {
                mWidth = width;
                mHeight = height;

                mat = new Mat(height, width, CvType.CV_8UC4); //RGBA
                buf = ByteBuffer.allocateDirect(width*height*4);
                bytes = new byte[width*height*4];
                timestamp = -1;

                mValid = true;
                full = false;
            }

            public synchronized void invalidate() {
                mValid = false;
                notifyAll();
            }

            public synchronized Mat get(long ts[]) {

                if (!mValid) return null;
                while (full == false) {
                    try {
                        wait();
                        if (!mValid) return null;
                    } catch (InterruptedException e) {
                        return null;
                    }
                }
                mat.put(0,0, bytes);
                full = false;
                notifyAll();
                ts[0] = timestamp;
                return mat;
            }

            public synchronized void put(long ts) {
                while (full) {
                    try {
                        wait();
                    } catch (InterruptedException e) {
                        Log.e(TAG, "Interrupted when waiting for space in buffer");
                    }
                }
                GLES20.glReadPixels(0, 0, mWidth, mHeight, GL10.GL_RGBA,
                        GL10.GL_UNSIGNED_BYTE, buf);
                buf.get(bytes);
                buf.rewind();

                timestamp = ts;
                full = true;
                notifyAll();
            }

            public Size getSize() {
                if (valid) {
                    return mat.size();
                }
                return new Size();
            }
        }
    }


    /* a small set of math functions */
    private static double [] quat2rpy( double [] q) {
        double [] rpy = {Math.atan2(2*(q[0]*q[1]+q[2]*q[3]), 1-2*(q[1]*q[1]+q[2]*q[2])),
                Math.asin(2*(q[0]*q[2] - q[3]*q[1])),
                Math.atan2(2*(q[0]*q[3]+q[1]*q[2]), 1-2*(q[2]*q[2]+q[3]*q[3]))};
        return rpy;
    }

    private static void quat2rpy( double [] q, double[] rpy) {
        rpy[0] = Math.atan2(2*(q[0]*q[1]+q[2]*q[3]), 1-2*(q[1]*q[1]+q[2]*q[2]));
        rpy[1] = Math.asin(2*(q[0]*q[2] - q[3]*q[1]));
        rpy[2] = Math.atan2(2*(q[0]*q[3]+q[1]*q[2]), 1-2*(q[2]*q[2]+q[3]*q[3]));
    }

    private static Mat quat2rpy(Mat quat) {
        double [] q = new double[4];
        quat.get(0,0,q);

        double [] rpy = {Math.atan2(2*(q[0]*q[1]+q[2]*q[3]), 1-2*(q[1]*q[1]+q[2]*q[2])),
                Math.asin(2*(q[0]*q[2] - q[3]*q[1])),
                Math.atan2(2*(q[0]*q[3]+q[1]*q[2]), 1-2*(q[2]*q[2]+q[3]*q[3]))};

        Mat rpym = new Mat(3,1, CvType.CV_64F);
        rpym.put(0,0, rpy);
        return rpym;
    }

    private static double [] rodr2quat( double [] r) {
        double t = Math.sqrt(r[0]*r[0]+r[1]*r[1]+r[2]*r[2]);
        double [] quat = {Math.cos(t/2), Math.sin(t/2)*r[0]/t,Math.sin(t/2)*r[1]/t,
                Math.sin(t/2)*r[2]/t};
        return quat;
    }

    private static void rodr2quat( double [] r, double [] quat) {
        double t = Math.sqrt(r[0]*r[0]+r[1]*r[1]+r[2]*r[2]);
        quat[0] = Math.cos(t/2);
        quat[1] = Math.sin(t/2)*r[0]/t;
        quat[2] = Math.sin(t/2)*r[1]/t;
        quat[3] = Math.sin(t/2)*r[2]/t;
    }

    private static Mat rodr2quat(Mat rodr) {
        double t = Core.norm(rodr);
        double [] r = new double[3];
        rodr.get(0,0,r);

        double [] quat = {Math.cos(t/2), Math.sin(t/2)*r[0]/t,Math.sin(t/2)*r[1]/t,
                Math.sin(t/2)*r[2]/t};
        Mat quatm = new Mat(4,1, CvType.CV_64F);
        quatm.put(0, 0, quat);
        return quatm;
    }

    private static double [] rodr2rpy( double [] r) {
        return quat2rpy(rodr2quat(r));
    }

    private double getDistanceBetweenPoints(Point a, Point b) {
        return Math.sqrt(Math.pow(a.x - b.x, 2) + Math.pow(a.y - b.y, 2));
    }
    //////////////////

}