//
// Created by dominik on 8/16/24.
//
#include <random>
#include "tetrahedral.h"

#include <iostream>
#include <ostream>

#include "../functions.h"


TetrahedralJump::TetrahedralJump(const double delta, const double eta, std::mt19937_64& rng) : Motion(delta, eta, rng) {}

TetrahedralJump::TetrahedralJump(std::mt19937_64& rng) : Motion(rng) {}

void TetrahedralJump::initialize() {
    const auto [cos_theta, phi] = draw_position();
    m_corners[0] = omega_q(cos_theta, phi);
    const double alpha = 2. * M_PI * m_uni_dist(m_rng);

    const auto vec = spherical_to_xyz(cos_theta, phi);
    const double norm = std::sqrt(1 - cos_theta * cos_theta) + 1e-15;

    for (int i = 1; i<4; i++) {
        const double cos_alpha = std::cos(alpha + (i-1) * 2*M_PI / 3.);
        const double sin_alpha = std::sin(alpha + (i-1) * 2*M_PI / 3.);
        std::array<double, 3> rotated_position{};
        if (cos_theta != 1 && cos_theta != -1) {
            // std::cout << cos_theta << std::endl;
            rotated_position = {
                m_cos_beta * vec[0] + m_sin_beta * (-vec[0] * vec[2] * sin_alpha - vec[1] * cos_alpha) / norm,
                m_cos_beta * vec[1] + m_sin_beta * (-vec[1] * vec[2] * sin_alpha + vec[0] * cos_alpha) / norm,
                m_cos_beta * vec[2] + m_sin_beta * norm * sin_alpha
            };
        } else {
            rotated_position = {
                m_sin_beta * cos_alpha,
                m_sin_beta * sin_alpha,
                m_cos_beta * cos_theta
            };
        }
        auto [new_cos_theta, new_phi] = xyz_to_spherical(rotated_position);
        m_corners[i] = omega_q(new_cos_theta, new_phi);
    }
}

double TetrahedralJump::jump() {
    m_corner_idx += m_chooser(m_rng);
    m_corner_idx %= 4;

    return m_corners[m_corner_idx];
}