package org.firstinspires.ftc.teamcode;

import android.graphics.Bitmap;

import com.vuforia.Frame;
import com.vuforia.Image;
import com.vuforia.PIXEL_FORMAT;
import com.vuforia.State;
import com.vuforia.Vuforia;

import org.firstinspires.ftc.robotcore.external.ClassFactory;
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;

public class BrightEyesInterface {
    private static final String VUFORIA_KEY =
         "AXvDB53/////AAABmU7XauHpH0aXjBLQcVVPpfk3omAEtqgcrofu3"
         + "wNrglhxAHt9SK9X3W01Jb58Y7/fCw9JwLN2lflT2pJeMXS7Bakp"
         + "79El2Et+fWdDHgQ5KcGceYIAQEENR6t9l1WkY8j0WVaIlcjOrPA"
         + "zZv4/U/YpUvcB7QFoUO33u1L2iJKoKPuJm5eChTrjR2iacZLH28"
         + "iriFGoRuQOfsBsgKK5InsJH1Xll9e8DZKhDBxBwGkfIlbqyv2LF"
         + "KKe0v2J5azm2W0NRbJ8dnevHf90YbXoXJARxcmu5zCMYvFXhKRK"
         + "6Si0W+Yqr7Gx8+Xd6wSwJ9LijMDBySi8hocf8Vl5ie2qmi22ty7"
         + "J6KPh7CehD8H6qmxmst2r";

    private VuforiaLocalizer vuforia;

    /** Input image size of the model along x axis. */
    private int imageSizeX;
    /** Input image size of the model along y axis. */
    private int imageSizeY;

    Image img = null;

    BrightEyesInterface() {

    }

    void init(int monitorViewId) {
        // Init Vuforia
        VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(monitorViewId);

        parameters.vuforiaLicenseKey = VUFORIA_KEY;
        parameters.cameraDirection = VuforiaLocalizer.CameraDirection.BACK;

        //  Instantiate the Vuforia engine
        vuforia = ClassFactory.getInstance().createVuforia(parameters);

        Vuforia.setFrameFormat(PIXEL_FORMAT.RGB565, true); //enables RGB565 format for the image
        vuforia.setFrameQueueCapacity(1); //tells VuforiaLocalizer to only store one frame at a time
    }

    Bitmap getBitmap() throws InterruptedException {
        VuforiaLocalizer.CloseableFrame f = vuforia.getFrameQueue().take(); //takes the frame at the head of the queue
        long num = f.getNumImages();

        for (short i = 0; i < num; i++) {
            if (f.getImage(i).getFormat() == PIXEL_FORMAT.RGB565) {
                img = f.getImage(i);
            }
        }
        if (img != null) {
            Bitmap bmp = Bitmap.createBitmap(img.getWidth(), img.getHeight(), Bitmap.Config.RGB_565);
            bmp.copyPixelsFromBuffer(img.getPixels());
            return bmp;
        }
        return null;
    }
}