package com.qualcomm.modernrobotics;

import com.qualcomm.robotcore.eventloop.EventLoopManager;
import com.qualcomm.robotcore.exception.RobotCoreException;
import com.qualcomm.robotcore.hardware.ServoController;
import com.qualcomm.robotcore.hardware.ServoController.PwmStatus;
import com.qualcomm.robotcore.hardware.usb.RobotUsbDevice;
import com.qualcomm.robotcore.util.Range;
import com.qualcomm.robotcore.util.SerialNumber;
import com.qualcomm.robotcore.util.TypeConversion;

class r
  extends o
  implements ServoController
{
  public static final byte[] a = { -1, 66, 67, 68, 69, 70, 71 };

  protected r(SerialNumber paramSerialNumber, RobotUsbDevice paramRobotUsbDevice, EventLoopManager paramEventLoopManager)
    throws RobotCoreException, InterruptedException
  {
    super(paramSerialNumber, paramEventLoopManager, new s(paramSerialNumber, paramRobotUsbDevice, 9, 64, false));

    pwmDisable();
  }

  public String getDeviceName()
  {
    return "Modern Robotics USB Servo Controller";
  }

  public String getConnectionInfo()
  {
    return "USB " + getSerialNumber();
  }

  public void close()
  {
    pwmDisable();
    super.close();
  }

  public void pwmEnable()
  {
    a(72, (byte)0);
  }

  public void pwmDisable()
  {
    a(72, (byte)-1);
  }

  public ServoController.PwmStatus getPwmStatus()
  {
    byte[] arrayOfByte = c(72, 1);
    if (arrayOfByte[0] == -1) {
      return ServoController.PwmStatus.DISABLED;
    }
    return ServoController.PwmStatus.ENABLED;
  }

  public void setServoPosition(int channel, double position)
  {
    c(channel);
    Range.throwIfRangeIsInvalid(position, 0.0D, 1.0D);

    a(a[channel], position * 240.0D);
    pwmEnable();
  }

  public double getServoPosition(int channel)
  {
    c(channel);

    byte[] arrayOfByte = c(a[channel], 1);

    return TypeConversion.unsignedByteToDouble(arrayOfByte[0]) / 240.0D;
  }

  private void c(int paramInt)
  {
    if ((paramInt < 1) || (paramInt > a.length)) {
      throw new IllegalArgumentException(String.format("Channel %d is invalid; valid channels are 1..%d", new Object[] {Integer.valueOf(paramInt), Integer.valueOf(6) }));
    }
  }
}