package com.rmrobotics.library.hardware;

import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorController;

import static java.lang.Math.abs;

public class Motor {

    private static final double MAX_POWER = 1.0;
    private static final double MIN_POWER = 0.1;
    private static final double MAX_ACCEL = 0.03;
    private static final double NVRST20_ENC = 560;
    private static final double NVRST40_ENC = 1120;
    private static final double NVRST60_ENC = 1680;

    private DcMotor parent;
    private DcMotor.Direction defaultDirection;
    private double minPower;
    private double maxPower;
    private MOTOR_TYPE motorType;
    private double desiredPower;
    private double currentPower;
    private int curPos;
    private int tarPos;
    private int dis;

    public Motor(DcMotor dc, DcMotor.Direction d, double min, double max, MOTOR_TYPE mType) {
        parent = dc;
        defaultDirection = d;
        minPower = min;
        maxPower = max;
        motorType = mType;

    } //Todo add string to send in DbgLog confirming motor settings once configured

    public void setDesiredPower(double d) {
        desiredPower = d;
    }

    public void updateCurrentPower() {
        /**
         * let it be zero if it needs to be
         * absolute value and switch direction if needed
         * clip to be within range
         *
         */
        double absDesPower = abs(desiredPower);
        if (parent.getTargetPosition() == parent.getCurrentPosition() && parent.getMode() == DcMotorController.RunMode.RUN_TO_POSITION) {
            desiredPower = 0;
            currentPower = desiredPower;
        } else if (absDesPower == 0.0 || absDesPower < minPower) {
            currentPower = desiredPower;
        } else {
            if (desiredPower < 0.0) {
                if (defaultDirection == DcMotor.Direction.FORWARD) {
                    parent.setDirection(DcMotor.Direction.REVERSE);
                } else if (defaultDirection == DcMotor.Direction.REVERSE) {
                    parent.setDirection(DcMotor.Direction.FORWARD);
                }
            } else {
                if (defaultDirection == DcMotor.Direction.FORWARD) {
                    parent.setDirection(DcMotor.Direction.FORWARD);
                } else if (defaultDirection == DcMotor.Direction.REVERSE) {
                    parent.setDirection(DcMotor.Direction.REVERSE);
                }
            }
            desiredPower = absDesPower;
            if (desiredPower > maxPower) {
                desiredPower = maxPower;
            }
            currentPower = desiredPower;
        }
    }

    public void setCurrentPower() {
        if (desiredPower > currentPower) {
            if (currentPower < 0.2 && Math.abs(currentPower - desiredPower) > 0.2) {
                currentPower += MAX_ACCEL;
            } else {
                currentPower += 0.05*(Math.abs(currentPower - desiredPower));
            }
            parent.setPower(currentPower);
        } else if (desiredPower < currentPower) {
            if (currentPower > 0.08 && Math.abs(currentPower - desiredPower) > 0.2) {
                currentPower -= MAX_ACCEL;
            } else {
                currentPower -= 0.05*(Math.abs(currentPower - desiredPower));
            }
            parent.setPower(currentPower);
        } else {
            parent.setPower(currentPower);
        }
    }

    public void setEncoderMove(double rotation, double power) {
        curPos = parent.getCurrentPosition();
        tarPos = curPos + (int)(rotation * 1120);
        parent.setTargetPosition(tarPos);
        setDesiredPower(power);
    }

    public double getCurrentPosition() {
        return parent.getCurrentPosition();
    }

    public double getTargetPosition() {
        return parent.getTargetPosition();
    }

    public DcMotorController.RunMode getMode() {
        return parent.getMode();
    }

    public void resetEncoder() {
        parent.setMode(DcMotorController.RunMode.RESET_ENCODERS);
    }

    public void runToPosition() {
        parent.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
    }

    public void runWithoutEncoders() { parent.setMode(DcMotorController.RunMode.RUN_WITHOUT_ENCODERS); }

    public double getPower() { return parent.getPower(); }

    public double getDesiredPower() { return desiredPower; }

    public String getMotorType() {
        return motorType.toString();
    }

    public void stop() { setDesiredPower(0); }

}
@@ -347,16 +347,67 @@
 = 0.03;%0A
+    private static final double TETRIX_ENC = 1440;%0A
     priv
@@ -722,16 +722,47 @@
 orType;%0A
+    private double motorPulse;%0A
     priv
@@ -1108,16 +1108,51 @@
 = mType;
+%0A        motorPulse = motorPulse();
 %0A%0A    %7D 
@@ -2690,24 +2690,108 @@
 = maxPower;%0A
+            %7D else if (desiredPower %3C minPower) %7B%0A                desiredPower = 0;%0A
             
@@ -2933,36 +2933,32 @@
 ) %7B%0A            
-if (
 currentPower %3C 0
@@ -2958,66 +2958,22 @@
 wer 
-%3C 0.2 && Math.abs(currentPower - desiredPower) %3E 0.2) %7B%0A  
++= MAX_ACCEL;%0A
     
@@ -2972,34 +2972,36 @@
 EL;%0A            
-  
+if (
 currentPower += 
@@ -3001,40 +3001,19 @@
 wer 
-+= MAX_ACCEL;%0A            %7D else
+%3E maxPower)
  %7B%0A 
@@ -3044,55 +3044,18 @@
 wer 
-+
 = 
-0.05*(Math.abs(currentPower - desired
+max
 Power
-))
 ;%0A  
@@ -3163,36 +3163,32 @@
 ) %7B%0A            
-if (
 currentPower %3E 0
@@ -3188,64 +3188,21 @@
 wer 
-%3E 0.08 && Math.abs(currentPower - desiredPower) %3E 0.2) %7B
+-= MAX_ACCEL;
 %0A   
@@ -3202,36 +3202,36 @@
 EL;%0A            
-    
+if (
 currentPower -= 
@@ -3231,40 +3231,19 @@
 wer 
--= MAX_ACCEL;%0A            %7D else
+%3C minPower)
  %7B%0A 
@@ -3274,55 +3274,11 @@
 wer 
--
 = 0
-.05*(Math.abs(currentPower - desiredPower))
 ;%0A  
@@ -3642,32 +3642,357 @@
 (power);%0A    %7D%0A%0A
+    private double motorPulse() %7B%0A        switch (motorType) %7B%0A            case NVRST_20:%0A                return NVRST20_ENC;%0A            case NVRST_40:%0A                return NVRST40_ENC;%0A            case NVRST_60:%0A                return NVRST60_ENC;%0A            default:%0A                return TETRIX_ENC;%0A        %7D%0A    %7D%0A%0A
     public doubl

Add motorType to motor constructor: includes NVRST_20, NVRST_40, NVRST_60, TETRIX(null also becomes tetrix)