@Test
  public void addCommandTest() {
    Command cmd = createMock(MoveCommand.class);
    Drone drone = Drone.getInstance();
    ThreadCmd tc = new ThreadCmd(drone);

    expect(cmd.isUnique()).andReturn(true);
    replayAll();
    tc.addCommand(cmd);
    verifyAll();
  }
  @Test
  public void setAxisMoveSingleParameterWrongTest() {
    Drone drone = Drone.getInstance();

    ThreadCmd tc = new ThreadCmd(drone);
    float prevValueState = 0;
    float testValue = 0;

    Whitebox.setInternalState(tc, "move_armed", 0);
    Whitebox.setInternalState(tc, "move_roll", 0);
    Whitebox.setInternalState(tc, "move_pitch", 0);
    Whitebox.setInternalState(tc, "move_altitude", 0);
    Whitebox.setInternalState(tc, "move_yaw", 0);

    // move_roll
    prevValueState = Whitebox.getInternalState(tc, "move_roll");
    tc.setAxisMove(-1.0001f, 0, 0, 0);
    testValue = Whitebox.getInternalState(tc, "move_roll");
    Assert.assertEquals(prevValueState, testValue);

    prevValueState = Whitebox.getInternalState(tc, "move_roll");
    tc.setAxisMove(1.0001f, 0, 0, 0);
    testValue = Whitebox.getInternalState(tc, "move_roll");
    Assert.assertEquals(prevValueState, testValue);

    // move_pitch
    prevValueState = Whitebox.getInternalState(tc, "move_pitch");
    tc.setAxisMove(0, -1.0001f, 0, 0);
    testValue = Whitebox.getInternalState(tc, "move_pitch");
    Assert.assertEquals(prevValueState, testValue);

    prevValueState = Whitebox.getInternalState(tc, "move_pitch");
    tc.setAxisMove(0, 1.0001f, 0, 0);
    testValue = Whitebox.getInternalState(tc, "move_pitch");
    Assert.assertEquals(prevValueState, testValue);

    // move_altitude
    prevValueState = Whitebox.getInternalState(tc, "move_altitude");
    tc.setAxisMove(0, -1.0001f, 0, 0);
    testValue = Whitebox.getInternalState(tc, "move_altitude");
    Assert.assertEquals(prevValueState, testValue);

    prevValueState = Whitebox.getInternalState(tc, "move_altitude");
    tc.setAxisMove(0, 1.0001f, 0, 0);
    testValue = Whitebox.getInternalState(tc, "move_altitude");
    Assert.assertEquals(prevValueState, testValue);

    // move_yaw
    prevValueState = Whitebox.getInternalState(tc, "move_yaw");
    tc.setAxisMove(0, -1.0001f, 0, 0);
    testValue = Whitebox.getInternalState(tc, "move_yaw");
    Assert.assertEquals(prevValueState, testValue);

    prevValueState = Whitebox.getInternalState(tc, "move_yaw");
    tc.setAxisMove(0, 1.0001f, 0, 0);
    testValue = Whitebox.getInternalState(tc, "move_yaw");
    Assert.assertEquals(prevValueState, testValue);
  }
  @Test
  public void wifiConnectThreadStartTest() throws Exception {
    WiFiConnection wifi = createMock(WiFiConnection.class);
    NavData navData = new NavData();
    PacketDemo packetDemo = new PacketDemo();
    mockStatic(Tools.class);

    wifi.connect();
    expectLastCall();
    expect(Tools.get_state_bit(navData.drone_state, 6)).andReturn(false);
    wifi.sendAtCommand("AT*CONFIG=#SEQ#,\"custom:session_id\",\"ca0000d1\"");
    expectLastCall();
    expect(Tools.get_state_bit(navData.drone_state, 6)).andReturn(true);
    wifi.sendAtCommand("AT*CTRL=#SEQ#,5,0");
    expectLastCall();
    expect(wifi.isRunning()).andReturn(true).anyTimes();
    //		wifi.setRunning(false);
    //		expectLastCall();

    replayAll();
    Drone drone = Drone.getInstance();
    Whitebox.setInternalState(drone, "wifi", wifi);
    Whitebox.setInternalState(navData, "drone_state", 0);
    Whitebox.setInternalState(drone, "navData", navData);
    drone.connect();
    //	Thread.sleep(500);
    ThreadNavData threadNavData = Whitebox.getInternalState(drone, "threadNavData");
    ThreadCmd threadCmd = Whitebox.getInternalState(drone, "threadCmd");

    //		threadNavData.stop();
    assertFalse("Drone.threadCmd is in state NEW", threadCmd.getState() == Thread.State.NEW);
    assertFalse(
        "Drone.threadCmd is in state TERMINATED", threadCmd.getState() == Thread.State.TERMINATED);
    assertFalse(
        "Drone.threadNavData is in state NEW", threadNavData.getState() == Thread.State.NEW);
    assertFalse(
        "Drone.threadNavData is in state TERMINATED",
        threadNavData.getState() == Thread.State.TERMINATED);
    threadCmd.stop();
    threadNavData.stop();
    verifyAll();
  }
  @SuppressWarnings("deprecation")
  @Test
  public void setAxisMoveParametersRightEdgeTest() {
    Drone drone = Drone.getInstance();
    ThreadCmd tc = new ThreadCmd(drone);
    int armed = 0;
    float roll = 0;
    float pitch = 0;
    float altitude = 0;
    float yaw = 0;

    // Min-Test
    Whitebox.setInternalState(tc, "move_armed", armed);
    Whitebox.setInternalState(tc, "move_roll", roll);
    Whitebox.setInternalState(tc, "move_pitch", pitch);
    Whitebox.setInternalState(tc, "move_altitude", altitude);
    Whitebox.setInternalState(tc, "move_yaw", yaw);

    tc.setAxisMove(-1, -1, -1, -1);

    armed = Whitebox.getInternalState(tc, "move_armed");
    roll = Whitebox.getInternalState(tc, "move_roll");
    pitch = Whitebox.getInternalState(tc, "move_pitch");
    altitude = Whitebox.getInternalState(tc, "move_altitude");
    yaw = Whitebox.getInternalState(tc, "move_yaw");

    Assert.assertEquals(1, armed);
    Assert.assertEquals(-1f, roll);
    Assert.assertEquals(-1f, pitch);
    Assert.assertEquals(-1f, altitude);
    Assert.assertEquals(-1f, yaw);

    // Zero Test
    armed = 0;
    roll = 0;
    pitch = 0;
    altitude = 0;
    yaw = 0;

    Whitebox.setInternalState(tc, "move_armed", armed);
    Whitebox.setInternalState(tc, "move_roll", roll);
    Whitebox.setInternalState(tc, "move_pitch", pitch);
    Whitebox.setInternalState(tc, "move_altitude", altitude);
    Whitebox.setInternalState(tc, "move_yaw", yaw);

    tc.setAxisMove(0, 0, 0, 0);

    armed = Whitebox.getInternalState(tc, "move_armed");
    roll = Whitebox.getInternalState(tc, "move_roll");
    pitch = Whitebox.getInternalState(tc, "move_pitch");
    altitude = Whitebox.getInternalState(tc, "move_altitude");
    yaw = Whitebox.getInternalState(tc, "move_yaw");

    Assert.assertEquals(0, armed);
    Assert.assertEquals(0f, roll);
    Assert.assertEquals(0f, pitch);
    Assert.assertEquals(0f, altitude);
    Assert.assertEquals(0f, yaw);

    // Max Test
    armed = 0;
    roll = 0;
    pitch = 0;
    altitude = 0;
    yaw = 0;

    Whitebox.setInternalState(tc, "move_armed", armed);
    Whitebox.setInternalState(tc, "move_roll", roll);
    Whitebox.setInternalState(tc, "move_pitch", pitch);
    Whitebox.setInternalState(tc, "move_altitude", altitude);
    Whitebox.setInternalState(tc, "move_yaw", yaw);

    tc.setAxisMove(1, 1, 1, 1);

    armed = Whitebox.getInternalState(tc, "move_armed");
    roll = Whitebox.getInternalState(tc, "move_roll");
    pitch = Whitebox.getInternalState(tc, "move_pitch");
    altitude = Whitebox.getInternalState(tc, "move_altitude");
    yaw = Whitebox.getInternalState(tc, "move_yaw");

    Assert.assertEquals(1, armed);
    Assert.assertEquals(1f, roll);
    Assert.assertEquals(1f, pitch);
    Assert.assertEquals(1f, altitude);
    Assert.assertEquals(1f, yaw);
  }