/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package org.team3309.frc2013;

import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.PIDOutput;
import edu.wpi.first.wpilibj.PIDSource;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
 *
 * @author vmagr_000
 */
public class Climber implements Runnable, PIDSource, PIDOutput {

    private static Climber instance = null;

    public static Climber getInstance() {
        if (instance == null) {
            instance = new Climber(RobotMap.CLIMBER_TIPPER_FORWARD, RobotMap.CLIMBER_TIPPER_REVERSE, RobotMap.DUMPER, RobotMap.CLIMBER_TOP_LIMIT, RobotMap.CLIMBER_BOTTOM_LIMIT, RobotMap.CLIMBER_LOCK);
        }
        return instance;
    }
    private DoubleSolenoid tipper = null;
    private Solenoid lock = null;
    private Victor dumper = null;
    private Drive drive = null;
    private DigitalInput topLimit = null;
    private DigitalInput bottomLimit = null;
    private boolean topLimitHit = false;
    private boolean bottomLimitHit = false;
    private boolean inClimbMode = false;
    
    private Encoder encoder;
    
    private PIDController pid = null;
    
    private boolean lastSignPositive = false;

    private Climber(int tipperForward, int tipperReverse, int dumper, int topLimit, int bottomLimit, int lock) {
        drive = Drive.getInstance();
        this.tipper = new DoubleSolenoid(tipperForward, tipperReverse);
        this.dumper = new Victor(dumper);
        this.topLimit = new DigitalInput(topLimit);
        this.bottomLimit = new DigitalInput(bottomLimit);
        this.lock = new Solenoid(lock);
        
        this.encoder = drive.getPtoEncoder();
        this.encoder.reset();
        new Thread(this).start();
        
        pid = new PIDController(0.001,0,0, this, this);
        SmartDashboard.putData("climberPid", pid);
    }

    public void tip() {
        tipper.set(DoubleSolenoid.Value.kForward);
    }

    public void retractTipper() {
        tipper.set(DoubleSolenoid.Value.kReverse);
    }

    public void setDumper(double x) {
        dumper.set(x);
    }

    public void enableClimbingMode() {
        inClimbMode = true;
        encoder.reset();
        encoder.start();
        unlock();
    }

    public void disableClimbingMode() {
        inClimbMode = false;
    }
    
    public void runTraveller(double perc) {
        double power = 1 * perc;
        
        if(power > 0){
            if(topLimitHit){
                System.out.println("trying to go up, but the top limit switch is hit");
                drive.setPto(0);
            }
            else
                drive.setPto(power);
            
            if(!lastSignPositive){
                //countAtLastDirection = encoder.get();
            }
            lastSignPositive = true;
        }
        
        if(power < 0){
            if(bottomLimitHit){
                System.out.println("trying to go down, but the bottom limit switch is hit");
                drive.setPto(0);
            }
            else
                drive.setPto(power);
        }
        
    }
    
    public void lock(){
        lock.set(false);
    }
    
    public void unlock(){
        lock.set(true);
    }
    
    public boolean getTopLimit(){
        return topLimitHit;
    }
    
    public boolean getBottomLimit(){
        return bottomLimitHit;
    }

    public void run() {
        while (true) {
            if (inClimbMode) {
                if (!topLimit.get()) {
                    topLimitHit = true;
                } else if (topLimit.get()) {
                    topLimitHit = false;
                }

                if (!bottomLimit.get()) {
                    bottomLimitHit = true;
                } else if (bottomLimit.get()) {
                    bottomLimitHit = false;
                }

            }
            try {
                Thread.sleep(10);
            } catch (InterruptedException ex) {
                ex.printStackTrace();
            }
        }
    }

    public double pidGet() {
        return encoder.get();
    }

    public void pidWrite(double d) {
        runTraveller(d);
    }
}

@@ -607,30 +607,8 @@
 able
-, PIDSource, PIDOutput
  %7B%0D%0A
@@ -974,24 +974,30 @@
 ce;%0D%0A    %7D%0D%0A
+    %0D%0A
     private 
@@ -1358,138 +1358,8 @@
   %0D%0A
-    private Encoder encoder;%0D%0A    %0D%0A    private PIDController pid = null;%0D%0A    %0D%0A    private boolean lastSignPositive = false;%0D%0A%0D%0A
     
@@ -1797,228 +1797,31 @@
     
-this.encoder = drive.getPtoEncoder();%0D%0A        this.encoder.reset();%0D%0A        new Thread(this).start();%0D%0A        %0D%0A        pid = new PIDController(0.001,0,0, this, this);%0D%0A        SmartDashboard.putData(%22climberPid%22, pid
+new Thread(this).start(
 );%0D%0A
@@ -2156,60 +2156,8 @@
 e;%0D%0A
-        encoder.reset();%0D%0A        encoder.start();%0D%0A
     
@@ -2619,154 +2619,8 @@
   %0D%0A
-            if(!lastSignPositive)%7B%0D%0A                //countAtLastDirection = encoder.get();%0D%0A            %7D%0D%0A            lastSignPositive = true;%0D%0A
     
@@ -3880,148 +3880,7 @@
 %0D%0A%0D%0A
-    public double pidGet() %7B%0D%0A        return encoder.get();%0D%0A    %7D%0D%0A%0D%0A    public void pidWrite(double d) %7B%0D%0A        runTraveller(d);%0D%0A    %7D%0D%0A
 %7D%0D%0A

Disable encoders in Climber so that we can use them in drive in non-quadrature mode