// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

#include "TestBench.h"
#include "frc/Encoder.h"
#include "frc/Timer.h"
#include "frc/motorcontrol/Jaguar.h"
#include "frc/motorcontrol/Talon.h"
#include "frc/motorcontrol/Victor.h"
#include "gtest/gtest.h"

using namespace frc;

enum MotorInvertingTestType { TEST_VICTOR, TEST_JAGUAR, TEST_TALON };
static const double motorSpeed = 0.15;
static const double delayTime = 0.5;
std::ostream& operator<<(std::ostream& os, MotorInvertingTestType const& type) {
  switch (type) {
    case TEST_VICTOR:
      os << "Victor";
      break;
    case TEST_JAGUAR:
      os << "Jaguar";
      break;
    case TEST_TALON:
      os << "Talon";
      break;
  }

  return os;
}
class MotorInvertingTest
    : public testing::TestWithParam<MotorInvertingTestType> {
 protected:
  MotorController* m_motorController;
  Encoder* m_encoder;

  void SetUp() override {
    switch (GetParam()) {
      case TEST_VICTOR:
        m_motorController = new Victor(TestBench::kVictorChannel);
        m_encoder = new Encoder(TestBench::kVictorEncoderChannelA,
                                TestBench::kVictorEncoderChannelB);
        break;

      case TEST_JAGUAR:
        m_motorController = new Jaguar(TestBench::kJaguarChannel);
        m_encoder = new Encoder(TestBench::kJaguarEncoderChannelA,
                                TestBench::kJaguarEncoderChannelB);
        break;

      case TEST_TALON:
        m_motorController = new Talon(TestBench::kTalonChannel);
        m_encoder = new Encoder(TestBench::kTalonEncoderChannelA,
                                TestBench::kTalonEncoderChannelB);
        break;
    }
  }

  void TearDown() override {
    delete m_motorController;
    delete m_encoder;
  }

  void Reset() {
    m_motorController->SetInverted(false);
    m_motorController->Set(0.0);
    m_encoder->Reset();
  }
};

TEST_P(MotorInvertingTest, InvertingPositive) {
  Reset();

  m_motorController->Set(motorSpeed);

  Wait(delayTime);

  bool initDirection = m_encoder->GetDirection();
  m_motorController->SetInverted(true);
  m_motorController->Set(motorSpeed);

  Wait(delayTime);

  EXPECT_TRUE(m_encoder->GetDirection() != initDirection)
      << "Inverting with Positive value does not change direction";

  Reset();
}

TEST_P(MotorInvertingTest, InvertingNegative) {
  Reset();

  m_motorController->SetInverted(false);
  m_motorController->Set(-motorSpeed);

  Wait(delayTime);

  bool initDirection = m_encoder->GetDirection();
  m_motorController->SetInverted(true);
  m_motorController->Set(-motorSpeed);

  Wait(delayTime);

  EXPECT_TRUE(m_encoder->GetDirection() != initDirection)
      << "Inverting with Negative value does not change direction";

  Reset();
}

TEST_P(MotorInvertingTest, InvertingSwitchingPosToNeg) {
  Reset();

  m_motorController->SetInverted(false);
  m_motorController->Set(motorSpeed);

  Wait(delayTime);

  bool initDirection = m_encoder->GetDirection();
  m_motorController->SetInverted(true);
  m_motorController->Set(-motorSpeed);

  Wait(delayTime);

  EXPECT_TRUE(m_encoder->GetDirection() == initDirection)
      << "Inverting with Switching value does change direction";

  Reset();
}

TEST_P(MotorInvertingTest, InvertingSwitchingNegToPos) {
  Reset();

  m_motorController->SetInverted(false);
  m_motorController->Set(-motorSpeed);

  Wait(delayTime);

  bool initDirection = m_encoder->GetDirection();
  m_motorController->SetInverted(true);
  m_motorController->Set(motorSpeed);

  Wait(delayTime);

  EXPECT_TRUE(m_encoder->GetDirection() == initDirection)
      << "Inverting with Switching value does change direction";

  Reset();
}

INSTANTIATE_TEST_SUITE_P(Test, MotorInvertingTest,
                         testing::Values(TEST_VICTOR, TEST_JAGUAR, TEST_TALON));