package edu.wpi.first.wpilibj.templates;

import com.sun.cldc.jna.*;
import edu.wpi.first.wpilibj.SimpleRobot;
import edu.wpi.first.wpilibj.camera.*;
import edu.wpi.first.wpilibj.image.*;

public class RobotTemplate extends SimpleRobot {
    /**
     * This function is called once each time the robot enters autonomous mode.
     */

    AxisCamera camera;

    // STUFF FOR IMPORTING MORE IMAGE PROCESSING JUNK //
    private static final TaskExecutor taskExecutor = new TaskExecutor("nivision task");

    private static final BlockingFunction imaqConvexHullFn = NativeLibrary.getDefaultInstance().getBlockingFunction("imaqConvexHull");
    static { imaqConvexHullFn.setTaskExecutor(taskExecutor); }

    public static void convexHull(Pointer source, Pointer dest, int connectivity8)  throws NIVisionException{
        assertCleanStatus(imaqConvexHullFn.call3(source.address().toUWord().toPrimitive(), dest.address().toUWord().toPrimitive(), connectivity8));
    }

    private static final BlockingFunction imaqGetLastErrorFn = NativeLibrary.getDefaultInstance().getBlockingFunction("imaqGetLastError");
    static { imaqGetLastErrorFn.setTaskExecutor(taskExecutor);  }

    public static int getLastError(){
        return imaqGetLastErrorFn.call0();
    }
    protected static void assertCleanStatus (int code) throws NIVisionException {
        if (code == 0) {
            throw new NIVisionException(imaqGetLastErrorFn.call0());
        }
    }
    /* ================= */



    public void autonomous() {
        camera = AxisCamera.getInstance();
        camera.writeResolution(AxisCamera.ResolutionT.k640x480);

        while(isAutonomous())
        {
            try {
                ColorImage img = camera.getImage();

                // Do the threshold
                BinaryImage bImg = img.thresholdHSL(0, 255, 0, 20, 239, 255);

                //Do convex hull (copy generated c++ code?)
                convexHull(bImg.image, bImg.image, 1);

                for(int i = 0; i < bImg.getNumberParticles(); i++)
                {
                    ParticleAnalysisReport report = bImg.getParticleAnalysisReport(i);
                }
            } catch (AxisCameraException ex) {
                ex.printStackTrace();
            } catch (NIVisionException ex) {
                ex.printStackTrace();
            }
        }
    }

    public void operatorControl() {

    }
}