//================================================================================================================================
//
// Copyright (c) 2015-2022 VisionStar Information Technology (Shanghai) Co., Ltd. All Rights Reserved.
// EasyAR is the registered trademark or trademark of VisionStar Information Technology (Shanghai) Co., Ltd in China
// and other countries for the augmented reality technology developed by VisionStar Information Technology (Shanghai) Co., Ltd.
//
//================================================================================================================================

package cn.easyar.samples.helloarcustomcamera;

import android.annotation.SuppressLint;
import android.annotation.TargetApi;
import android.content.Context;
import android.graphics.*;
import android.hardware.camera2.*;
import android.media.Image;
import android.media.ImageReader;
import android.os.Handler;
import android.os.HandlerThread;
import android.os.SystemClock;
import android.util.Log;
import android.util.Range;
import android.util.Size;

import java.nio.ByteBuffer;
import java.util.Arrays;
import java.util.Collections;
import java.util.LinkedList;
import java.util.Queue;
import java.util.concurrent.locks.Lock;
import java.util.concurrent.locks.ReentrantLock;
import java.util.concurrent.locks.Condition;

import cn.easyar.CameraDeviceFocusMode;
import cn.easyar.CameraDeviceType;
import cn.easyar.CameraParameters;
import cn.easyar.FunctorOfVoid;
import cn.easyar.FunctorOfVoidFromInputFrame;
import cn.easyar.InputFrame;
import cn.easyar.JniUtility;
import cn.easyar.PixelFormat;
import cn.easyar.Vec2I;

@TargetApi(21)
class CameraDeviceCamera2 implements CameraDevice {
    private static final String TAG = "EasyAR";

    private final Context mContext;
    private final FunctorOfVoidFromInputFrame mInputFrameHandler;
    private CameraParameters mCameraParameters;
    private int mBufferCapacity = 8;
    private int mCameraIndex = -1;
    private String mCameraId;
    private boolean firstOpen = true;
    private int mCameraDeviceType = CameraDeviceType.Unknown;
    private int mCameraOrientation = 0;

    private boolean mIsTimeStampSourceRealTime = true;
    private boolean mIsTimeStampSystemRealTime = false;
    private long mTimeStampOffset = 0;
    private long mL1;
    private CameraCharacteristics mCharacteristics;
    private Size[] mSupportedSizes;
    private Rect mCameraActiveSize;
    private Range<Integer>[] mSupportedFpsRanges;
    private int mCurrentFpsRange = -1;
    private int mCurrentFocusMode = CameraDeviceFocusMode.Normal;
    private boolean mFlashSupported = false;
    private boolean mFlashEnabled = false;

    private HandlerThread mBackgroundThread;
    private Handler mBackgroundHandler;
    private android.hardware.camera2.CameraDevice mCameraDevice;
    private AutoResetEvent mCameraDeviceCloseSignal;
    private CameraCaptureSession mCaptureSession;
    private ImageReader mImageReader;

    private ByteArrayPool mByteArrayPool;

    private boolean mIsStarted = false;

    private Size mCameraSize;

    public CameraDeviceCamera2(Context context, FunctorOfVoidFromInputFrame inputFrameHandler) {
        this.mContext = context;
        this.mInputFrameHandler = inputFrameHandler;
        this.mCameraParameters = CameraParameters.createWithDefaultIntrinsics(new Vec2I(640, 480), CameraDeviceType.Unknown, 0);
    }

    @Override
    public int getBufferCapacity() {
        return mBufferCapacity;
    }

    @Override
    public void setBufferCapacity(int capacity) {
        mBufferCapacity = capacity;
    }

    @SuppressLint("MissingPermission")
    @Override
    public boolean openWithIndex(int index) {
        close();

        boolean success = false;

        try {
            CameraManager manager = (CameraManager) mContext.getSystemService(Context.CAMERA_SERVICE);
            if (manager == null) {
                return false;
            }

            try {
                String[] ids = manager.getCameraIdList();
                if ((index < 0) || (index >= ids.length)) { return false; }
                String cameraId = ids[index];
                CameraCharacteristics characteristics = manager.getCameraCharacteristics(cameraId);
                mCameraId = cameraId;
                mCameraIndex = index;
                mCharacteristics = characteristics;
            } catch (CameraAccessException e) {
                Log.e(TAG, "Camera2: \n" + Log.getStackTraceString(e));
                return false;
            } catch (Exception e) {
                Log.e(TAG, "Camera2: \n" + Log.getStackTraceString(e));
                return false;
            }

            mIsTimeStampSourceRealTime = mCharacteristics.get(CameraCharacteristics.SENSOR_INFO_TIMESTAMP_SOURCE) == CameraCharacteristics.SENSOR_INFO_TIMESTAMP_SOURCE_REALTIME;
            if (firstOpen && !mIsTimeStampSourceRealTime) {
                firstOpen = false;
                Log.w(TAG, "Camera2: Timestamp is not precise.");
            }

            int facing = mCharacteristics.get(CameraCharacteristics.LENS_FACING);
            mCameraDeviceType = CameraDeviceType.Unknown;
            if (facing == CameraCharacteristics.LENS_FACING_BACK) {
                mCameraDeviceType = CameraDeviceType.Back;
            } else if (facing == CameraCharacteristics.LENS_FACING_FRONT) {
                mCameraDeviceType = CameraDeviceType.Front;
            }

            mCameraOrientation = mCharacteristics.get(CameraCharacteristics.SENSOR_ORIENTATION);
            mSupportedSizes = mCharacteristics.get(CameraCharacteristics.SCALER_STREAM_CONFIGURATION_MAP).getOutputSizes(ImageFormat.YUV_420_888);
            mCameraActiveSize = mCharacteristics.get(CameraCharacteristics.SENSOR_INFO_ACTIVE_ARRAY_SIZE);
            mSupportedFpsRanges = mCharacteristics.get(CameraCharacteristics.CONTROL_AE_AVAILABLE_TARGET_FPS_RANGES);
            mFlashSupported = mCharacteristics.get(CameraCharacteristics.FLASH_INFO_AVAILABLE);

            if (mCameraSize == null) {
                mCameraSize = getOptimalPreviewSize(1280, 720);
            }

            if ((mCurrentFpsRange < 0) || (mCurrentFpsRange >= mSupportedFpsRanges.length)) {
                mCurrentFpsRange = mSupportedFpsRanges.length - 1;
                int i = 0;
                for (Range<Integer> range : mSupportedFpsRanges) {
                    if ((range.getLower() == 30) && (range.getUpper() == 30)) {
                        mCurrentFpsRange = i;
                    }
                    i += 1;
                }
            }

            mBackgroundThread = new HandlerThread("CameraBackground");
            mBackgroundThread.start();
            mBackgroundHandler = new Handler(mBackgroundThread.getLooper());

            final AutoResetEvent signal = new AutoResetEvent();

            try {
                manager.openCamera(mCameraId, new android.hardware.camera2.CameraDevice.StateCallback() {
                    @Override
                    public void onOpened(android.hardware.camera2.CameraDevice cameraDevice) {
                        mCameraDevice = cameraDevice;
                        mCameraDeviceCloseSignal = new AutoResetEvent();
                        signal.set();
                    }

                    @Override
                    public void onDisconnected(android.hardware.camera2.CameraDevice cameraDevice) {
                        cameraDevice.close();
                        signal.set();
                    }

                    @Override
                    public void onError(android.hardware.camera2.CameraDevice cameraDevice, int error) {
                        cameraDevice.close();
                        signal.set();
                        Log.e(TAG, "Camera2: CameraManager.openCamera onError(" + error + ")\n");
                    }

                    @Override
                    public void onClosed(android.hardware.camera2.CameraDevice camera) {
                        mCameraDeviceCloseSignal.set();
                    }
                }, mBackgroundHandler);
            } catch (CameraAccessException e) {
                Log.e(TAG, "Camera2: \n" + Log.getStackTraceString(e));
                return false;
            } catch (Exception e) {
                Log.e(TAG, "Camera2: \n" + Log.getStackTraceString(e));
                return false;
            }

            signal.waitOne();

            if (mCameraDevice == null) {
                return false;
            }

            if (mCameraParameters != null) {
                mCameraParameters.dispose();
            }
            mCameraParameters = CameraParameters.createWithDefaultIntrinsics(new Vec2I(640, 480), mCameraDeviceType, mCameraOrientation);

            success = true;
        } finally {
            if (!success) {
                close();
            }
        }

        return success;
    }

    @Override
    public boolean openWithSpecificType(int type) {
        CameraManager manager = (CameraManager) mContext.getSystemService(Context.CAMERA_SERVICE);
        if (manager == null) {
            return false;
        }

        try {
            String[] ids = manager.getCameraIdList();
            for (int k = 0; k < ids.length; k += 1) {
                String cameraId = ids[k];
                CameraCharacteristics characteristics = manager.getCameraCharacteristics(cameraId);

                Integer facing = characteristics.get(CameraCharacteristics.LENS_FACING);
                if ((type == CameraDeviceType.Back) && (facing == CameraCharacteristics.LENS_FACING_BACK)) {
                    return openWithIndex(k);
                }
                if ((type == CameraDeviceType.Front) && (facing == CameraCharacteristics.LENS_FACING_FRONT)) {
                    return openWithIndex(k);
                }
                if (type == CameraDeviceType.Unknown) {
                    return openWithIndex(k);
                }
            }
            return false;
        } catch (CameraAccessException e) {
            Log.e(TAG, "Camera2: \n" + Log.getStackTraceString(e));
            return false;
        } catch (Exception e) {
            Log.e(TAG, "Camera2: \n" + Log.getStackTraceString(e));
            return false;
        }
    }

    @Override
    public void close() {
        stop();
        if (mCameraParameters != null) {
            mCameraParameters.dispose();
            mCameraParameters = null;
        }
    }

    @Override
    public boolean start() {
        if ((mBackgroundThread == null) || (mBackgroundHandler == null) || (mCameraDevice == null)) {
            if (mContext == null) {
                return false;
            } else if (!openWithIndex(mCameraIndex)) {
                return false;
            }
        }

        if (mIsStarted) {
            return true;
        }

        if (mCameraSize == null) {
            return false;
        }

        mImageReader = ImageReader.newInstance(mCameraSize.getWidth(), mCameraSize.getHeight(), ImageFormat.YUV_420_888, 2);
        mImageReader.setOnImageAvailableListener(new ImageReader.OnImageAvailableListener() {
            private boolean notSupportedFired = false;

            @Override
            public void onImageAvailable(ImageReader reader) {
                Image image = reader.acquireLatestImage();
                if (image == null) { return; }
                try {
                    Image.Plane[] planes = image.getPlanes();
                    if (planes.length != 3) {
                        if (!notSupportedFired) {
                            Log.e(TAG, "Camera2: Not supported image format.");
                            notSupportedFired = true;
                        }
                        return;
                    }
                    int width = image.getWidth();
                    int height = image.getHeight();
                    long timestamp = image.getTimestamp();
                    Image.Plane planeY = planes[0];
                    Image.Plane planeU = planes[1];
                    Image.Plane planeV = planes[2];
                    ByteBuffer dataY = planeY.getBuffer();
                    ByteBuffer dataU = planeU.getBuffer();
                    ByteBuffer dataV = planeV.getBuffer();
                    long rawByteY = getDirectBufferAddress(dataY);
                    long rawByteU = getDirectBufferAddress(dataU);
                    long rawByteV = getDirectBufferAddress(dataV);
                    int pixelStrideY = planeY.getPixelStride();
                    int pixelStrideU = planeU.getPixelStride();
                    int pixelStrideV = planeV.getPixelStride();
                    int rowStrideY = planeY.getRowStride();
                    int rowStrideU = planeU.getRowStride();
                    int rowStrideV = planeV.getRowStride();
                    int pixelFormat = CameraDeviceCamera2.checkPixelFormat(dataY, dataU, dataV, rawByteY, rawByteU, rawByteV, pixelStrideY, pixelStrideU, pixelStrideV, rowStrideY, rowStrideU, rowStrideV);
                    if (pixelFormat == PixelFormat.Unknown) {
                        if (!notSupportedFired) {
                            Log.e(TAG, "Camera2: Not supported image format.");
                            notSupportedFired = true;
                        }
                        return;
                    }

                    int pixelWidth = rowStrideY;
                    int pixelHeight = (int)((Math.min(rawByteU, rawByteV) - rawByteY) / rowStrideY);

                    int bufferBlockSize = pixelWidth * pixelHeight * 3 / 2;
                    if ((mByteArrayPool == null) || (mByteArrayPool.getBlockSize() != bufferBlockSize) || (mByteArrayPool.getCapacity() != mBufferCapacity)) {
                        mByteArrayPool = new ByteArrayPool(bufferBlockSize, mBufferCapacity);
                    }
                    ByteArrayPair p = mByteArrayPool.tryAcquire();
                    if (p == null) {
                        return;
                    }
                    byte[] imageBuffer = p.array;
                    fillImageBuffer(dataY, dataU, dataV, pixelWidth, pixelHeight, pixelFormat, imageBuffer);
                    cn.easyar.Buffer b = p.buffer;
                    cn.easyar.Image i = null;
                    InputFrame inputFrame = null;
                    CameraParameters camParams = null;
                    try {
                        i = cn.easyar.Image.create(b, pixelFormat, width, height, pixelWidth, pixelHeight);
                        long timestampWithOffset = timestamp + mTimeStampOffset + mL1;
                        double double_timestamp = timestampWithOffset == 0 ? SystemClock.elapsedRealtimeNanos() * 1e-9 : timestampWithOffset * 1e-9;
                        camParams = mCameraParameters.getResized(new Vec2I(width, height));
                        inputFrame = InputFrame.createWithImageAndCameraParametersAndTemporal(i, camParams, double_timestamp);
                        mInputFrameHandler.invoke(inputFrame);
                    } finally {
                        if (camParams != null) { camParams.dispose(); }
                        if (inputFrame != null) { inputFrame.dispose(); }
                        if (i != null) { i.dispose(); }
                        if (b != null) { b.dispose(); }
                    }
                } catch (IllegalStateException e) {
                } finally {
                    image.close();
                }
            }
        }, mBackgroundHandler);

        final AutoResetEvent signal = new AutoResetEvent();

        try {
            mCameraDevice.createCaptureSession(Collections.singletonList(mImageReader.getSurface()),
                new CameraCaptureSession.StateCallback() {
                    @Override
                    public void onConfigured(CameraCaptureSession cameraCaptureSession) {
                        mCaptureSession = cameraCaptureSession;
                        signal.set();
                    }

                    @Override
                    public void onConfigureFailed(CameraCaptureSession cameraCaptureSession) {
                        Log.e(TAG, "Camera2: createCaptureSession onConfigureFailed!");
                        signal.set();
                    }
                }, mBackgroundHandler
            );
        } catch (CameraAccessException e) {
            Log.e(TAG, "Camera2: \n" + Log.getStackTraceString(e));
            return false;
        } catch (Exception e) {
            Log.e(TAG, "Camera2: \n" + Log.getStackTraceString(e));
            return false;
        }

        signal.waitOne();

        if (mCaptureSession == null) {
            close();
            return false;
        }

        if (!runRequest()) {
            close();
            return false;
        }

        mIsStarted = true;

        return true;
    }

    @Override
    public void stop() {
        mIsStarted = false;
        if (mCaptureSession != null) {
            mCaptureSession.close();
            mCaptureSession = null;
        }
        if (mCameraDevice != null) {
            mCameraDevice.close();
            mCameraDeviceCloseSignal.waitOne();
            mCameraDeviceCloseSignal = null;
            mCameraDevice = null;
        }
        if (mImageReader != null) {
            mImageReader.close();
            mImageReader = null;
        }
        if (mBackgroundThread != null) {
            mBackgroundThread.quitSafely();
            try {
                mBackgroundThread.join();
            } catch (InterruptedException e) {
            }
            mBackgroundThread = null;
            mBackgroundHandler = null;
        }
    }

    @Override
    public int getIndex() {
        return mCameraIndex;
    }

    @Override
    public int getType() {
        return mCameraDeviceType;
    }

    @Override
    public int getCameraOrientation() {
        return mCameraOrientation;
    }

    @Override
    public CameraParameters getCameraParameters() {
        return new CameraParameters(mCameraParameters.size(), mCameraParameters.focalLength(), mCameraParameters.principalPoint(), mCameraParameters.cameraDeviceType(), mCameraParameters.cameraOrientation());
    }

    @Override
    public void setCameraParameters(CameraParameters cameraParameters) {
        if (cameraParameters == null) { return; }
        if (mCameraParameters != null) {
            mCameraParameters.dispose();
        }
        mCameraParameters = new CameraParameters(cameraParameters.size(), cameraParameters.focalLength(), cameraParameters.principalPoint(), cameraParameters.cameraDeviceType(), cameraParameters.cameraOrientation());
    }

    @Override
    public int getSizeWidth() {
        if (mCameraSize == null) { return 0; }
        return mCameraSize.getWidth();
    }

    @Override
    public int getSizeHeight() {
        if (mCameraSize == null) { return 0; }
        return mCameraSize.getHeight();
    }

    @Override
    public boolean setSize(int width, int height) {
        if (mSupportedSizes == null) { return false; }
        mCameraSize = getOptimalPreviewSize(width, height);
        if (mIsStarted) {
            close();
            start();
        }
        return true;
    }

    @Override
    public int getNumSupportedSize() {
        if (mSupportedSizes == null) { return 0; }
        return mSupportedSizes.length;
    }

    @Override
    public int getSupportedSizeWidth(int index) {
        if ((mSupportedSizes == null) || ((index < 0) || (index >= mSupportedSizes.length))) {
            return 0;
        }
        return mSupportedSizes[index].getWidth();
    }

    @Override
    public int getSupportedSizeHeight(int index) {
        if ((mSupportedSizes == null) || ((index < 0) || (index >= mSupportedSizes.length))) {
            return 0;
        }
        return mSupportedSizes[index].getHeight();
    }

    @Override
    public int getNumSupportedFrameRateRange() {
        if (mSupportedFpsRanges == null) { return 0; }
        return mSupportedFpsRanges.length;
    }

    @Override
    public float getSupportedFrameRateRangeLower(int index) {
        if (mSupportedFpsRanges == null) { return 0; }
        if ((index < 0) || (index >= mSupportedFpsRanges.length)) { return 0; }
        return mSupportedFpsRanges[index].getLower();
    }

    @Override
    public float getSupportedFrameRateRangeUpper(int index) {
        if (mSupportedFpsRanges == null) { return 0; }
        if ((index < 0) || (index >= mSupportedFpsRanges.length)) { return 0; }
        return mSupportedFpsRanges[index].getUpper();
    }

    @Override
    public int getFrameRateRange() {
        if (mSupportedFpsRanges == null) { return -1; }
        return mCurrentFpsRange;
    }

    @Override
    public boolean setFrameRateRange(int index) {
        if (mSupportedFpsRanges == null) { return false; }
        if ((index < 0) || (index >= mSupportedFpsRanges.length)) { return false; }
        mCurrentFpsRange = index;
        if (mIsStarted) {
            runRequest();
        }
        return true;
    }

    @Override
    public boolean setFlashTorchMode(boolean on) {
        if (!mFlashSupported) {
            return false;
        }
        mFlashEnabled = on;
        if (mIsStarted) {
            runRequest();
        }
        return true;
    }

    @Override
    public boolean setFocusMode(int focusMode) {
        boolean focusModeSupport = true;
        if (mCharacteristics == null) { return false; }
        if (focusMode == CameraDeviceFocusMode.Normal) {
        } else if (focusMode == CameraDeviceFocusMode.Continousauto) {
            if (contains(mCharacteristics.get(CameraCharacteristics.REQUEST_AVAILABLE_CAPABILITIES), CameraMetadata.CONTROL_AF_MODE_CONTINUOUS_PICTURE)) {
            } else if (contains(mCharacteristics.get(CameraCharacteristics.REQUEST_AVAILABLE_CAPABILITIES), CameraMetadata.CONTROL_AF_MODE_CONTINUOUS_VIDEO)) {
            } else {
                Log.e(TAG, String.format("Camera2: FocusMode.Continousauto not supported"));
                focusModeSupport = false;
            }
        } else if (focusMode == CameraDeviceFocusMode.Infinity) {
            if (!contains(mCharacteristics.get(CameraCharacteristics.REQUEST_AVAILABLE_CAPABILITIES), CameraMetadata.REQUEST_AVAILABLE_CAPABILITIES_MANUAL_SENSOR)) {
                Log.e(TAG, String.format("Camera2: FocusMode.Infinity not supported: manual sensor not available"));
                focusModeSupport = false;
            }
        } else if (focusMode == CameraDeviceFocusMode.Macro) {
        } else if (focusMode == CameraDeviceFocusMode.Medium) {
            if (!contains(mCharacteristics.get(CameraCharacteristics.REQUEST_AVAILABLE_CAPABILITIES), CameraMetadata.REQUEST_AVAILABLE_CAPABILITIES_MANUAL_SENSOR)) {
                Log.e(TAG, String.format("Camera2: FocusMode.Medium not supported: manual sensor not available"));
                focusModeSupport = false;
            }
        } else {
            return false;
        }
        mCurrentFocusMode = focusMode;
        if (mIsStarted) {
            runRequest();
        }
        return focusModeSupport;
    }

    @Override
    public boolean autoFocus() {
        runRequest(false);
        runRequest(true);
        return true;
    }

    private boolean runRequest() {
        return runRequest(true);
    }
    private boolean runRequest(boolean repeating) {
        if (mCameraDevice == null) {
            return false;
        }
        if (mCaptureSession == null) {
            return false;
        }

        CaptureRequest.Builder builder;
        try {
            builder = mCameraDevice.createCaptureRequest(android.hardware.camera2.CameraDevice.TEMPLATE_PREVIEW);
        } catch (CameraAccessException e) {
            Log.e(TAG, "Camera2: \n" + Log.getStackTraceString(e));
            return false;
        } catch (Exception e) {
            Log.e(TAG, "Camera2: \n" + Log.getStackTraceString(e));
            return false;
        }

        builder.addTarget(mImageReader.getSurface());

        int newWidth = (int)(Math.round(Math.ceil(mCameraActiveSize.width() / 8))) * 8;
        int newHeight = (int)(Math.round(Math.ceil(mCameraActiveSize.height() / 8))) * 8;
        int left = mCameraActiveSize.left + (mCameraActiveSize.width() - newWidth) / 2;
        int top = mCameraActiveSize.top + (mCameraActiveSize.height() - newHeight) / 2;
        int right = left + newWidth;
        int bottom = top + newHeight;
        builder.set(CaptureRequest.SCALER_CROP_REGION, new Rect(left, top, right, bottom));

        builder.set(CaptureRequest.CONTROL_AE_MODE, Integer.valueOf(1));
        builder.set(CaptureRequest.STATISTICS_FACE_DETECT_MODE, Integer.valueOf(0));
        builder.set(CaptureRequest.CONTROL_VIDEO_STABILIZATION_MODE, Integer.valueOf(0));
        builder.set(CaptureRequest.LENS_OPTICAL_STABILIZATION_MODE, Integer.valueOf(0));
        builder.set(CaptureRequest.CONTROL_AE_TARGET_FPS_RANGE, mSupportedFpsRanges[mCurrentFpsRange]);
        if (mFlashEnabled) {
            //CONTROL_AE_MODE must be ON or OFF
            builder.set(CaptureRequest.FLASH_MODE, CaptureRequest.FLASH_MODE_TORCH);
        }

        builder.set(CaptureRequest.CONTROL_MODE, CameraMetadata.CONTROL_MODE_AUTO);
        if (mCurrentFocusMode == CameraDeviceFocusMode.Normal) {
            builder.set(CaptureRequest.CONTROL_AF_MODE, CameraMetadata.CONTROL_AF_MODE_AUTO);
            if (!repeating) {
                //Generally, applications should set this entry to START or CANCEL for only a single capture, and then return it to IDLE (or not set at all). Specifying START for multiple captures in a row means restarting the AF operation over and over again.
                builder.set(CaptureRequest.CONTROL_AF_TRIGGER, CameraMetadata.CONTROL_AF_TRIGGER_START);
            }
        } else if (mCurrentFocusMode == CameraDeviceFocusMode.Continousauto) {
            if (contains(mCharacteristics.get(CameraCharacteristics.REQUEST_AVAILABLE_CAPABILITIES), CameraMetadata.CONTROL_AF_MODE_CONTINUOUS_PICTURE)) {
                builder.set(CaptureRequest.CONTROL_AF_MODE, CameraMetadata.CONTROL_AF_MODE_CONTINUOUS_PICTURE);
            } else if (contains(mCharacteristics.get(CameraCharacteristics.REQUEST_AVAILABLE_CAPABILITIES), CameraMetadata.CONTROL_AF_MODE_CONTINUOUS_VIDEO)) {
                builder.set(CaptureRequest.CONTROL_AF_MODE, CameraMetadata.CONTROL_AF_MODE_CONTINUOUS_VIDEO);
            } else {
                builder.set(CaptureRequest.CONTROL_AF_MODE, CameraMetadata.CONTROL_AF_MODE_AUTO);
            }
        } else if (mCurrentFocusMode == CameraDeviceFocusMode.Infinity) {
            builder.set(CaptureRequest.CONTROL_AF_MODE, CameraMetadata.CONTROL_AF_MODE_OFF);
            if (contains(mCharacteristics.get(CameraCharacteristics.REQUEST_AVAILABLE_CAPABILITIES), CameraMetadata.REQUEST_AVAILABLE_CAPABILITIES_MANUAL_SENSOR)) {
                builder.set(CaptureRequest.LENS_FOCUS_DISTANCE, 0.0f);
            }
        } else if (mCurrentFocusMode == CameraDeviceFocusMode.Macro) {
            builder.set(CaptureRequest.CONTROL_AF_MODE, CameraMetadata.CONTROL_AF_MODE_MACRO);
            if (!repeating) {
                //Generally, applications should set this entry to START or CANCEL for only a single capture, and then return it to IDLE (or not set at all). Specifying START for multiple captures in a row means restarting the AF operation over and over again.
                builder.set(CaptureRequest.CONTROL_AF_TRIGGER, CameraMetadata.CONTROL_AF_TRIGGER_START);
            }
        } else if (mCurrentFocusMode == CameraDeviceFocusMode.Medium) {
            builder.set(CaptureRequest.CONTROL_AF_MODE, CameraMetadata.CONTROL_AF_MODE_OFF);
            if (contains(mCharacteristics.get(CameraCharacteristics.REQUEST_AVAILABLE_CAPABILITIES), CameraMetadata.REQUEST_AVAILABLE_CAPABILITIES_MANUAL_SENSOR)) {
                builder.set(CaptureRequest.LENS_FOCUS_DISTANCE, 0.6f);
            }
        } else {
            return false;
        }

        try {
            CameraCaptureSession.CaptureCallback callback = new CameraCaptureSession.CaptureCallback() {
                @Override
                public void onCaptureStarted(CameraCaptureSession session, CaptureRequest request, long timestamp, long frameNumber) {
                    if (!mIsTimeStampSourceRealTime && mTimeStampOffset == 0 && !mIsTimeStampSystemRealTime) {
                        mTimeStampOffset = SystemClock.elapsedRealtimeNanos() - timestamp;
                        //Does not replace system time within 1s
                        if(Math.abs(mTimeStampOffset)<1000000000){
                            mTimeStampOffset=0;
                            mIsTimeStampSystemRealTime=true;
                        }
                    }
                }

                @Override
                public void onCaptureProgressed(CameraCaptureSession session, CaptureRequest request, CaptureResult partialResult) {
                }

                @Override
                public void onCaptureCompleted(CameraCaptureSession session, CaptureRequest request, TotalCaptureResult result) {
                    long SKEW =0;
                    if(result.get(CaptureResult.SENSOR_ROLLING_SHUTTER_SKEW)!=null)
                        SKEW = result.get(CaptureResult.SENSOR_ROLLING_SHUTTER_SKEW);
                    long EXPOSURE_TIME = 0;
                    if(result.get(CaptureResult.SENSOR_EXPOSURE_TIME)!=null)
                        EXPOSURE_TIME = result.get(CaptureResult.SENSOR_EXPOSURE_TIME);
                    mL1=(SKEW+EXPOSURE_TIME)/2;
                    //Log.e(TAG, "SKEW= " + SKEW+" EXPOSURE_TIME= "+EXPOSURE_TIME);
                }
            };
            if (repeating) {
                mCaptureSession.setRepeatingRequest(builder.build(), callback, mBackgroundHandler);
            } else {
                mCaptureSession.capture(builder.build(), callback, mBackgroundHandler);
            }
        } catch (CameraAccessException e) {
            Log.e(TAG, "Camera2: \n" + Log.getStackTraceString(e));
            return false;
        } catch (Exception e) {
            Log.e(TAG, "Camera2: \n" + Log.getStackTraceString(e));
            return false;
        }
        return true;
    }

    private static class AutoResetEvent {
        private Lock lock;
        private Condition condition;
        private boolean signaled;
        public AutoResetEvent() {
            lock = new ReentrantLock();
            condition = lock.newCondition();
            signaled = false;
        }
        public void set() {
            lock.lock();
            try {
                signaled = true;
                condition.signal();
            } finally {
                lock.unlock();
            }
        }
        public void waitOne() {
            while (true) {
                lock.lock();
                try {
                    if (signaled) {
                        signaled = false;
                        return;
                    }
                    condition.await();
                } catch (InterruptedException e) {
                } finally {
                    lock.unlock();
                }
            }
        }
    }

    private static class ByteArrayPair {
        public byte[] array;
        public cn.easyar.Buffer buffer;
    }
    private static class ByteArrayPool {
        private final Lock lock;
        private final int blockSize;
        private final int capacity;
        private int size;
        private final Queue<byte[]> queue;

        public ByteArrayPool(int blockSize, int capacity) {
            lock = new ReentrantLock();
            this.blockSize = blockSize;
            this.capacity = capacity;
            this.queue = new LinkedList<byte[]>();
        }
        public int getBlockSize() {
            return blockSize;
        }
        public int getCapacity() {
            return capacity;
        }
        public int getSize() {
            lock.lock();
            try {
                return size;
            } finally {
                lock.unlock();
            }
        }
        public ByteArrayPair tryAcquire() {
            byte[] block = null;
            lock.lock();
            try {
                if (size >= capacity) { return null; }
                block = queue.poll();
                size += 1;
            } finally {
                lock.unlock();
            }
            if (block == null) {
                block = new byte[blockSize];
            }
            final byte[] finalBlock = block;
            cn.easyar.Buffer buffer = cn.easyar.Buffer.wrapByteArray(block, 0, block.length, true, new FunctorOfVoid() {
                @Override
                public void invoke() {
                    Arrays.fill(finalBlock, (byte) 0);
                    lock.lock();
                    try {
                        size -= 1;
                        if (size >= capacity) { return; }
                        queue.add(finalBlock);
                    } finally {
                        lock.unlock();
                    }
                }
            });
            ByteArrayPair pair = new ByteArrayPair();
            pair.array = block;
            pair.buffer = buffer;
            return pair;
        }
    }

    private static float getRatioError(float x, float x0) {
        float a = (x / Math.max(x0, 1) - 1);
        float b = (x0 / Math.max(x, 1) - 1);
        return a * a + b * b;
    }
    private Size getOptimalPreviewSize(int width, int height) {
        if (mSupportedSizes == null) return null;

        Size s = null;
        float minError = Float.MAX_VALUE;
        for (Size size : mSupportedSizes) {
            float error = getRatioError(width, size.getWidth()) + getRatioError(height, size.getHeight());

            //Log.d("EasyAR", "Camera2: getOptimalPreviewSize: " + width + " " + height + " " + size.getWidth() + " " + size.getHeight() + " " + error);

            if (error < minError) {
                minError = error;
                s = size;
            }
        }
        return s;
    }
    private static boolean contains(int[] values, int value) {
        for (int v : values) {
            if (v == value) { return true; }
        }
        return false;
    }

    private static int checkPixelFormat(ByteBuffer dataY, ByteBuffer dataU, ByteBuffer dataV, long rawByteY, long rawByteU, long rawByteV, int pixelStrideY, int pixelStrideU, int pixelStrideV, int rowStrideY, int rowStrideU, int rowStrideV) {
        if ((pixelStrideY != 1) || (pixelStrideU != pixelStrideV) || (rowStrideU != rowStrideV)) { return PixelFormat.Unknown; }

        if ((rawByteY == 0) || (rawByteU == 0) || (rawByteV == 0)) { return PixelFormat.Unknown; }

        if (pixelStrideU == 2) {
            if (rowStrideY != rowStrideU) {
                return PixelFormat.Unknown;
            }
            if (rawByteU + 1 == rawByteV) {
                return PixelFormat.YUV_NV12;
            }
            else if (rawByteV + 1 == rawByteU) {
                return PixelFormat.YUV_NV21;
            }
        } else if (pixelStrideU == pixelStrideY && pixelStrideU == 1) {
            if (rowStrideY != rowStrideU * 2) {
                return PixelFormat.Unknown;
            }
            if (rawByteU < rawByteV) {
                return PixelFormat.YUV_I420;
            } else {
                return PixelFormat.YUV_YV12;
            }
        }

        return PixelFormat.Unknown;
    }
    private static void copyBufferOrFillZero(ByteBuffer src, int srcOffset, byte[] dest, int destOffset, int size) {
        int limit = src.limit();
        src.position(srcOffset);
        if (srcOffset + size <= limit) {
            src.get(dest, destOffset, size);
        } else {
            src.get(dest, destOffset, limit - srcOffset);
            Arrays.fill(dest, destOffset + (limit - srcOffset), destOffset + size, (byte)0);
        }
        src.position(0);
    }
    private static void fillImageBuffer(ByteBuffer dataY, ByteBuffer dataU, ByteBuffer dataV, int pixelWidth, int pixelHeight, int pixelFormat, byte[] imageBuffer) {
        int yLen = pixelWidth * pixelHeight;
        int uvLen = yLen / 2;

        copyBufferOrFillZero(dataY, 0, imageBuffer, 0, yLen);

        if (pixelFormat == PixelFormat.YUV_NV12) {
            copyBufferOrFillZero(dataU, 0, imageBuffer, yLen, uvLen);
            if ((dataU.limit() == dataV.limit()) && (dataU.limit() < uvLen)) {
                copyBufferOrFillZero(dataV, dataV.limit() - 1, imageBuffer, yLen + dataU.limit(), 1);
            }
        } else if (pixelFormat == PixelFormat.YUV_NV21) {
            copyBufferOrFillZero(dataV, 0, imageBuffer, yLen, uvLen);
            if ((dataU.limit() == dataV.limit()) && (dataU.limit() < uvLen)) {
                copyBufferOrFillZero(dataU, dataU.limit() - 1, imageBuffer, yLen + dataV.limit(), 1);
            }
        } else if (pixelFormat == PixelFormat.YUV_I420) {
            copyBufferOrFillZero(dataU, 0, imageBuffer, yLen, uvLen / 2);
            copyBufferOrFillZero(dataV, 0, imageBuffer, yLen + uvLen / 2, uvLen / 2);
        } else if (pixelFormat == PixelFormat.YUV_YV12) {
            copyBufferOrFillZero(dataV, 0, imageBuffer, yLen, uvLen / 2);
            copyBufferOrFillZero(dataU, 0, imageBuffer, yLen + uvLen / 2, uvLen / 2);
        } else {
            throw new RuntimeException("NotSupported");
        }
    }
    private static long getDirectBufferAddress(java.nio.ByteBuffer buffer) {
        long ptr = JniUtility.newJniGlobalPointer(buffer);
        try {
            return JniUtility.getDirectBufferAddress(ptr);
        } finally {
            JniUtility.deleteJniGlobalPointer(ptr);
        }
    }
}
