Vehicle.cpp

#include "Vehicle.h"

#include <iostream>

void Vehicle::set_location(const shared_ptr<Point> &location) {
    _location = std::make_shared<Point>(*location);
}
const shared_ptr<Point> &Vehicle::get_location() const {
    return _location;
}

void Vehicle::stop() {
    this->angle = 0.0;
    this->speed = 0.0;
    this->destination = nullptr;
    this->en_route = false;
    setState("Stopped");
}

double Vehicle::computeDistance(double x1, double y1, double x2, double y2) {
    double dx = x2 - x1;
    double dy = y2 - y1;
    return std::sqrt(dx * dx + dy * dy);
}

void Vehicle::setSpeed(double x) {
    if (x< 0) {
        std::cerr << "Speed cannot be negative. Setting to 0." << std::endl;
        x = 0;
    }
    this->speed = x;
}
void Vehicle::position(double x, double y, double speed) {
    pending_position = true;
    pending_pos_x = x;
    pending_pos_y = y;
    pending_pos_speed = speed;
}

void Vehicle::course(double angle, double speed) {
    pending_course = true;
    pending_course_angle = angle;
    pending_course_speed = speed;
}

void Vehicle::beginPositionMode(double worldSpeed) {
    destination = std::make_shared<Point>(pending_pos_x, pending_pos_y);
    angle = to_degrees(std::atan2(pending_pos_x - _location->x,
                                  pending_pos_y - _location->y));
    speed = worldSpeed;
    mode = 0;
    en_route = true;
}

void Vehicle::beginCourseMode(double worldSpeed) {
    angle = pending_course_angle;
    speed = worldSpeed;
    destination = nullptr;
    mode = 1;
    en_route = true;
}

void Vehicle::stepAlongHeading(double stepDistance) {
    double radians = to_radians(angle);
    _location->x += stepDistance * std::sin(radians);
    _location->y += stepDistance * std::cos(radians);
}