/*
 * Copyright (C) Photon Vision.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <https://www.gnu.org/licenses/>.
 */

#include <networktables/NetworkTable.h>
#include <networktables/NetworkTableEntry.h>
#include <networktables/NetworkTableInstance.h>
#include <units/angle.h>
#include <units/length.h>

#include "gtest/gtest.h"
#include "photonlib/PhotonCamera.h"
#include "photonlib/PhotonUtils.h"
#include "photonlib/SimVisionSystem.h"

TEST(SimVisionSystemTest, testEmpty) {
  photonlib::SimVisionSystem sysUnderTest("Test", 80.0_deg, 0.0_deg,
                                          frc::Transform2d(), 1.0_m, 99999.0_m,
                                          320, 240, 0.0);

  for (int loopIdx = 0; loopIdx < 100; loopIdx++) {
    sysUnderTest.ProcessFrame(frc::Pose2d());
  }
}

class SimVisionSystemTestDistParam : public testing::TestWithParam<double> {};
INSTANTIATE_TEST_SUITE_P(SimVisionSystemTestDistParamInst,
                         SimVisionSystemTestDistParam,
                         testing::Values(5, 10, 15, 20, 25, 30));

TEST_P(SimVisionSystemTestDistParam, testDistanceAligned) {
  double dist = GetParam();

  auto targetPose =
      frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d());

  photonlib::SimVisionSystem sysUnderTest("Test", 80.0_deg, 0.0_deg,
                                          frc::Transform2d(), 1.0_m, 99999.0_m,
                                          320, 240, 0.0);

  sysUnderTest.AddSimVisionTarget(
      photonlib::SimVisionTarget(targetPose, 0.0_m, 1.0_m, 1.0_m));

  auto robotPose = frc::Pose2d(
      frc::Translation2d(units::meter_t(35.0 - dist), 0_m), frc::Rotation2d());

  sysUnderTest.ProcessFrame(robotPose);
  auto result = sysUnderTest.cam.GetLatestResult();
  ASSERT_TRUE(result.HasTargets());
  ASSERT_EQ(result.GetBestTarget()
                .GetCameraRelativePose()
                .Translation()
                .Norm()
                .to<double>(),
            dist);
}

TEST(SimVisionSystemTest, testVisibilityCupidShuffle) {
  auto targetPose =
      frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d());

  photonlib::SimVisionSystem sysUnderTest("Test", 80.0_deg, 0.0_deg,
                                          frc::Transform2d(), 1.0_m, 99999.0_m,
                                          320, 240, 0.0);

  sysUnderTest.AddSimVisionTarget(
      photonlib::SimVisionTarget(targetPose, 0.0_m, 3.0_m, 3.0_m));

  // To the right, to the right
  auto robotPose =
      frc::Pose2d(frc::Translation2d(5.0_m, 0.0_m), frc::Rotation2d(-70.0_deg));
  sysUnderTest.ProcessFrame(robotPose);
  auto result = sysUnderTest.cam.GetLatestResult();
  EXPECT_FALSE(result.HasTargets());

  // To the right, to the right
  robotPose =
      frc::Pose2d(frc::Translation2d(5.0_m, 0.0_m), frc::Rotation2d(-95.0_deg));
  sysUnderTest.ProcessFrame(robotPose);
  result = sysUnderTest.cam.GetLatestResult();
  EXPECT_FALSE(result.HasTargets());

  // To the left, to the left
  robotPose =
      frc::Pose2d(frc::Translation2d(5.0_m, 0.0_m), frc::Rotation2d(90.0_deg));
  sysUnderTest.ProcessFrame(robotPose);
  result = sysUnderTest.cam.GetLatestResult();
  EXPECT_FALSE(result.HasTargets());

  // To the left, to the left
  robotPose =
      frc::Pose2d(frc::Translation2d(5.0_m, 0.0_m), frc::Rotation2d(65.0_deg));
  sysUnderTest.ProcessFrame(robotPose);
  result = sysUnderTest.cam.GetLatestResult();
  EXPECT_FALSE(result.HasTargets());

  // now kick, now kick
  robotPose =
      frc::Pose2d(frc::Translation2d(2.0_m, 0.0_m), frc::Rotation2d(5.0_deg));
  sysUnderTest.ProcessFrame(robotPose);
  result = sysUnderTest.cam.GetLatestResult();
  EXPECT_TRUE(result.HasTargets());

  // now kick, now kick
  robotPose =
      frc::Pose2d(frc::Translation2d(2.0_m, 0.0_m), frc::Rotation2d(-5.0_deg));
  sysUnderTest.ProcessFrame(robotPose);
  result = sysUnderTest.cam.GetLatestResult();
  EXPECT_TRUE(result.HasTargets());

  // now walk it by yourself
  robotPose = frc::Pose2d(frc::Translation2d(2.0_m, 0.0_m),
                          frc::Rotation2d(-179.0_deg));
  sysUnderTest.ProcessFrame(robotPose);
  result = sysUnderTest.cam.GetLatestResult();
  EXPECT_FALSE(result.HasTargets());

  // now walk it by yourself
  sysUnderTest.MoveCamera(
      frc::Transform2d(frc::Translation2d(), frc::Rotation2d(180_deg)), 0.0_m,
      1.0_deg);
  sysUnderTest.ProcessFrame(robotPose);
  result = sysUnderTest.cam.GetLatestResult();
  EXPECT_TRUE(result.HasTargets());
}

TEST(SimVisionSystemTest, testNotVisibleVert1) {
  auto targetPose =
      frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d());

  photonlib::SimVisionSystem sysUnderTest("Test", 80.0_deg, 0.0_deg,
                                          frc::Transform2d(), 1.0_m, 99999.0_m,
                                          640, 480, 0.0);

  sysUnderTest.AddSimVisionTarget(
      photonlib::SimVisionTarget(targetPose, 1.0_m, 3.0_m, 2.0_m));

  auto robotPose =
      frc::Pose2d(frc::Translation2d(5.0_m, 0.0_m), frc::Rotation2d(5.0_deg));

  sysUnderTest.ProcessFrame(robotPose);
  auto result = sysUnderTest.cam.GetLatestResult();
  ASSERT_TRUE(result.HasTargets());

  sysUnderTest.MoveCamera(
      frc::Transform2d(frc::Translation2d(), frc::Rotation2d(0_deg)), 5000.0_m,
      1.0_deg);
  sysUnderTest.ProcessFrame(robotPose);
  result = sysUnderTest.cam.GetLatestResult();
  EXPECT_FALSE(result.HasTargets());
}

TEST(SimVisionSystemTest, testNotVisibleVert2) {
  auto targetPose =
      frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d());

  photonlib::SimVisionSystem sysUnderTest("Test", 80.0_deg, 45.0_deg,
                                          frc::Transform2d(), 1.0_m, 99999.0_m,
                                          1234, 1234, 0.5);

  sysUnderTest.AddSimVisionTarget(
      photonlib::SimVisionTarget(targetPose, 3.0_m, 0.5_m, 0.5_m));

  auto robotPose =
      frc::Pose2d(frc::Translation2d(32.0_m, 0.0_m), frc::Rotation2d(5.0_deg));

  sysUnderTest.ProcessFrame(robotPose);
  auto result = sysUnderTest.cam.GetLatestResult();
  EXPECT_TRUE(result.HasTargets());

  robotPose =
      frc::Pose2d(frc::Translation2d(0.0_m, 0.0_m), frc::Rotation2d(5.0_deg));
  sysUnderTest.ProcessFrame(robotPose);
  result = sysUnderTest.cam.GetLatestResult();
  EXPECT_FALSE(result.HasTargets());
}

TEST(SimVisionSystemTest, testNotVisibleTgtSize) {
  auto targetPose =
      frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d());

  photonlib::SimVisionSystem sysUnderTest("Test", 80.0_deg, 0.0_deg,
                                          frc::Transform2d(), 1.0_m, 99999.0_m,
                                          640, 480, 20.0);

  sysUnderTest.AddSimVisionTarget(
      photonlib::SimVisionTarget(targetPose, 1.10_m, 0.25_m, 0.1_m));

  auto robotPose =
      frc::Pose2d(frc::Translation2d(32.0_m, 0.0_m), frc::Rotation2d(5.0_deg));
  sysUnderTest.ProcessFrame(robotPose);
  auto result = sysUnderTest.cam.GetLatestResult();
  EXPECT_TRUE(result.HasTargets());

  robotPose =
      frc::Pose2d(frc::Translation2d(0.0_m, 0.0_m), frc::Rotation2d(5.0_deg));
  sysUnderTest.ProcessFrame(robotPose);
  result = sysUnderTest.cam.GetLatestResult();
  EXPECT_FALSE(result.HasTargets());
}

TEST(SimVisionSystemTest, testNotVisibleTooFarForLEDs) {
  auto targetPose =
      frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d());

  photonlib::SimVisionSystem sysUnderTest("Test", 80.0_deg, 0.0_deg,
                                          frc::Transform2d(), 1.0_m, 10.0_m,
                                          640, 480, 1.0);

  sysUnderTest.AddSimVisionTarget(
      photonlib::SimVisionTarget(targetPose, 1.10_m, 0.25_m, 0.1_m));

  auto robotPose =
      frc::Pose2d(frc::Translation2d(28.0_m, 0.0_m), frc::Rotation2d(5.0_deg));
  sysUnderTest.ProcessFrame(robotPose);
  auto result = sysUnderTest.cam.GetLatestResult();
  EXPECT_TRUE(result.HasTargets());

  robotPose =
      frc::Pose2d(frc::Translation2d(0.0_m, 0.0_m), frc::Rotation2d(5.0_deg));
  sysUnderTest.ProcessFrame(robotPose);
  result = sysUnderTest.cam.GetLatestResult();
  EXPECT_FALSE(result.HasTargets());
}

class SimVisionSystemTestYawParam : public testing::TestWithParam<double> {};
INSTANTIATE_TEST_SUITE_P(SimVisionSystemTestYawParamInst,
                         SimVisionSystemTestYawParam,
                         testing::Values(-10, -5, -0, -1, -2, 5, 7, 10.23));
TEST_P(SimVisionSystemTestYawParam, testYawAngles) {
  double testYaw = GetParam();  // Nope, Chuck testYaw
  auto targetPose =
      frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d(45_deg));

  photonlib::SimVisionSystem sysUnderTest("Test", 80.0_deg, 0.0_deg,
                                          frc::Transform2d(), 1.0_m, 99999.0_m,
                                          640, 480, 0.0);

  sysUnderTest.AddSimVisionTarget(
      photonlib::SimVisionTarget(targetPose, 0.0_m, 0.5_m, 0.5_m));

  auto robotPose = frc::Pose2d(frc::Translation2d(32_m, 0.0_m),
                               frc::Rotation2d(units::degree_t(testYaw)));

  sysUnderTest.ProcessFrame(robotPose);
  auto result = sysUnderTest.cam.GetLatestResult();
  ASSERT_TRUE(result.HasTargets());
  auto tgt = result.GetBestTarget();
  EXPECT_DOUBLE_EQ(tgt.GetYaw(), testYaw);
}

class SimVisionSystemTestCameraPitchParam
    : public testing::TestWithParam<double> {};
INSTANTIATE_TEST_SUITE_P(SimVisionSystemTestCameraPitchParamInst,
                         SimVisionSystemTestCameraPitchParam,
                         testing::Values(-10, -5, -0, -1, -2, 5, 7, 10.23,
                                         20.21, -19.999));
TEST_P(SimVisionSystemTestCameraPitchParam, testCameraPitch) {
  double testPitch = GetParam();
  auto targetPose =
      frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d(45_deg));

  auto robotPose =
      frc::Pose2d(frc::Translation2d(30_m, 0.0_m), frc::Rotation2d(0.0_deg));

  photonlib::SimVisionSystem sysUnderTest("Test", 80.0_deg, 0.0_deg,
                                          frc::Transform2d(), 1.0_m, 99999.0_m,
                                          640, 480, 0.0);

  sysUnderTest.AddSimVisionTarget(
      photonlib::SimVisionTarget(targetPose, 0.0_m, 0.5_m, 0.5_m));

  sysUnderTest.MoveCamera(
      frc::Transform2d(frc::Translation2d(), frc::Rotation2d()), 0.0_m,
      units::degree_t(testPitch));

  sysUnderTest.ProcessFrame(robotPose);
  auto result = sysUnderTest.cam.GetLatestResult();
  ASSERT_TRUE(result.HasTargets());
  auto tgt = result.GetBestTarget();
  // If the camera is pitched down by 10 degrees, the target should appear
  // in the upper part of the image (ie, pitch positive). Therefor,
  // pass/fail involves -1.0.
  EXPECT_DOUBLE_EQ(tgt.GetPitch(), -1.0 * testPitch);
}

class SimVisionSystemTestDistCalcParam
    : public testing::TestWithParam<std::tuple<double, double, double>> {};
INSTANTIATE_TEST_SUITE_P(
    SimVisionSystemTestDistCalcParamInst, SimVisionSystemTestDistCalcParam,
    testing::Values(std::tuple<double, double, double>(5, 35, 0),
                    std::tuple<double, double, double>(6, 35, 1),
                    std::tuple<double, double, double>(10, 35, 0),
                    std::tuple<double, double, double>(15, 35, 2),
                    std::tuple<double, double, double>(19.95, 35, 0),
                    std::tuple<double, double, double>(20, 35, 0),
                    std::tuple<double, double, double>(5, 42, 1),
                    std::tuple<double, double, double>(6, 42, 0),
                    std::tuple<double, double, double>(10, 42, 2),
                    std::tuple<double, double, double>(15, 42, 0.5),
                    std::tuple<double, double, double>(19.42, 35, 0),
                    std::tuple<double, double, double>(20, 42, 0),
                    std::tuple<double, double, double>(5, 55, 2),
                    std::tuple<double, double, double>(6, 55, 0),
                    std::tuple<double, double, double>(10, 54, 2.2),
                    std::tuple<double, double, double>(15, 53, 0),
                    std::tuple<double, double, double>(19.52, 35, 1.1),
                    std::tuple<double, double, double>(20, 51, 2.87),
                    std::tuple<double, double, double>(20, 55, 3)));
TEST_P(SimVisionSystemTestDistCalcParam, testDistanceCalc) {
  std::tuple<double, double, double> testArgs = GetParam();
  double testDist = std::get<0>(testArgs);
  double testPitch = std::get<1>(testArgs);
  double testHeight = std::get<2>(testArgs);

  auto targetPose = frc::Pose2d(frc::Translation2d(35_m, 0_m),
                                frc::Rotation2d(units::radian_t(3.14159 / 42)));

  auto robotPose =
      frc::Pose2d(frc::Translation2d(units::meter_t(35 - testDist), 0.0_m),
                  frc::Rotation2d(0.0_deg));

  photonlib::SimVisionSystem sysUnderTest(
      "absurdlylongnamewhichshouldneveractuallyhappenbuteehwelltestitanywaysoho"
      "wsyourdaygoingihopegoodhaveagreatrestofyourlife",
      160.0_deg, units::degree_t(testPitch), frc::Transform2d(),
      units::meter_t(testHeight), 99999.0_m, 640, 480, 0.0);

  sysUnderTest.AddSimVisionTarget(photonlib::SimVisionTarget(
      targetPose, units::meter_t(testDist), 0.5_m, 0.5_m));

  sysUnderTest.ProcessFrame(robotPose);
  auto result = sysUnderTest.cam.GetLatestResult();
  ASSERT_TRUE(result.HasTargets());
  auto tgt = result.GetBestTarget();
  EXPECT_DOUBLE_EQ(tgt.GetYaw(), 0.0);
  units::meter_t distMeas = photonlib::PhotonUtils::CalculateDistanceToTarget(
      units::meter_t(testHeight), units::meter_t(testDist),
      units::degree_t(testPitch), units::degree_t(tgt.GetPitch()));
  EXPECT_DOUBLE_EQ(distMeas.to<double>(), testDist);
}

TEST(SimVisionSystemTest, testMultipleTargets) {
  auto targetPoseL =
      frc::Pose2d(frc::Translation2d(35_m, 2_m), frc::Rotation2d());
  auto targetPoseC =
      frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d());
  auto targetPoseR =
      frc::Pose2d(frc::Translation2d(35_m, -2_m), frc::Rotation2d());

  photonlib::SimVisionSystem sysUnderTest("test", 160.0_deg, 0.0_deg,
                                          frc::Transform2d(), 5.0_m, 99999.0_m,
                                          640, 480, 20.0);

  sysUnderTest.AddSimVisionTarget(
      photonlib::SimVisionTarget(targetPoseL, 0.0_m, 0.25_m, 0.1_m));
  sysUnderTest.AddSimVisionTarget(
      photonlib::SimVisionTarget(targetPoseC, 1.0_m, 0.25_m, 0.1_m));
  sysUnderTest.AddSimVisionTarget(
      photonlib::SimVisionTarget(targetPoseR, 2.0_m, 0.25_m, 0.1_m));
  sysUnderTest.AddSimVisionTarget(
      photonlib::SimVisionTarget(targetPoseL, 3.0_m, 0.25_m, 0.1_m));
  sysUnderTest.AddSimVisionTarget(
      photonlib::SimVisionTarget(targetPoseC, 4.0_m, 0.25_m, 0.1_m));
  sysUnderTest.AddSimVisionTarget(
      photonlib::SimVisionTarget(targetPoseR, 5.0_m, 0.25_m, 0.1_m));
  sysUnderTest.AddSimVisionTarget(
      photonlib::SimVisionTarget(targetPoseL, 6.0_m, 0.25_m, 0.1_m));
  sysUnderTest.AddSimVisionTarget(
      photonlib::SimVisionTarget(targetPoseC, 7.0_m, 0.25_m, 0.1_m));
  sysUnderTest.AddSimVisionTarget(
      photonlib::SimVisionTarget(targetPoseL, 8.0_m, 0.25_m, 0.1_m));
  sysUnderTest.AddSimVisionTarget(
      photonlib::SimVisionTarget(targetPoseR, 9.0_m, 0.25_m, 0.1_m));
  sysUnderTest.AddSimVisionTarget(
      photonlib::SimVisionTarget(targetPoseL, 10.0_m, 0.25_m, 0.1_m));

  auto robotPose =
      frc::Pose2d(frc::Translation2d(30_m, 0.0_m), frc::Rotation2d(5.0_deg));

  sysUnderTest.ProcessFrame(robotPose);
  auto result = sysUnderTest.cam.GetLatestResult();
  ASSERT_TRUE(result.HasTargets());

  auto tgtList = result.GetTargets();
  EXPECT_EQ(11ul, tgtList.size());
}