package com.edinarobotics.zeke.subsystems;

import com.edinarobotics.utils.log.Level;
import com.edinarobotics.utils.log.LogSystem;
import com.edinarobotics.utils.log.Logger;
import com.edinarobotics.utils.subsystems.Subsystem1816;
import com.edinarobotics.zeke.Components;
import edu.wpi.first.wpilibj.AnalogPotentiometer;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.Talon;

public class Shooter extends Subsystem1816 {
    
    public static final double FIRING_HEIGHT = Shooter.MIN_SAFE_HEIGHT;
    public static final double MAX_SHOOT_DISTANCE = 18.0; //In feet
    public static final double MIN_SHOOT_DISTANCE = 12.0; //In feet
    
    private Logger log = LogSystem.getLogger("zeke.shooter");
    
    private Talon winch;
    private DoubleSolenoid winchSolenoidRelease;
    private AnalogPotentiometer shooterPot;
    private DigitalInput lowerLimitSwitch;
   
    private WinchState winchState, lastState;
    
    private static final double SCALE = 6.317;
    private static final double OFFSET = -1.579;
    private static final double MIN_SAFE_HEIGHT = 2.24;
    private static final double MAX_PUSH_HEIGHT = 3.0;
    
    private static final DoubleSolenoid.Value ENGAGED = DoubleSolenoid.Value.kForward;
    private static final DoubleSolenoid.Value DISENGAGED = DoubleSolenoid.Value.kReverse;

    public Shooter(int winchPort, int winchSolenoidForward, int winchSolenoidReverse,
            int shooterPotPort, int limitSwitchPort) {
        winch = new Talon(winchPort);
        winchSolenoidRelease = new DoubleSolenoid(winchSolenoidForward, winchSolenoidReverse);
        shooterPot = new AnalogPotentiometer(shooterPotPort, SCALE, OFFSET);
        lowerLimitSwitch = new DigitalInput(1, 6);
        winchState = WinchState.STOPPED;
        lastState = WinchState.STOPPED;
    }
    
    public void setWinchState(WinchState winchState) {
        if(winchState != null) {
            this.winchState = winchState;
        } else {
            log.log(Level.SEVERE, "Shooter.setWinchState got null.");
        }
        update();
    }
    
    public double getStringPot() {
        return shooterPot.get();
    }
    
    public WinchState getWinchState() {
        return winchState;
    }
    
    public boolean getShooterLimitSwitch() {
        return lowerLimitSwitch.get();
    }
    
    public void update() {
        // Winch motor
        if((getStringPot() < MIN_SAFE_HEIGHT || getShooterLimitSwitch())
                    && winchState.equals(WinchState.LOWERING)) {
            winchState = WinchState.STOPPED;
        }
        else if(Components.getInstance().collector.getDeployed() && lastState.isPistonEngaged()  && winchState.equals(WinchState.FREE)){
            winchState = WinchState.STOPPED;
        }
        
        winch.set(winchState.getMotorSpeed());
        lastState = winchState;
        DoubleSolenoid.Value pistonState = (winchState.isPistonEngaged() ? ENGAGED : DISENGAGED);
        winchSolenoidRelease.set(pistonState);
    }
    
    public static final class WinchState {
        public static final WinchState LOWERING = new WinchState((byte)0, false, 1.0, "lowering");
        public static final WinchState STOPPED = new WinchState((byte)1, false, 0.0, "stopped");
        public static final WinchState FREE = new WinchState((byte)2, true, 0.0, "free");
        
        private byte winchState;
        private boolean isPistonEngaged;
        private double motorSpeed;
        private String stateName;
        
        private WinchState(byte winchState, boolean isPistonEngaged,
                double motorSpeed, String stateName) {
            this.winchState = winchState;
            this.isPistonEngaged = isPistonEngaged;
            this.motorSpeed = motorSpeed;
            this.stateName = stateName;
        }
        
        private byte getWinchState() {
            return winchState;
        }

        public boolean isPistonEngaged() {
            return isPistonEngaged;
        }

        private double getMotorSpeed() {
            return motorSpeed;
        }
        
        public boolean equals(Object winchState) {
            if(winchState instanceof WinchState) {
                return ((WinchState)winchState).getWinchState() == this.getWinchState();
            }
            return false;
        }
        
        public String toString() {
            return stateName.toLowerCase();
        }
    }
    
}

@@ -1138,65 +1138,9 @@
 T = 
-2.24;%0A    private static final double MAX_PUSH_HEIGHT = 3
+0
 .0;%0A
@@ -1222,23 +1222,23 @@
 .Value.k
-Forward
+Reverse
 ;%0A    pr
@@ -1312,23 +1312,23 @@
 .Value.k
-Reverse
+Forward
 ;%0A%0A    p
@@ -2662,17 +2662,16 @@
 ngaged()
- 
  && winc
@@ -3119,20 +3119,19 @@
 byte)0, 
-fals
+tru
 e, 1.0, 
@@ -3216,20 +3216,19 @@
 byte)1, 
-fals
+tru
 e, 0.0, 
@@ -3309,19 +3309,20 @@
 byte)2, 
-tru
+fals
 e, 0.0, 

Remove limit switch from shooting and fix piston re-engaging too quickly.