package org.usfirst.frc178.devices;

import com.sun.squawk.platform.posix.natives.LibC;
import edu.wpi.first.wpilibj.*;
import org.usfirst.frc178.custom.*;

public class Sensors
{
	public final short PHOTOCELL_THRESHOLD = 200;
	public Encoder /*shooterOneEncoder, */shooterTwoEncoder, driveEncoderLeft, driveEncoderRight;
	public EncoderMod shooterOneEncoder;
	public LimitSwitch elevationLowSwitch, elevationHighSwitch, elevationLoadSwitch, pressureSwitch;
	public Photosensor shooterPhoto, feederPhoto;

	public Sensors()
	{
			//driveEncoderLeft = new Encoder(1,2);			//DIO ports 1,2
			//driveEncoderLeft = new Encoder(3,4);			//DIO ports 3,4
			//shooterOneEncoder = new Encoder(5,6);			//DIO ports 5,6
			shooterOneEncoder = new EncoderMod(5,6);
			shooterOneEncoder.setDistancePerPulse(1/64d);
			//shooterTwoEncoder = new Encoder(7,8);			//DIO ports 7,8
			elevationHighSwitch = new LimitSwitch(9, false);
			elevationLoadSwitch = new LimitSwitch(10, false);
			elevationLowSwitch = new LimitSwitch(12, false);
			pressureSwitch = new LimitSwitch(13,false);		//DIO port 14, cutoff at 120 psi --Check Normally Open State******
			shooterPhoto = new Photosensor(3);
			//feederPhoto = new Photosensor(3);
	}
	
	public void printEncoders(){
		System.out.println(shooterOneEncoder.getRate());
	}
	
	public void printPhotocells(){
		System.out.println(shooterPhoto.analog.getAverageValue());
	}
	
	public void PID(){
		
	}
	
	public double proportaionalShooterControl(double setPoint, Encoder encoder){
		double currentRate = encoder.getRate();
		double error = setPoint - currentRate;

		double gain = 50;
		
		double output = error * gain + setPoint;
		double scaledOutput =(output/(setPoint * gain + setPoint));
		return (scaledOutput * scaledOutput * scaledOutput);  //scaledOutput ^ 3
	}

	public void printLimitSwitches() {
		System.out.print(elevationHighSwitch.getState() + "\t");
		System.out.print(elevationLoadSwitch.getState() + "\t");
		System.out.println(elevationLowSwitch.getState());
	}

}
@@ -174,10 +174,11 @@
 sors
+ %7B
 %0A
-%7B
 %0A%09pu
@@ -221,16 +221,17 @@
  = 200;%0A
+%0A
 %09public 
@@ -242,10 +242,8 @@
 der 
-/*
 shoo
@@ -261,10 +261,8 @@
 er, 
-*/
 shoo
@@ -317,46 +317,8 @@
 ht;%0A
-%09public EncoderMod shooterOneEncoder;%0A
 %09pub
@@ -393,17 +393,37 @@
 adSwitch
-,
+;%0A%09public LimitSwitch
  pressur
@@ -496,18 +496,17 @@
 ensors()
-%0A%09
+ 
 %7B%0A%09%09%09//d
@@ -728,57 +728,8 @@
 6);%0A
-%09%09%09shooterOneEncoder.setDistancePerPulse(1/64d);%0A
 %09%09%09/
@@ -981,16 +981,17 @@
 itch(13,
+ 
 false);%09
@@ -1134,16 +1134,66 @@
 nsor(3);
+%0A%0A%09%09%09shooterOneEncoder.setDistancePerPulse(1/64d);
 %0A%09%7D%0A%09%0A%09p
@@ -1364,33 +1364,243 @@
 ageValue());%0A%09%7D%0A
-%09
+%0A%09public void printLimitSwitches() %7B%0A%09%09System.out.print(elevationHighSwitch.getState() + %22%5Ct%22);%0A%09%09System.out.print(elevationLoadSwitch.getState() + %22%5Ct%22);%0A%09%09System.out.println(elevationLowSwitch.getState());%0A%09%7D%0A
 %0A%09public void PI
@@ -1985,216 +1985,5 @@
 %09%7D%0A%0A
-%09public void printLimitSwitches() %7B%0A%09%09System.out.print(elevationHighSwitch.getState() + %22%5Ct%22);%0A%09%09System.out.print(elevationLoadSwitch.getState() + %22%5Ct%22);%0A%09%09System.out.println(elevationLowSwitch.getState());%0A%09%7D%0A%0A
 %7D

Remove EncoderMod. Not required after all.