[Bennett]

/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.commands;



import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.camera.AxisCameraException;
import edu.wpi.first.wpilibj.image.*;

public class CameraSeek extends CommandBase {
  
    AxisCamera camera;
    CriteriaCollection cc;    
     
     public CameraSeek() {
        
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
    }

    // Called just before this Command runs the first time
    protected void initialize() {
        String camip = "10.29.77.11";
        camera = AxisCamera.getInstance(camip);
        cc = new CriteriaCollection();      // create the criteria for the particle filter
        cc.addCriteria(NIVision.MeasurementType.IMAQ_MT_BOUNDING_RECT_WIDTH, 20, 400, false);
        cc.addCriteria(NIVision.MeasurementType.IMAQ_MT_BOUNDING_RECT_HEIGHT, 20, 300, false); 
    try {
                ColorImage picture = camera.getImage();   //should read image from camera and then save it to a jpg file   
                picture.write("runthis.jpg");
                
                ColorImage trypicture = new RGBImage("runthis.jpg");    //workaround that should allow us to process the image
                
                //BinaryImage thresholdImage = trypicture.thresholdRGB(205, 255, 205, 255, 205, 255);   // keep only black objects
                BinaryImage thresholdImage = trypicture.thresholdHSL(126, 255, 6, 255, 201, 255);     //Saturation/Hue/Lumination filter
                BinaryImage bigObjectsImage = thresholdImage.removeSmallObjects(false, 2);  // remove small artifacts
                BinaryImage convexHullImage = bigObjectsImage.convexHull(false);          // fill in occluded rectangles
                BinaryImage filteredImage = convexHullImage.particleFilter(cc);           // find filled in rectangles                
                ParticleAnalysisReport[] reports = filteredImage.getOrderedParticleAnalysisReports();  // get list of results 
                
                //System.out.println("Step 2 Achieved-Threshold set, image filtered");
              
        
               
    for (int i = 0; i < reports.length; ++i) {                               // print results              
                ParticleAnalysisReport r = reports[i];
                System.out.println("Particle: " + i + ":  Center of mass x: " + r.center_mass_x_normalized);                   
                System.out.println("Particle: " + i + ": Center of mass y: " + r.center_mass_y_normalized);
                
                if (r.center_mass_x_normalized < 0.7) {
                  System.out.println("Found it, its to the left");
                  shootrun.Pan(r.center_mass_x_normalized);
                  
                    if (r.center_mass_x_normalized < 0.0) {
                  Timer.delay(-1.0 * r.center_mass_x_normalized); 
                  }
                   else if (r.center_mass_x_normalized > 0.0) {
                   Timer.delay(r.center_mass_x_normalized);     
                   }
                    shootrun.Pan(0);
                }  
                else if (r.center_mass_x_normalized < -0.9) {
                  System.out.println("Found it, its to the right");
                  shootrun.Pan(r.center_mass_x_normalized);
                   if (r.center_mass_x_normalized < 0.0) {
                  Timer.delay(-1.0 * r.center_mass_x_normalized); 
                  }
                   else if (r.center_mass_x_normalized > 0.0) {
                   Timer.delay(r.center_mass_x_normalized);     
                   }
                  shootrun.Pan(0);    
                }
                else 
                  shootrun.Pan(0);  
                  System.out.println("It be trippen, your right on top of it.");
                
    }
           shootrun.Pan(0);
            filteredImage.write("A-5-filteredpic.jpg");
            bigObjectsImage.write("A-3-bigimage.jpg");
            convexHullImage.write("A-4-convex.jpg");
            thresholdImage.write("A-2-threshold.jpg");
            picture.write("A-1-Fresh.jpg");
            trypicture.write("A-6-riu.jpg");
            //thresholdImagebw.write("A-7-Secondarythreshold");
            
                System.out.println(filteredImage.getNumberParticles() + "  " + Timer.getFPGATimestamp()); 
                filteredImage.free();
                convexHullImage.free();
                bigObjectsImage.free();
                thresholdImage.free();
                picture.free();
                trypicture.free();
                //thresholdImagebw.free();
                
                //System.out.println("Step 3 Achieved-Images Freed");   
                  
            } catch (AxisCameraException ex) {
            } catch (NIVisionException ex) {

            }
        
       
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
        
    
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return false;
    }

    // Called once after isFinished returns true
    protected void end() {
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    }
}