package org.firstinspires.ftc.teamcode;

import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cRangeSensor;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.ColorSensor;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DistanceSensor;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.util.ElapsedTime;

/**
 * This is NOT an opmode.
 *
 * This class defines all the specific hardware for the 6417 Mechanum Drive Robot.
 */

public class HardwareMech
{
    /* Robot Hardware Members */
    public DcMotor  leftFront   = null;
    public DcMotor  rightFront  = null;
    public DcMotor  leftRear   = null;
    public DcMotor  rightRear  = null;

    public DcMotor  liftMotor  = null;
    public DcMotor  slideMotor = null;

    public Servo    clawLeft    = null;
    public Servo    clawRight   = null;

    public Servo    tailServoY   = null;
    public Servo    tailServoX   = null;

    public Servo    arm         = null;
    public Servo    armClaw     = null;

    public CRServo  leftBotWheel = null;
    public CRServo  rightBotWheel = null;
    public CRServo  leftTopWheel = null;
    public CRServo  rightTopWheel = null;

    public ColorSensor colorSensor = null;
    DistanceSensor glyphSensor;
    public ModernRoboticsI2cRangeSensor rangeSensor = null;


    /* Robot Constants */
    public static final double LIFT_UP_POWER    =  0.9;
    public static final double LIFT_DOWN_POWER  = -0.7;

    public static final double SLIDE_OUT_POWER    =  0.70; //.75
    public static final double SLIDE_IN_POWER  = 0.5; //.6

    public static final double ARM_UP    =  0.80; //MAX ARM UP POSITION
    public static final double ARM_DOWN  =  0.15; //MAX ARM DOWN POSITION
    public static final double ARM_OPENED    =  0.45;
    public static final double ARM_CLOSED  = 1.0;

    public static final double CLAW_LEFT_INIT   =  0.40;
    public static final double CLAW_LEFT_MAX   =  0.65; //original 0.65
    public static final double CLAW_LEFT_MIN   =  0.05;

    public static final double CLAW_RIGHT_INIT   =  0.40; //CLAWS FORMING A SQUARE SHAPE (|  |)
    public static final double CLAW_RIGHT_MAX   =  0.65; //CLOSED REASONABLY ENOUGH (SO THAT CLAWS FORM A V SHAPE) BUT NOT FULLY
    public static final double CLAW_RIGHT_MIN   =  0.05; //FULLY OPEN

    public static final double TAIL_UP   =  0.90;
    public static final double TAIL_DOWN   =  0.30;

    public static final double TAIL_INIT   =  0.45;
    public static final double TAIL_LEFT   =  0.60;
    public static final double TAIL_RIGHT   =  0.30;


    /* local software members/helpers */
    HardwareMap hwMap           =  null;
    private ElapsedTime period  = new ElapsedTime();

    /* Constructor */
    public HardwareMech(){
    }

    /* Initialize standard Hardware interfaces */
    public void define(HardwareMap ahwMap) {

        // Save reference to Hardware map
        hwMap = ahwMap;


        // Define and Initialize Motors
        leftFront   = hwMap.dcMotor.get("front_left");
        rightFront  = hwMap.dcMotor.get("front_right");
        leftFront.setDirection(DcMotor.Direction.REVERSE); // Set to REVERSE if using AndyMark motors
        rightFront.setDirection(DcMotor.Direction.FORWARD);// Set to FORWARD if using AndyMark motors
        leftRear   = hwMap.dcMotor.get("back_left");
        rightRear  = hwMap.dcMotor.get("back_right");
        leftRear.setDirection(DcMotor.Direction.REVERSE); // Set to REVERSE if using AndyMark motors
        rightRear.setDirection(DcMotor.Direction.FORWARD);// Set to FORWARD if using AndyMark motors

        leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        leftRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        rightRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);

        liftMotor  = hwMap.dcMotor.get("lift");
        liftMotor.setDirection(DcMotor.Direction.REVERSE);
        liftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        slideMotor  = hwMap.dcMotor.get("slide");
        slideMotor.setDirection(DcMotor.Direction.REVERSE);

        // Set all motors to zero power
        leftFront.setPower(0);
        rightFront.setPower(0);
        leftRear.setPower(0);
        rightRear.setPower(0);
        liftMotor.setPower(0);
        slideMotor.setPower(0);
        slideMotor.setPower(0);

        leftFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        rightFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        leftRear.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        rightRear.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);

        // Set all motors to run without encoders.
        // May want to use RUN_USING_ENCODER if encoders are installed.
        // Use RUN_WITHOUT_ENCODER if no encoders
        //TODO: encoders or nah?
        leftFront.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        rightFront.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        //leftMotor.setMaxSpeed(10);
        //rightMotor.setMaxSpeed(10);
        //armMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
        leftRear.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        rightRear.setMode(DcMotor.RunMode.RUN_USING_ENCODER);

        liftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        liftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
        slideMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        slideMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);


        // Define Sensors
        rangeSensor = hwMap.get(ModernRoboticsI2cRangeSensor.class, "range");
        colorSensor = hwMap.colorSensor.get("color");
        glyphSensor = hwMap.get(DistanceSensor.class, "glyph");

        // Define and initialize all installed servos.
        clawLeft = hwMap.servo.get("claw_left");
        clawRight = hwMap.servo.get("claw_right");
        clawRight.setDirection(Servo.Direction.REVERSE);

        leftBotWheel = hwMap.crservo.get("left_wheel_bot");
        rightBotWheel = hwMap.crservo.get("right_wheel_bot");
        rightBotWheel.setDirection(CRServo.Direction.REVERSE);
        leftTopWheel = hwMap.crservo.get("left_wheel_top");
        rightTopWheel = hwMap.crservo.get("right_wheel_top");
        rightTopWheel.setDirection(CRServo.Direction.REVERSE);

        tailServoY = hwMap.servo.get("tail_Y");
        tailServoX = hwMap.servo.get("tail_X");

        arm = hwMap.servo.get("arm");
        armClaw = hwMap.servo.get("armclaw");

    }

    public void init(HardwareMap ahwMap) {

        define(ahwMap);
        moveServos();

    }

    public void moveServos(){

        clawLeft.setPosition(CLAW_LEFT_MIN);
        clawRight.setPosition(CLAW_RIGHT_MIN);

        tailServoX.setPosition(TAIL_INIT);
        tailServoY.setPosition(TAIL_UP);

        arm.setPosition(ARM_DOWN);
        armClaw.setPosition(ARM_CLOSED);

    }

    /***
     * waitForTick implements a periodic delay. However, this acts like a metronome with a regular
     * periodic tick.  This is used to compensate for varying processing times for each cycle.
     * The function looks at the elapsed cycle time, and sleeps for the remaining time interval.
     *
     * @param periodMs  Length of wait cycle in mSec.
     */
    public void waitForTick(long periodMs) {
        long  remaining = periodMs - (long)period.milliseconds();
        // sleep for the remaining portion of the regular cycle period.
        if (remaining > 0) {
            try {
                Thread.sleep(remaining);
            } catch (InterruptedException e) {
                Thread.currentThread().interrupt();
            }
        }
        // Reset the cycle clock for the next pass.
        period.reset();
    }

    /*
	 * This method scales the joystick input so for low joystick values, the
	 * scaled value is less than linear.  This is to make it easier to drive
	 * the robot more precisely at slower speeds.
	 */
    double scaleInput(double dVal)  {
        double[] scaleArray = { 0.0, 0.05, 0.09, 0.10, 0.12, 0.15, 0.18, 0.24,
                0.30, 0.36, 0.43, 0.50, 0.60, 0.72, 0.85, 1.00, 1.00 };

        // get the corresponding index for the scaleInput array.
        int index = (int) (dVal * 16.0);
        if (index < 0) {
            index = -index;
        } else if (index > 16) {
            index = 16;
        }

        double dScale = 0.0;
        if (dVal < 0) {
            dScale = -scaleArray[index];
        } else {
            dScale = scaleArray[index];
        }

        return dScale;
    }
}

