// SPDX-FileCopyrightText: 2025 Filip Leonarski, Paul Scherrer Institute <filip.leonarski@psi.ch>
// SPDX-License-Identifier: GPL-3.0-only

#include <cmath>

#include "GoniometerAxis.h"
#include "JFJochException.h"

#define check_finite(param, val) if (!std::isfinite(val)) throw JFJochException(JFJochExceptionCategory::InputParameterInvalid, param)

GoniometerAxis::GoniometerAxis(const std::string& in_name,
                               float in_start,
                               float in_increment,
                               const Coord &in_axis,
                               const std::optional<Coord> &in_helical_step) {
    if (in_name.empty())
        throw JFJochException(JFJochExceptionCategory::InputParameterInvalid,
                              "Name of goniometer axis cannot be empty");

    check_finite("Rotation angle increment", in_increment);
    check_finite("Rotation angle start", in_start);

    if (in_axis.Length() == 0.0f)
        throw JFJochException(JFJochExceptionCategory::InputParameterInvalid,
                              "Rotation axis cannot have 0 length");

    name = in_name;
    start = in_start;
    increment = in_increment;
    axis = in_axis.Normalize(); // Make sure rotation axis is normalized!
    helical_step = in_helical_step;
}

std::string GoniometerAxis::GetName() const {
    return name;
}

float GoniometerAxis::GetStart_deg() const {
    return start;
}

float GoniometerAxis::GetIncrement_deg() const {
    return increment;
}

Coord GoniometerAxis::GetAxis() const {
    return axis;
}

std::optional<Coord> GoniometerAxis::GetHelicalStep() const {
    return helical_step;
}

Coord GoniometerAxis::GetPosition(int64_t image_number) const {
    return helical_step.value_or(Coord()) * static_cast<float>(image_number);
}

float GoniometerAxis::GetAngle_deg(float image_number) const {
    return start + increment * image_number;
}

std::vector<double> GoniometerAxis::GetAxisVector() const {
    return {axis[0], axis[1], axis[2]};
}

std::vector<double> GoniometerAxis::GetXContainer_m(int64_t max_image_number) const {
    if (!helical_step.has_value())
        return {};
    std::vector<double> angle_container(max_image_number);
    for (int32_t i = 0; i < max_image_number; i++)
        angle_container[i] = helical_step->x * i * 1e-6;
    return angle_container;
}

std::vector<double> GoniometerAxis::GetYContainer_m(int64_t max_image_number) const {
    if (!helical_step.has_value())
        return {};
    std::vector<double> angle_container(max_image_number);
    for (int32_t i = 0; i < max_image_number; i++)
        angle_container[i] = helical_step->y * i * 1e-6;
    return angle_container;
}

std::vector<double> GoniometerAxis::GetZContainer_m(int64_t max_image_number) const {
    if (!helical_step.has_value())
        return {};
    std::vector<double> angle_container(max_image_number);
    for (int32_t i = 0; i < max_image_number; i++)
        angle_container[i] = helical_step->z * i * 1e-6;
    return angle_container;
}

std::vector<double> GoniometerAxis::GetAngleContainer(int64_t max_image_number) const {
    std::vector<double> angle_container(max_image_number);
    for (int32_t i = 0; i < max_image_number; i++)
        angle_container[i] = GetAngle_deg(i);
    return angle_container;
}

GoniometerAxis &GoniometerAxis::ScreeningWedge(const std::optional<float> &input) {
    screening_wedge = input;
    return *this;
}

std::optional<float> GoniometerAxis::GetScreeningWedge() const {
    return screening_wedge;
}

float GoniometerAxis::GetWedge_deg() const {
    if (!screening_wedge.has_value())
        return GetIncrement_deg();
    return *screening_wedge;
}

std::vector<double> GoniometerAxis::GetAngleContainerEnd(int64_t max_image_number) const {
    float wedge = GetWedge_deg();
    std::vector<double> angle_container(max_image_number);
    for (int32_t i = 0; i < max_image_number; i++)
        angle_container[i] = GetAngle_deg(i) + wedge;
    return angle_container;
}

RotMatrix GoniometerAxis::GetTransformation(int64_t image_number) const {
    // Transformation goes back from rotated to "start"
    auto angle_deg = GetAngle_deg(image_number);
    auto angle_rad = angle_deg / 180.0f * static_cast<float>(M_PI);
    return {angle_rad, axis};
}