# Template Java Code

package frc.robot;

import com.ctre.phoenix.motorcontrol.*;
import com.ctre.phoenix.motorcontrol.can.*;
import com.ctre.phoenix.motorcontrol.can.TalonFX;
import com.ctre.phoenix.sensors.CANCoder;
import com.kauailabs.navx.frc.AHRS;
import edu.wpi.first.wpilibj.Servo;

import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.SPI;
import edu.wpi.first.wpilibj.I2C;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

public class Robot extends TimedRobot {
  // 移動motorX4
  public static TalonSRX mLeftFront = new TalonSRX(3);
  public static TalonSRX mLeftBack = new TalonSRX(2);
  public static TalonSRX mRightFront = new TalonSRX(0);
  public static TalonSRX mRightBack = new TalonSRX(1);
  // 射球轉仰角motor
  public static TalonSRX aimVert = new TalonSRX(7);
  // 射球轉盤motor
  public static TalonSRX aimHori = new TalonSRX(8);
  // 射球 5 follow 6
  public static TalonFX shooter1 = new TalonFX(5);
  public static TalonFX shooter2 = new TalonFX(6);
  // ardunio i2c
  public static I2C wire = new I2C(I2C.Port.kOnboard, 4);
  // navx
  public static AHRS ahrs = new AHRS(SPI.Port.kMXP);
  // 搖桿
  public static Joystick js1 = new Joystick(0);
  // 左蘑菇
  public final int leftStick_x = 0;
  public final int leftStick_y = 1;
  // 右蘑菇
  public final int rightStick_x = 4;
  public final int rightStick_y = 5;
  // limelight
  NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight");
  NetworkTableEntry tx = table.getEntry("tx");
  NetworkTableEntry ty = table.getEntry("ty");
  NetworkTableEntry ta = table.getEntry("ta");

  double tkp=0.015,tki=0.00,tkd=0.015;
  double dkp=0.04,dki=0.00,dkd=0.01;
  PIDController turnController = new PIDController(tkp, tki, tkd);
  PIDController driveController = new PIDController(dkp, dki, dkd);
  double temp;
  
  public void drive(double temp, double aim){
    double derror = driveController.calculate(ahrs.getAngle(),temp);
    mLeftBack.set(ControlMode.PercentOutput, (-js1.getRawAxis(rightStick_y)+derror-aim)*0.5);
    mLeftFront.set(ControlMode.PercentOutput, (-js1.getRawAxis(rightStick_y)+derror-aim)*0.5);
    mRightBack.set(ControlMode.PercentOutput, (-js1.getRawAxis(rightStick_y)-derror+aim)*0.5);
    mRightFront.set(ControlMode.PercentOutput, (-js1.getRawAxis(rightStick_y)-derror+aim)*0.5);
  }

  public double aim() {
    double error = tx.getDouble(100);
    if (error==100) error = 0;
    else error=turnController.calculate(error);
    return error*js1.getRawAxis(3);
  }

  public void macDrive(double temp, double aim){
    double derror = driveController.calculate(ahrs.getAngle(), temp);
    double macy = definition.sinaplb(Math.atan2(-js1.getRawAxis(leftStick_y),js1.getRawAxis(leftStick_x)),Math.toRadians(ahrs.getAngle()));
    double macx = definition.cosaplb(Math.atan2(-js1.getRawAxis(leftStick_y),js1.getRawAxis(leftStick_x)),Math.toRadians(ahrs.getAngle()));
    mLeftBack.set(ControlMode.PercentOutput, (macy - macx + derror) * 1);
    mLeftFront.set(ControlMode.PercentOutput, (macy + macx + derror) * 1);
    mRightBack.set(ControlMode.PercentOutput, (macy + macx - derror) * 1);
    mRightFront.set(ControlMode.PercentOutput, (macy - macx - derror) * 1);
  }

  @Override
  public void robotInit() {
    mLeftFront.setInverted(false);
    mLeftBack.setInverted(false);
    mRightFront.setInverted(true);
    mRightBack.setInverted(true);
    shooter1.setInverted(true);
    shooter2.setInverted(false);
    shooter2.follow(shooter1);
    ahrs.reset();
    driveController.setSetpoint(0.0);
    turnController.setSetpoint(0.0);

    shooter1.configPeakOutputForward(1, 30);
    shooter1.configPeakOutputReverse(-1, 30);
    
    shooter1.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder, 0, 30);
    shooter1.setSelectedSensorPosition(0,  0, 30);

    aimVert.configPeakOutputForward(0.4, 30);
    aimVert.configPeakOutputReverse(-0.4, 30);

    aimVert.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder, 0, 30);
    aimVert.setSelectedSensorPosition(0, 0, 30);

    aimHori.configPeakOutputForward(0.4, 30);
    aimHori.configPeakOutputReverse(-0.4, 30);

    aimHori.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder, 0, 30);
    aimHori.setSelectedSensorPosition(0, 0, 30);
    
  }


  @Override
  public void robotPeriodic() {
  }

  @Override
  public void autonomousInit() {
    ahrs.reset();
  }


  @Override
  public void autonomousPeriodic() {
    double tempdistant = (1/Math.atan(ty.getDouble(0)-22))*240.55;
    SmartDashboard.putNumber("distance", tempdistant);
  }

  @Override

  public void teleopInit() {
    ahrs.reset();
    temp=0;
  }

  @Override
  public void teleopPeriodic() {
    temp += js1.getRawAxis(rightStick_x) * 2;
    double derror = driveController.calculate(ahrs.getAngle(), temp);
    mLeftBack.set(ControlMode.PercentOutput, (definition.sinaplb(Math.atan2(-js1.getRawAxis(leftStick_y),js1.getRawAxis(leftStick_x)),Math.toRadians(ahrs.getAngle())) + derror) * 1);
    mLeftFront.set(ControlMode.PercentOutput, (-js1.getRawAxis(leftStick_y)+js1.getRawAxis(leftStick_x)+derror)*1);
    mRightBack.set(ControlMode.PercentOutput, (-js1.getRawAxis(leftStick_y)+js1.getRawAxis(leftStick_x)-derror)*1);
    mRightFront.set(ControlMode.PercentOutput, (-js1.getRawAxis(leftStick_y)-js1.getRawAxis(leftStick_x)-derror)*1);
  }

  @Override
  public void testInit() {
    /*aimVert.setSelectedSensorPosition(0, 0, 30);
    aimHori.setSelectedSensorPosition(0, 0, 30);*/
  }

  @Override
  public void testPeriodic() {
    shooter1.set(ControlMode.Velocity, 1000*js1.getRawAxis(3));
    SmartDashboard.putNumber("shooter1", shooter1.getSelectedSensorVelocity());
    /*SmartDashboard.putNumber("shooter2", shooter2.getSelectedSensorVelocity());
    SmartDashboard.putNumber("position", aimHori.getSelectedSensorPosition());*/


    /*String WriteString;
    if (js1.getRawButton(4)) WriteString = "go";
    else WriteString = "stop";
    // Turn the string into a character array
    char[] CharArray = WriteString.toCharArray();
    // Make each character a single byte
    byte[] WriteData = new byte[CharArray.length];
    // For each item in the character array, add it as a single byte to the I2C connection
    for (int i = 0; i < CharArray.length; i++) {
      WriteData[i] = (byte) CharArray[i];
    }

    byte[] ReceiveData = new byte[100];
    // Send the data to the I2C connection
    wire.transaction(WriteData, WriteData.length, ReceiveData, ReceiveData.length);
} =