#if GTEST

// #include <gmock/gmock.h>
// not supported using Visual Studio 2022 compiler...
#include <gtest/gtest.h>

#include "../DifferentialDrive.h"
#include "../DistanceSensor.h"
#include "../Roboid.h"
#include "../VectorAlgebra/Angle.h"

/// @brief A Distance sensor with testing support
/// With this sensor it is possible to simulate a measurement
class MockDistanceSensor : public DistanceSensor {
public:
  MockDistanceSensor() : DistanceSensor(){};
  MockDistanceSensor(float triggerDistance) : DistanceSensor(triggerDistance){};

  void SimulateMeasurement(float distance) { this->distance = distance; }
};

TEST(BB2B, NoObstacle) {
#pragma region Setup

  Motor *motorLeft = new Motor();
  Motor *motorRight = new Motor();

  MockDistanceSensor *sensorLeft = new MockDistanceSensor(10.0F);
  sensorLeft->position.angle = -30;
  MockDistanceSensor *sensorRight = new MockDistanceSensor(10.0F);
  sensorRight->position.angle = 30;

  Sensor *sensors[] = {sensorLeft, sensorRight};

  Perception *perception = new Perception(sensors, 2);

  DifferentialDrive *propulsion = new DifferentialDrive(motorLeft, motorRight);
  propulsion->SetDimensions(
      1 / Angle::pi, // we use this, such that motor speed 1 -> velocity 1 m/s
      1);

  Roboid *roboid = new Roboid(perception, propulsion);

#pragma endregion

#pragma region Test initial state

  // Sensors should return no valid value
  float distanceLeft = sensorLeft->GetDistance();
  float distanceRight = sensorRight->GetDistance();

  EXPECT_LT(distanceLeft, 0.0F);  // negative values are invalid
  EXPECT_LT(distanceRight, 0.0F); // negative values are invalid

#pragma endregion

#pragma region Measurement 1

  /// closest objects are too far away
  sensorLeft->SimulateMeasurement(100.0F);
  sensorRight->SimulateMeasurement(100.0F);

  roboid->Update(0.0F);

#pragma endregion

#pragma region Test state 1
  distanceLeft = sensorLeft->GetDistance();
  distanceRight = sensorRight->GetDistance();

  EXPECT_LT(distanceLeft, 0.0F);  // negative values are invalid
  EXPECT_LT(distanceRight, 0.0F); // negative values are invalid

  int trackedObjectCount = roboid->perception->TrackedObjectCount();
  EXPECT_EQ(trackedObjectCount, 0);

  bool obstacleLeft = roboid->perception->ObjectNearby(-30);
  bool obstacleRight = roboid->perception->ObjectNearby(30);

  EXPECT_FALSE(obstacleLeft);
  EXPECT_FALSE(obstacleRight);

  float leftMotorSpeed = obstacleRight ? -1.0F : 1.0F;
  float rightMotorSpeed = obstacleLeft ? -1.0F : 1.0F;

  EXPECT_FLOAT_EQ(leftMotorSpeed, 1.0F);
  EXPECT_FLOAT_EQ(rightMotorSpeed, 1.0F);

  DifferentialDrive *diffDrive = (DifferentialDrive *)roboid->propulsion;
  diffDrive->SetMotorTargetSpeeds(leftMotorSpeed, rightMotorSpeed);

  float leftActualSpeed = motorLeft->GetActualSpeed();
  float rightActualSpeed = motorRight->GetActualSpeed();

  EXPECT_FLOAT_EQ(leftActualSpeed, 1.0F);
  EXPECT_FLOAT_EQ(rightActualSpeed, 1.0F);

  Polar velocity =
      diffDrive->GetVelocity(); // this depends on the wheel diameter.
  EXPECT_FLOAT_EQ(velocity.distance, 1.0F);
  EXPECT_FLOAT_EQ(velocity.angle, 0.0F);

  trackedObjectCount = roboid->perception->TrackedObjectCount();
  EXPECT_EQ(trackedObjectCount, 0);

  TrackedObject **trackedObjects = roboid->perception->GetTrackedObjects();
  TrackedObject *trackedObject = nullptr;
  for (int i = 0; i < roboid->perception->maxObjectCount; i++) {
    if (trackedObjects[i] != nullptr)
      trackedObject = trackedObjects[0];
  }

  EXPECT_TRUE(trackedObject == NULL);
#pragma endregion
}

TEST(BB2B, ObstacleLeft) {
#pragma region Setup

  Motor *motorLeft = new Motor();
  Motor *motorRight = new Motor();

  MockDistanceSensor *sensorLeft = new MockDistanceSensor();
  sensorLeft->position.angle = -30;
  MockDistanceSensor *sensorRight = new MockDistanceSensor();
  sensorRight->position.angle = 30;

  Sensor *sensors[] = {sensorLeft, sensorRight};

  Perception *perception = new Perception(sensors, 2);

  DifferentialDrive *propulsion = new DifferentialDrive(motorLeft, motorRight);
  propulsion->SetDimensions(
      1 / Angle::pi,  // we use this, such that motor speed 1 -> velocity 1 m/s
      8 / Angle::pi); // we use this, such that angular velocity will be 90
                      // degrees per second

  Roboid *roboid = new Roboid(perception, propulsion);

#pragma endregion

#pragma region Sensor Measurement - only left

  // place obstacle on the left
  sensorLeft->SimulateMeasurement(0.1F);

  roboid->Update(0.1F);

#pragma endregion

#pragma region Test Sensor output
  // Distance
  float distanceLeft = sensorLeft->GetDistance();
  float distanceRight = sensorRight->GetDistance();

  EXPECT_FLOAT_EQ(distanceLeft, 0.1F);
  EXPECT_LT(distanceRight, 0.0F); // invalid

  // Obstacle
  bool obstacleLeft = roboid->perception->ObjectNearby(-30);
  bool obstacleRight = roboid->perception->ObjectNearby(30);

  EXPECT_TRUE(obstacleLeft);
  EXPECT_FALSE(obstacleRight);

  // Tracked objects
  int trackedObjectCount = roboid->perception->TrackedObjectCount();
  EXPECT_EQ(trackedObjectCount, 1);

  // Find the single tracked object
  TrackedObject **trackedObjects = roboid->perception->GetTrackedObjects();
  TrackedObject *trackedObject = nullptr;
  for (int i = 0; i < roboid->perception->maxObjectCount; i++) {
    if (trackedObjects[i] != nullptr)
      trackedObject = trackedObjects[i];
  }

  ASSERT_FALSE(trackedObject == nullptr);
  EXPECT_FLOAT_EQ(trackedObject->position.distance, 0.1F);
  EXPECT_FLOAT_EQ(trackedObject->position.angle, -30);

#pragma endregion

#pragma region Motor Control

  // Motor speeds
  float leftMotorSpeed = obstacleRight ? -1.0F : 1.0F;
  float rightMotorSpeed = obstacleLeft ? -1.0F : 1.0F;

  EXPECT_FLOAT_EQ(leftMotorSpeed, 1.0F);
  EXPECT_FLOAT_EQ(rightMotorSpeed, -1.0F);

  DifferentialDrive *diffDrive = (DifferentialDrive *)roboid->propulsion;
  diffDrive->SetMotorTargetSpeeds(leftMotorSpeed, rightMotorSpeed);

#pragma endregion

#pragma region Test motor output results

  float leftActualSpeed = motorLeft->GetActualSpeed();
  float rightActualSpeed = motorRight->GetActualSpeed();

  EXPECT_FLOAT_EQ(leftActualSpeed, 1.0F);
  EXPECT_FLOAT_EQ(rightActualSpeed, -1.0F);

  // Roboid velocity
  Polar velocity =
      diffDrive->GetVelocity(); // this depends on the wheel diameter.
  EXPECT_FLOAT_EQ(velocity.distance, 0.0F);

  float angularVelocity =
      diffDrive->GetAngularVelocity(); // this also depends on wheel separation
  EXPECT_FLOAT_EQ(angularVelocity, 90.0F);

#pragma endregion
}

TEST(BB2B, ObstacleRight) {
#pragma region Setup

  Motor *motorLeft = new Motor();
  Motor *motorRight = new Motor();

  MockDistanceSensor *sensorLeft = new MockDistanceSensor();
  sensorLeft->position.angle = -30;
  MockDistanceSensor *sensorRight = new MockDistanceSensor();
  sensorRight->position.angle = 30;

  Sensor *sensors[] = {sensorLeft, sensorRight};

  Perception *perception = new Perception(sensors, 2);

  DifferentialDrive *propulsion = new DifferentialDrive(motorLeft, motorRight);
  propulsion->SetDimensions(
      1 / Angle::pi,  // we use this, such that motor speed 1 -> velocity 1 m/s
      8 / Angle::pi); // we use this, such that angular velocity will be 90
                      // degrees per second

  Roboid *roboid = new Roboid(perception, propulsion);

#pragma endregion

#pragma region Sensor Measurement - only right

  // place obstacle on the left
  sensorRight->SimulateMeasurement(0.1F);

  roboid->Update(0.1F);

#pragma endregion

#pragma region Test Sensor output
  // Distance
  float distanceLeft = sensorLeft->GetDistance();
  float distanceRight = sensorRight->GetDistance();

  EXPECT_LT(distanceLeft, 0.0F); // invalid
  EXPECT_FLOAT_EQ(distanceRight, 0.1F);

  // Obstacle
  bool obstacleLeft = roboid->perception->ObjectNearby(-30);
  bool obstacleRight = roboid->perception->ObjectNearby(30);

  EXPECT_FALSE(obstacleLeft);
  EXPECT_TRUE(obstacleRight);

  // Tracked objects
  int trackedObjectCount = roboid->perception->TrackedObjectCount();
  EXPECT_EQ(trackedObjectCount, 1);

  // Find the single tracked object
  TrackedObject **trackedObjects = roboid->perception->GetTrackedObjects();
  TrackedObject *trackedObject = nullptr;
  for (int i = 0; i < roboid->perception->maxObjectCount; i++) {
    if (trackedObjects[i] != nullptr)
      trackedObject = trackedObjects[i];
  }

  ASSERT_FALSE(trackedObject == nullptr);
  EXPECT_FLOAT_EQ(trackedObject->position.distance, 0.1F);
  EXPECT_FLOAT_EQ(trackedObject->position.angle, 30);

#pragma endregion

#pragma region Motor Control

  // Motor speeds
  float leftMotorSpeed = obstacleRight ? -1.0F : 1.0F;
  float rightMotorSpeed = obstacleLeft ? -1.0F : 1.0F;

  EXPECT_FLOAT_EQ(leftMotorSpeed, -1.0F);
  EXPECT_FLOAT_EQ(rightMotorSpeed, 1.0F);

  DifferentialDrive *diffDrive = (DifferentialDrive *)roboid->propulsion;
  diffDrive->SetMotorTargetSpeeds(leftMotorSpeed, rightMotorSpeed);

#pragma endregion

#pragma region Test motor output results

  float leftActualSpeed = motorLeft->GetActualSpeed();
  float rightActualSpeed = motorRight->GetActualSpeed();

  EXPECT_FLOAT_EQ(leftActualSpeed, -1.0F);
  EXPECT_FLOAT_EQ(rightActualSpeed, 1.0F);

  // Roboid velocity
  Polar velocity = diffDrive->GetVelocity();
  EXPECT_FLOAT_EQ(velocity.distance, 0.0F);

  float angularVelocity = diffDrive->GetAngularVelocity();
  EXPECT_FLOAT_EQ(angularVelocity, -90.0F);

#pragma endregion
}

TEST(BB2B, ObstacleBoth) {
#pragma region Setup

  Motor *motorLeft = new Motor();
  Motor *motorRight = new Motor();

  MockDistanceSensor *sensorLeft = new MockDistanceSensor();
  sensorLeft->position.angle = -30;
  MockDistanceSensor *sensorRight = new MockDistanceSensor();
  sensorRight->position.angle = 30;

  Sensor *sensors[] = {sensorLeft, sensorRight};

  Perception *perception = new Perception(sensors, 2);

  DifferentialDrive *propulsion = new DifferentialDrive(motorLeft, motorRight);
  propulsion->SetDimensions(
      1 / Angle::pi,  // we use this, such that motor speed 1 -> velocity 1 m/s
      8 / Angle::pi); // we use this, such that angular velocity will be 90
                      // degrees per second

  Roboid *roboid = new Roboid(perception, propulsion);

#pragma endregion

#pragma region Sensor Measurement - left and right

  sensorLeft->SimulateMeasurement(0.1F);
  sensorRight->SimulateMeasurement(0.1F);

  roboid->Update(0.1F);

#pragma endregion

#pragma region Test Sensor output
  // Distance
  float distanceLeft = sensorLeft->GetDistance();
  float distanceRight = sensorRight->GetDistance();

  EXPECT_FLOAT_EQ(distanceLeft, 0.1F);
  EXPECT_FLOAT_EQ(distanceRight, 0.1F);

  // Obstacle
  bool obstacleLeft = roboid->perception->ObjectNearby(-30);
  bool obstacleRight = roboid->perception->ObjectNearby(30);

  EXPECT_TRUE(obstacleLeft);
  EXPECT_TRUE(obstacleRight);

  // Tracked objects
  int trackedObjectCount = roboid->perception->TrackedObjectCount();
  EXPECT_EQ(trackedObjectCount, 2);

  // Find the single tracked object
  TrackedObject **trackedObjects = roboid->perception->GetTrackedObjects();
  for (int i = 0; i < roboid->perception->maxObjectCount; i++) {
    if (trackedObjects[i] != nullptr) {
      EXPECT_FLOAT_EQ(trackedObjects[i]->position.distance, 0.1F);
      // EXPECT_THAT(trackedObjects[i] ->position.angle, AnyOf(FloatEq(-30),
      // FloatEq(30));
    }
  }

#pragma endregion

#pragma region Motor Control

  // Motor speeds
  float leftMotorSpeed = obstacleRight ? -1.0F : 1.0F;
  float rightMotorSpeed = obstacleLeft ? -1.0F : 1.0F;

  EXPECT_FLOAT_EQ(leftMotorSpeed, -1.0F);
  EXPECT_FLOAT_EQ(rightMotorSpeed, -1.0F);

  DifferentialDrive *diffDrive = (DifferentialDrive *)roboid->propulsion;
  diffDrive->SetMotorTargetSpeeds(leftMotorSpeed, rightMotorSpeed);

#pragma endregion

#pragma region Test motor output results

  float leftActualSpeed = motorLeft->GetActualSpeed();
  float rightActualSpeed = motorRight->GetActualSpeed();

  EXPECT_FLOAT_EQ(leftActualSpeed, -1.0F);
  EXPECT_FLOAT_EQ(rightActualSpeed, -1.0F);

  // Roboid velocity
  Polar velocity = diffDrive->GetVelocity();
  EXPECT_FLOAT_EQ(velocity.distance, 1.0F);
  EXPECT_FLOAT_EQ(velocity.angle, 180.0F);

  float angularVelocity = diffDrive->GetAngularVelocity();
  EXPECT_FLOAT_EQ(angularVelocity, 0.0F);

#pragma endregion
}

#endif