// RobotBuilder Version: 2.0
//
// This file was generated by RobotBuilder. It contains sections of
// code that are automatically generated and assigned by robotbuilder.
// These sections will be updated in the future when you export to
// Java from RobotBuilder. Do not put any code or make any change in
// the blocks indicating autogenerated code or it will be lost on an
// update. Deleting the comments indicating the section will prevent
// it from being updated in the future.


package org.usfirst.frc3824.BetaBot.subsystems;

import org.usfirst.frc3824.BetaBot.Constants;
import org.usfirst.frc3824.BetaBot.RobotMap;
import org.usfirst.frc3824.BetaBot.commands.*;

import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.networktables.NetworkTable;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;


/**
 *
 */
public class Targets extends Subsystem {

    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTANTS

    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTANTS

    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS

    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS

	NetworkTable m_contoursReport;
	NetworkTable m_imageReport;
	NetworkTable m_frameRateReport;
	double[] m_defaultValue = new double[0];
	double m_targetCenterX;
	double m_positionFromImageCenterX;
	double m_normalizedOffestFromImageCenter;
	int m_imageWidth;
	
    // Put methods for controlling this subsystem
    // here. Call these from Commands.

    public void initDefaultCommand() {
        // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DEFAULT_COMMAND


    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DEFAULT_COMMAND

        // Set the default command for a subsystem here.
        // setDefaultCommand(new MySpecialCommand());
    }
    

	public Targets()
	{
		m_contoursReport = NetworkTable.getTable("GRIP/cameraTargets");
		m_imageReport = NetworkTable.getTable("GRIP/imageSize");
		m_frameRateReport = NetworkTable.getTable("GRIP");

	}
	
	// if the image is positioned to the right, the robot is too far left.
	// so this return value is flipped 
	public double getTargetOffsetFromCenter()
	{
		// Drive the robot given the speed and direction
		m_targetCenterX = getCenterXOfLargestTarget();
		
		m_positionFromImageCenterX = m_targetCenterX - (m_imageWidth/2); // calculate offset from center in pixels
		m_normalizedOffestFromImageCenter = m_positionFromImageCenterX / (m_imageWidth / 2); // convert to a range of -1 (left edge) to 1 (right edge)
		
		return -m_normalizedOffestFromImageCenter;
	}
	
	public void updateSmartDashboard()
	{
		SmartDashboard.putNumber("Targets FrameRate", m_frameRateReport.getNumber("cameraFrameRate", 0.0));
		SmartDashboard.putNumber("Targets X offset from image center", m_positionFromImageCenterX);
		SmartDashboard.putNumber("Targets Normalized X offset from image center", m_normalizedOffestFromImageCenter);
		SmartDashboard.putNumber("Targets center X", m_targetCenterX);
		SmartDashboard.putNumber("Targets Image Width", m_imageWidth);
	}
	
	private double getCenterXOfLargestTarget()
	{
		double[] centerXs = m_contoursReport.getNumberArray("centerX", m_defaultValue);
		double[] areas = m_contoursReport.getNumberArray("area", m_defaultValue); 
		m_imageWidth = (int)m_imageReport.getNumber("x", 640.0);
		
		double maxArea = 0.0;
		int maxAreaIndex = -1;
		for (int areaIndex = 0; areaIndex < areas.length; areaIndex++)
		{
			if (maxArea < areas[areaIndex])
			{
				maxAreaIndex = areaIndex;
			}
		}
		
		if (maxAreaIndex >= 0 && centerXs.length < maxAreaIndex)
		{
			m_targetCenterX = centerXs[maxAreaIndex];
		}
		else
		{
			m_targetCenterX = m_imageWidth / 2.0;
		}
		
		return m_targetCenterX;
	}


}


@@ -3671,42 +3671,8 @@
 %3E= 0
- && centerXs.length %3C maxAreaIndex
 )%0D%0A%09

Revert "Added an additional check to targets subsystem"