package com.edinarobotics.zed.subsystems;

import com.edinarobotics.utils.sensors.StringPot;
import com.edinarobotics.utils.subsystems.Subsystem1816;
import com.sun.squawk.util.MathUtils;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.PIDSource;
import edu.wpi.first.wpilibj.Relay;

public class Lifter extends Subsystem1816 implements PIDSource {
    private static final double MIN_VOLTAGE = 0;
    private static final double MAX_VOLTAGE = 5;
    private static final double STRING_LENGTH = 1;
    private static final double STRING_POT_POSITION = 1;
    
    private LifterDirection direction;
    private Relay lifterRelay;
    private StringPot stringPot;
    private DigitalInput upperSwitch;
    private DigitalInput lowerSwitch;
    
    public Lifter(int lifterRelay, int lifterStringPot, int upperLimitSwitch, int lowerLimitSwitch) {
        super("Lifter");
        this.lifterRelay = new Relay(lifterRelay);
        stringPot = new StringPot(lifterStringPot, MIN_VOLTAGE, MAX_VOLTAGE, STRING_LENGTH);
        direction = LifterDirection.LIFTER_STOP;
        upperSwitch = new DigitalInput(upperLimitSwitch);
        lowerSwitch = new DigitalInput(lowerLimitSwitch);
    }
    
    public void setLifterDirection(LifterDirection direction){
        this.direction = direction;
        update();
    }
    
    public LifterDirection getLifterDirection(){
        return direction;
    }
    
    public double getStringPotLength() {
        return STRING_LENGTH;
    }
    
    public double getShooterAngle() {
        return Math.toDegrees(MathUtils.atan(stringPot.getStringLength() / STRING_POT_POSITION));
    }
    
    public double pidGet() {
        return getShooterAngle();
    }
    
    public void update() {
        LifterDirection processedDirection = direction;
        if((direction.equals(LifterDirection.LIFTER_UP) && getUpperLimitSwitch()) ||
           (direction.equals(LifterDirection.LIFTER_DOWN) && getLowerLimitSwitch())){
            processedDirection = LifterDirection.LIFTER_STOP;
        }
        lifterRelay.set(processedDirection.getRelayValue());
    }
    
    public boolean getUpperLimitSwitch(){
        return upperSwitch.get();
    }
    
    public boolean getLowerLimitSwitch(){
        return lowerSwitch.get();
    }
    
    public static class LifterDirection {
        public static final LifterDirection LIFTER_UP = new LifterDirection((byte)1);
        public static final LifterDirection LIFTER_DOWN = new LifterDirection((byte)-1);
        public static final LifterDirection LIFTER_STOP = new LifterDirection((byte)0);
        
        private byte value;
        
        private LifterDirection(byte value){
            this.value = value;
        }
        
        protected byte getValue(){
            return value;
        }
        
        public boolean equals(Object other){
            if(other instanceof LifterDirection){
                return ((LifterDirection)other).getValue() == getValue();
            }
            return false;
        }
        
        public int hashCode(){
            return new Byte(getValue()).hashCode();
        }
        
        public Relay.Value getRelayValue(){
            if(equals(LIFTER_UP)){
                return Relay.Value.kReverse;
            }
            else if(equals(LIFTER_DOWN)){
                return Relay.Value.kForward;
            }
            return Relay.Value.kOff;
        }
    }
}

@@ -210,28 +210,161 @@
 wpilibj.
-DigitalInput
+ADXL345_I2C;%0Aimport edu.wpi.first.wpilibj.DigitalInput;%0Aimport edu.wpi.first.wpilibj.DriverStation;%0Aimport edu.wpi.first.wpilibj.DriverStationLCD
 ;%0Aimport
@@ -706,24 +706,92 @@
 N = 1;%0A    %0A
+    public static final double PYRAMID_BACK_MIDDLE_ANGLE = 33;%0A    %0A
     private 
@@ -864,27 +864,33 @@
 ate 
-StringPot stringPot
+ADXL345_I2C accelerometer
 ;%0A  
@@ -959,24 +959,98 @@
 owerSwitch;%0A
+    private DriverStationLCD textOutput = DriverStationLCD.getInstance();%0A
     %0A    pub
@@ -1232,90 +1232,75 @@
     
-stringPot = new StringPot(lifterStringPot, MIN_VOLTAGE, MAX_VOLTAGE, STRING_LENGTH
+accelerometer = new ADXL345_I2C(1, ADXL345_I2C.DataFormat_Range.k2G
 );%0A 
@@ -1853,61 +1853,62 @@
 ls.a
-tan(stringPot.getStringLength() / STRING_POT_POSITION
+cos(accelerometer.getAcceleration(ADXL345_I2C.Axes.kX)
 ));%0A
@@ -2310,32 +2310,32 @@
 STOP;%0A        %7D%0A
-
         lifterRe
@@ -2375,24 +2375,150 @@
 ayValue());%0A
+        textOutput.println(DriverStationLCD.Line.kUser4, 1, %22Shoot Ang: %22+getShooterAngle());%0A        textOutput.updateLCD();%0A
     %7D%0A    %0A 

Modify Lifter to include an accelerometer for angle determination.