package org.TexasTorque.TexasTorque2013.subsystem.drivebase;

import org.TexasTorque.TexasTorque2013.TorqueSubsystem;
import org.TexasTorque.TexasTorque2013.constants.Constants;

public class Drivebase extends TorqueSubsystem
{   
    private static Drivebase instance;
    
    private double leftDriveSpeed;
    private double rightDriveSpeed;
    
    private boolean shiftState;
    
    public double highSensitivity;
    public double lowSensitivity;
    
    public static Drivebase getInstance()
    {
        return (instance == null) ? instance = new Drivebase() : instance;
    }
            
    private Drivebase()
    {
        super();
        
        leftDriveSpeed = Constants.MOTOR_STOPPED;
        rightDriveSpeed = Constants.MOTOR_STOPPED;
        
        shiftState = Constants.LOW_GEAR;
    }
    
    public void run()
    {
        if(dashboardManager.getDS().isOperatorControl())
        {
           mixChannels(driverInput.getThrottle(), driverInput.getTurn());
           shiftState = driverInput.shiftHighGear();
        }
        
        robotOutput.setShifters(shiftState);
        robotOutput.setDriveMotors(leftDriveSpeed, rightDriveSpeed);
    }
    
    public void setDriveSpeeds(double leftSpeed, double rightSpeed)
    {
        leftDriveSpeed = leftSpeed;
        rightDriveSpeed = rightSpeed;
    }
    
    public void setShifters(boolean highGear)
    {
        if(highGear != shiftState)
        {
            shiftState = highGear;
        }
    }
    
    private void mixChannels(double yAxis, double xAxis)
    {
        yAxis = driverInput.applyDeadband(yAxis, Constants.SPEED_AXIS_DEADBAND);
        xAxis = driverInput.applyDeadband(xAxis, Constants.TURN_AXIS_DEADBAND);
        
        simpleDrive(yAxis, xAxis);
    }
    
    private void simpleDrive(double yAxis, double xAxis)
    {
        yAxis = applySqrtCurve(yAxis);
        xAxis = applySqrtCurve(xAxis);
        
        double leftSpeed = yAxis + xAxis;
        double rightSpeed = yAxis - xAxis;
        
        setDriveSpeeds(leftSpeed, rightSpeed);
    }
    
    private double applySqrtCurve(double axisValue)
    {
        int sign = (axisValue > 0) ? 1 : -1;
        axisValue = Math.sqrt(Math.abs(axisValue)) * sign;
        return axisValue;
    }
    
    public String getKeyNames()
    {
        String names = "LeftDriveSpeed,LeftDriveEncoderPosition,LeftDriveEncoderVelocity,"
                + "RightDriveSpeed,RightDriveEncoderPosition,RightDriveEncoderVelocity,"
                + "GyroAngle,ShiftState";
        
        return names;
    }
    
    public String logData()
    {
        String data = leftDriveSpeed + ",";
        data += sensorInput.getLeftDriveEncoder() + ",";
        data += sensorInput.getLeftDriveEncoderRate() + ",";
        
        data += rightDriveSpeed + ",";
        data += sensorInput.getRightDriveEncoder() + ",";
        data += sensorInput.getRightDriveEncoderRate() + ",";
        
        data += sensorInput.getGyroAngle() + ",";
        data += shiftState + ",";
        
        return data;
    }
    
    public void loadParameters()
    {
        highSensitivity = params.getAsDouble("D_HighSensitivity", Constants.DEFAULT_HIGH_SENSITIVITY);
        lowSensitivity = params.getAsDouble("D_LowSensitivity", Constants.DEFAULT_LOW_SENSITIVITY);
    }
}

@@ -170,16 +170,69 @@
 nstants;
+%0Aimport org.TexasTorque.TorqueLib.controlLoop.SimPID;
 %0A%0Apublic
@@ -317,24 +317,88 @@
 tance;%0A    %0A
+    private SimPID encoderPID;%0A    private SimPID gyroPID;%0A    %0A
     private 
@@ -762,32 +762,108 @@
 per();%0A        %0A
+        encoderPID = new SimPID();%0A        gyroPID = new SimPID();%0A        %0A
         leftDriv
@@ -3504,20 +3504,1033 @@
 NSITIVITY);%0A
+        %0A        double p = params.getAsDouble(%22D_DriveEncoderP%22, 0.0);%0A        double i = params.getAsDouble(%22D_DriveEncoderI%22, 0.0);%0A        double d = params.getAsDouble(%22D_DriveEncoderD%22, 0.0);%0A        double e = params.getAsDouble(%22D_DriveEncoderEpsilon%22, 0.0);%0A        double r = params.getAsDouble(%22D_DriveEncoderDoneRange%22, 0.0);%0A        %0A        encoderPID.setConstants(p, i, d);%0A        encoderPID.setErrorEpsilon(e);%0A        encoderPID.setDoneRange(r);%0A        encoderPID.resetErrorSum();%0A        encoderPID.resetPreviousVal();%0A        %0A        p = params.getAsDouble(%22D_DriveGyroP%22, 0.0);%0A        i = params.getAsDouble(%22D_DriveGyroI%22, 0.0);%0A        d = params.getAsDouble(%22D_DriveGyroD%22, 0.0);%0A        e = params.getAsDouble(%22D_DriveGyroEpsilon%22, 0.0);%0A        r = params.getAsDouble(%22D_DriveGyroDoneRange%22, 0.0);%0A        %0A        gyroPID.setConstants(p, i, d);%0A        gyroPID.setErrorEpsilon(e);%0A        gyroPID.setDoneRange(r);%0A        gyroPID.resetErrorSum();%0A        gyroPID.resetPreviousVal();%0A
     %7D%0A%7D%0A

put drive pid stuff into drivebase