package com.phantommentalists.Twenty14;

import edu.wpi.first.wpilibj.camera.*;
import edu.wpi.first.wpilibj.image.*;

/*
 * AimingSystem allocation
 */
public class AimingSystem {

    public AxisCamera myCamera;
    public Ultrasonic myUltrasonic;
    public ColorImage image;
    public BinaryImage threshold;
    public ParticleAnalysisReport[] reports = null;
    private int imageState;
    private boolean busy;
    private boolean hot;
    private int xCenter;
    private int yCenter;
    private int xDeadband;
    private int yDeadband;
    
    public AimingSystem()
    {
        myUltrasonic = new Ultrasonic(-1);
        myCamera = AxisCamera.getInstance("10.20.28.11");
        myCamera.writeResolution(AxisCamera.ResolutionT.k640x480);
        myCamera.writeExposurePriority(AxisCamera.ExposurePriorityT.imageQuality);
        myCamera.writeExposureControl(AxisCamera.ExposureT.hold);
        myCamera.writeWhiteBalance(AxisCamera.WhiteBalanceT.fixedIndoor);
    }
    
    public void processImage() {
        try {
            switch (imageState) {
                    case 0:
                        busy = true;
                        image = myCamera.getImage();
                        image.write("/image.jpg");
                        System.out.println("Got image");
                        imageState++;
                        busy = false;
                        break;
                    case 1:
                        busy = true;
                        threshold = image.thresholdRGB(0, 70, 185, 255, 145, 225);  // green values
                        threshold.write("/threshold.jpg");
                        System.out.println("Got threshold");
                        imageState++;
                        busy = false;
                        break;
                    case 2:
                        busy = true;
                        reports = threshold.getOrderedParticleAnalysisReports(10);
                        CheckHot();
                        threshold.free();
                        threshold = null;
                        imageState = 0;
                        break;
                }
        } catch (AxisCameraException ex) {
                ex.printStackTrace();
            } catch (NIVisionException ex) {
                ex.printStackTrace();
            }
    }
    
    /**
     * 
     */
    public void CheckHot() {
        for (int i = 0; i < reports.length; i++) {
            hot = reports[i].center_mass_x >= xCenter-xDeadband || reports[i].center_mass_x < xCenter+xDeadband;
            hot &= reports[i].center_mass_y >= yCenter-yDeadband || reports[i].center_mass_y < yCenter+yDeadband;
            if (hot) return;
        }
    }
    
    /**
     * 
     */
    public void DisableAimingSystem()
    {
        try
        {
            if (image != null) {
                image.free();
                image = null;
            }
            if (threshold != null) {
                threshold.free();
                threshold = null;
            }
        } catch (NIVisionException e)
        {
        }
        imageState = 0;
        busy = false;
    }
}

@@ -76,48 +76,18 @@
 era.
-*;%0D%0Aimport edu.wpi.first.wpilibj.image.*
+AxisCamera
 ;%0D%0A%0D
@@ -233,2987 +233,220 @@
     
-public ColorImage image;%0D%0A    public BinaryImage threshold;%0D%0A    public ParticleAnalysisReport%5B%5D reports = null;%0D%0A    private int imageState;%0D%0A    private boolean busy;%0D%0A    private boolean hot;%0D%0A    private int xCenter;%0D%0A    private int yCenter;%0D%0A    private int xDeadband;%0D%0A    private int yDeadband;%0D%0A    %0D%0A    public AimingSystem()%0D%0A    %7B%0D%0A        myUltrasonic = new Ultrasonic(-1);%0D%0A        myCamera = AxisCamera.getInstance(%2210.20.28.11%22);%0D%0A        myCamera.writeResolution(AxisCamera.ResolutionT.k640x480);%0D%0A        myCamera.writeExposurePriority(AxisCamera.ExposurePriorityT.imageQuality);%0D%0A        myCamera.writeExposureControl(AxisCamera.ExposureT.hold);%0D%0A        myCamera.writeWhiteBalance(AxisCamera.WhiteBalanceT.fixedIndoor);%0D%0A    %7D%0D%0A    %0D%0A    public void processImage() %7B%0D%0A        try %7B%0D%0A            switch (imageState) %7B%0D%0A                    case 0:%0D%0A                        busy = true;%0D%0A                        image = myCamera.getImage();%0D%0A                        image.write(%22/image.jpg%22);%0D%0A                        System.out.println(%22Got image%22);%0D%0A                        imageState++;%0D%0A                        busy = false;%0D%0A                        break;%0D%0A                    case 1:%0D%0A                        busy = true;%0D%0A                        threshold = image.thresholdRGB(0, 70, 185, 255, 145, 225);  // green values%0D%0A                        threshold.write(%22/threshold.jpg%22);%0D%0A                        System.out.println(%22Got threshold%22);%0D%0A                        imageState++;%0D%0A                        busy = false;%0D%0A                        break;%0D%0A                    case 2:%0D%0A                        busy = true;%0D%0A                        reports = threshold.getOrderedParticleAnalysisReports(10);%0D%0A                        CheckHot();%0D%0A                        threshold.free();%0D%0A                        threshold = null;%0D%0A                        imageState = 0;%0D%0A                        break;%0D%0A                %7D%0D%0A        %7D catch (AxisCameraException ex) %7B%0D%0A                ex.printStackTrace();%0D%0A            %7D catch (NIVisionException ex) %7B%0D%0A                ex.printStackTrace();%0D%0A            %7D%0D%0A    %7D%0D%0A    %0D%0A    /**%0D%0A     * %0D%0A     */%0D%0A    public void CheckHot() %7B%0D%0A        for (int i = 0; i %3C reports.length; i++) %7B%0D%0A            hot = reports%5Bi%5D.center_mass_x %3E= xCenter-xDeadband %7C%7C reports%5Bi%5D.center_mass_x %3C xCenter+xDeadband;%0D%0A            hot &= reports%5Bi%5D.center_mass_y %3E= yCenter-yDeadband %7C%7C reports%5Bi%5D.center_mass_y %3C yCenter+yDeadband;%0D%0A            if (hot) return;%0D%0A        %7D%0D%0A    %7D%0D%0A    %0D%0A    /**%0D%0A     * %0D%0A     */%0D%0A    public void DisableAimingSystem()%0D%0A    %7B%0D%0A        try%0D%0A        %7B%0D%0A            if (image != null) %7B%0D%0A                image.free();%0D%0A                image = null;%0D%0A            %7D%0D%0A            if (threshold != null) %7B%0D%0A                threshold.free();%0D%0A                threshold = null;%0D%0A            %7D%0D%0A        %7D catch (NIVisionException e)%0D%0A        %7B%0D%0A        %7D%0D%0A        imageState = 0;%0D%0A        busy = false;%0D%0A    %7D%0D%0A%7D%0D
+%0D%0A    public AimingSystem()%0D%0A    %7B%0D%0A        myUltrasonic = new Ultrasonic(-1);%0D%0A        myCamera = AxisCamera.getInstance(%2210.20.28.11%22);%0D%0A        myCamera.writeResolution(AxisCamera.ResolutionT.k640x480);%0D%0A    %7D%0D%0A%7D
 %0A

Revert "Added in the rough aiming code"