//
// Created by dominik on 8/22/24.
//

#include "coordinates.h"
#include <cmath>
#include <iostream>
#include <ostream>

SphericalPos rotate(const SphericalPos& old_pos, const double alpha, const double beta) {
    const double sin_alpha{std::sin(alpha)};
    const double cos_alpha{std::cos(alpha)};

    const double sin_beta{std::sin(beta)};
    const double cos_beta{std::cos(beta)};

    const double cos_theta{old_pos.cos_theta};

    if (std::abs(cos_theta) == 1) {
        return xyz_to_spherical(CartesianPos{cos_alpha * cos_beta, cos_alpha * sin_beta, cos_alpha * cos_theta});
    }

    const double norm{std::sqrt(1 - cos_theta * cos_theta) + 1e-15};

    auto [x, y , z] = spherical_to_xyz(old_pos);

    const auto new_pos = CartesianPos{
        cos_alpha * x + sin_alpha * (-x * z * sin_beta - y * cos_beta) / norm,
        cos_alpha * y + sin_alpha * (-y * z * sin_beta + x * cos_beta) / norm,
        cos_alpha * z + sin_alpha * norm * sin_beta
    };

    return xyz_to_spherical(new_pos);
}

CartesianPos spherical_to_xyz(const SphericalPos& pos) {
    const double sin_theta = std::sin(std::acos(pos.cos_theta));
    return {sin_theta * std::cos(pos.phi), sin_theta * std::sin(pos.phi), pos.cos_theta};
}

SphericalPos xyz_to_spherical(const CartesianPos& pos) {
    return {pos.z, std::atan2(pos.y, pos.x)};
}