CollisionTest.cpp

#include "Simulation.h"
#include <iomanip>
#include <iostream>

void Simulation::initializeCollisionTest() {
  std::cout << "\n===== INITIALIZING COLLISION TEST =====" << std::endl;
  collision_test_mode = true;
  detected_collisions = 0;
  reported_collisions.clear();

  int houston = 1;
  int dallas = 2;

  for (int i = 0; i < 3; ++i) {
    auto train = std::make_shared<Train>(i + 1, houston, 1000.0);
    auto station = network->getStation(houston);
    if (station) {
      train->updateLocation(station->location);
    }
    trains.push_back(train);
  }

  for (auto &train : trains) {
    std::vector<int> route = {houston, dallas};
    train->setRoute(route);
    train->setStatus(TrainStatus::MOVING);
  }

  std::cout << "Created " << trains.size() << " trains on the same route"
            << std::endl;
  std::cout << "Route: Houston Central -> Dallas Fort Worth" << std::endl;
  std::cout << "Collision avoidance: DISABLED" << std::endl;
  std::cout << "========================================\n" << std::endl;
}

bool Simulation::detectCollisions() {
  bool collision_found = false;

  for (size_t i = 0; i < trains.size(); ++i) {
    for (size_t j = i + 1; j < trains.size(); ++j) {
      auto train1 = trains[i];
      auto train2 = trains[j];

      if (train1->getStatus() == TrainStatus::MOVING &&
          train2->getStatus() == TrainStatus::MOVING) {

        const auto &route1 = train1->getPlannedRoute();
        const auto &route2 = train2->getPlannedRoute();
        int idx1 = train1->getCurrentRouteIndex();
        int idx2 = train2->getCurrentRouteIndex();

        if (!route1.empty() && !route2.empty() &&
            idx1 < static_cast<int>(route1.size()) - 1 &&
            idx2 < static_cast<int>(route2.size()) - 1) {

          int curr1 = route1[idx1];
          int next1 = route1[idx1 + 1];
          int curr2 = route2[idx2];
          int next2 = route2[idx2 + 1];

          if ((curr1 == curr2 && next1 == next2) ||
              (curr1 == next2 && next1 == curr2)) {

            double distance =
                train1->getLocation().distanceTo(train2->getLocation());
            if (distance < 5.0) {
              int id1 = train1->getId();
              int id2 = train2->getId();
              auto collision_pair =
                  std::make_pair(std::min(id1, id2), std::max(id1, id2));

              if (reported_collisions.find(collision_pair) ==
                  reported_collisions.end()) {
                detected_collisions++;
                reported_collisions.insert(collision_pair);
                collision_found = true;
                std::cout << "[COLLISION DETECTED] Train #" << id1
                          << " and Train #" << id2 << " at time " << std::fixed
                          << std::setprecision(2) << current_time << " min"
                          << std::endl;
              }
            }
          }
        }
      }
    }
  }
  return collision_found;
}

void Simulation::runCollisionTest(double duration_minutes) {
  std::cout << "\n===== STARTING COLLISION TEST =====" << std::endl;

  // Print initial train positions
  std::cout << "Initial Train Positions:" << std::endl;
  for (const auto &train : trains) {
    auto loc = train->getLocation();
    std::cout << "Train #" << train->getId() << " at (" << std::fixed
              << std::setprecision(2) << loc.latitude << ", " << loc.longitude
              << ")" << std::endl;
  }
  std::cout << "===================================\n" << std::endl;

  double end_time = current_time + duration_minutes;
  bool collision_detected = false;

  while (current_time < end_time) {
    updateTrainMovements();

    if (detectCollisions()) {
      collision_detected = true;
      break;
    }

    current_time += time_step;
  }

  std::cout << "\n===== COLLISION TEST COMPLETE =====" << std::endl;
  std::cout << "Total Collisions Detected: " << detected_collisions
            << std::endl;
  if (collision_detected) {
    std::cout << "Test stopped: Collision(s) detected" << std::endl;
  } else {
    std::cout << "Test ended: No collisions detected" << std::endl;
  }
  std::cout << "====================================\n" << std::endl;
}

void Simulation::initializeHeadOnCollisionTest() {
  std::cout << "\n===== INITIALIZING HEAD-ON COLLISION TEST =====" << std::endl;
  collision_test_mode = true;
  detected_collisions = 0;
  reported_collisions.clear();

  int houston = 1;
  int dallas = 2;

  // Train 1 starts in Houston, moving to Dallas
  auto train1 = std::make_shared<Train>(1, houston, 1000.0);
  auto station1 = network->getStation(houston);
  if (station1) {
    train1->updateLocation(station1->location);
  }
  trains.push_back(train1);

  // Train 2 starts in Dallas, moving to Houston
  auto train2 = std::make_shared<Train>(2, dallas, 1000.0);
  auto station2 = network->getStation(dallas);
  if (station2) {
    train2->updateLocation(station2->location);
  }
  trains.push_back(train2);

  // Set routes for head-on collision
  train1->setRoute({houston, dallas});
  train1->setStatus(TrainStatus::MOVING);

  train2->setRoute({dallas, houston});
  train2->setStatus(TrainStatus::MOVING);

  std::cout << "Created 2 trains on opposite routes" << std::endl;
  std::cout << "Train #1: Houston → Dallas" << std::endl;
  std::cout << "Train #2: Dallas → Houston (opposite direction)" << std::endl;
  std::cout << "Expected: Head-on collision mid-route" << std::endl;
  std::cout << "Collision avoidance: DISABLED" << std::endl;
  std::cout << "===============================================\n" << std::endl;
}

void Simulation::initializeCrossingRoutesTest() {
  std::cout << "\n===== INITIALIZING CROSSING ROUTES TEST =====" << std::endl;
  collision_test_mode = true;
  detected_collisions = 0;
  reported_collisions.clear();

  int san_antonio = 3;
  int corpus_christi = 6;

  // Train 1: San Antonio → Corpus Christi
  auto train1 = std::make_shared<Train>(1, san_antonio, 1000.0);
  auto station1 = network->getStation(san_antonio);
  if (station1) {
    train1->updateLocation(station1->location);
  }
  trains.push_back(train1);

  // Train 2: Corpus Christi → San Antonio (opposite direction on same track)
  auto train2 = std::make_shared<Train>(2, corpus_christi, 1000.0);
  auto station2 = network->getStation(corpus_christi);
  if (station2) {
    train2->updateLocation(station2->location);
  }
  trains.push_back(train2);

  // Both routes use Track 8 (San Antonio ↔ Corpus Christi), crossing paths
  train1->setRoute({san_antonio, corpus_christi});
  train1->setStatus(TrainStatus::MOVING);

  train2->setRoute({corpus_christi, san_antonio});
  train2->setStatus(TrainStatus::MOVING);

  std::cout << "Created 2 trains on crossing routes" << std::endl;
  std::cout << "Train #1: San Antonio → Corpus Christi" << std::endl;
  std::cout << "Train #2: Corpus Christi → San Antonio (opposite direction)" << std::endl;
  std::cout << "Expected: Trains collide on shared track segment" << std::endl;
  std::cout << "Collision avoidance: DISABLED" << std::endl;
  std::cout << "=============================================\n" << std::endl;
}