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 com.sun.squawk.debugger.Log;
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 {
    
    private Talon winch;
    private DoubleSolenoid solenoidRelease;
    private AnalogPotentiometer shooterPot;
    private DigitalInput lowerLimitSwitch;
    private Logger log = LogSystem.getLogger("zeke.shooter");
   
    private WinchState winchState;
    
    private static final double SCALE = 1;
    private static final double OFFSET = 0;
    private static final DoubleSolenoid.Value ENGAGED = DoubleSolenoid.Value.kForward;
    private static final DoubleSolenoid.Value DISENGAGED = DoubleSolenoid.Value.kReverse;

    public Shooter(int winchPort, int doubleSolenoidForward,
                int doubleSolenoidReverse, int shooterPort,
                DigitalInput lowerLimitSwitch) {
        winch = new Talon(winchPort);
        solenoidRelease = new DoubleSolenoid(doubleSolenoidForward
                        , doubleSolenoidReverse);
        shooterPot = new AnalogPotentiometer(shooterPort, SCALE, OFFSET);
        this.lowerLimitSwitch = lowerLimitSwitch;
        winchState = 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 void update() {
        // Safety check
        if(getShooterLimitSwitch() && winchState.equals(WinchState.LOWERING)) {
            winchState = WinchState.STOPPED;
        }
        
        if(winchState.isMotorOn()) {
            winch.set(1);
        } else {
            winch.set(0);
        }
        
        if(winchState.isPistonEngaged() && !Components.getInstance().collector.getDeployed()) {
            solenoidRelease.set(ENGAGED);
        } else {
            solenoidRelease.set(DISENGAGED);
        }
    }
    
    public boolean getShooterLimitSwitch() {
        return lowerLimitSwitch.get();
    }
    
    public static final class WinchState {
        public static final WinchState LOWERING = new WinchState((byte)-1, false, true, "lowering");
        public static final WinchState STOPPED = new WinchState((byte)0, false, false, "stopped");
        public static final WinchState FREE = new WinchState((byte)1, true, false, "free");
        
        private byte winchState;
        private boolean isPistonEngaged;
        private boolean isMotorOn;
        private String stateName;
        
        private WinchState(byte winchState, boolean isPistonEngaged,
                boolean isMotorOn, String stateName) {
            this.winchState = winchState;
            this.isPistonEngaged = isPistonEngaged;
            this.isMotorOn = isMotorOn;
            this.stateName = stateName;
        }
        
        private byte getWinchState() {
            return winchState;
        }

        private boolean isPistonEngaged() {
            return isPistonEngaged;
        }

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

@@ -271,44 +271,8 @@
 ts;%0A
-import com.sun.squawk.debugger.Log;%0A
 impo
@@ -2137,34 +2137,41 @@
        %0A        
-if
+winch.set
 (winchState.isMo
@@ -2170,101 +2170,25 @@
 ate.
-is
+get
 Motor
-On()) %7B%0A            winch.set(1);%0A        %7D else %7B%0A            winch.set(0);%0A        %7D
+Speed());
 %0A   
@@ -2634,20 +2634,19 @@
  false, 
-true
+1.0
 , %22lower
@@ -2732,21 +2732,19 @@
  false, 
-false
+0.0
 , %22stopp
@@ -2825,21 +2825,19 @@
 , true, 
-false
+0.0
 , %22free%22
@@ -2938,33 +2938,33 @@
 private 
-boolean isMotorOn
+double motorSpeed
 ;%0A      
@@ -3085,33 +3085,33 @@
         
-boolean isMotorOn
+double motorSpeed
 , String
@@ -3239,29 +3239,31 @@
 his.
-isM
+m
 otor
-On = isMotorOn
+Speed = motorSpeed
 ;%0A  
@@ -3511,25 +3511,28 @@
 ate 
-boolean isMotorOn
+double getMotorSpeed
 () %7B
@@ -3555,17 +3555,18 @@
 urn 
-isM
+m
 otor
-On
+Speed
 ;%0A  

Fix Shooter class.