#include "sim_posemath.h"

#include "posemath.h"

#include <math.h>

static int finite_cart(const sim_posemath_cartesian *value) {
    return value != 0 && isfinite(value->x) && isfinite(value->y) && isfinite(value->z);
}

static PmCartesian to_pm(const sim_posemath_cartesian *value) {
    PmCartesian result = {value->x, value->y, value->z};
    return result;
}

static void from_pm(const PmCartesian *value, sim_posemath_cartesian *output) {
    output->x = value->x;
    output->y = value->y;
    output->z = value->z;
}

int sim_posemath_is_finite(const sim_posemath_cartesian *value) {
    return finite_cart(value);
}

int sim_posemath_normalize(const sim_posemath_cartesian *input, sim_posemath_cartesian *output) {
    PmCartesian source;
    PmCartesian result;
    double magnitude_squared = 0.0;
    if (!finite_cart(input) || output == 0) return SIM_POSEMATH_ERR_ARGUMENT;
    source = to_pm(input);
    if (pmCartMagSq(&source, &magnitude_squared) != 0 || !isfinite(magnitude_squared)) return SIM_POSEMATH_ERR_NONFINITE;
    if (magnitude_squared <= 1e-24) return SIM_POSEMATH_ERR_DEGENERATE;
    if (pmCartUnit(&source, &result) != 0) return SIM_POSEMATH_ERR_DEGENERATE;
    from_pm(&result, output);
    return SIM_POSEMATH_OK;
}

int sim_posemath_dot(const sim_posemath_cartesian *left, const sim_posemath_cartesian *right, double *output) {
    PmCartesian lhs;
    PmCartesian rhs;
    if (!finite_cart(left) || !finite_cart(right) || output == 0) return SIM_POSEMATH_ERR_ARGUMENT;
    lhs = to_pm(left);
    rhs = to_pm(right);
    if (pmCartCartDot(&lhs, &rhs, output) != 0 || !isfinite(*output)) return SIM_POSEMATH_ERR_NONFINITE;
    return SIM_POSEMATH_OK;
}

int sim_posemath_line_point(const sim_posemath_cartesian *start, const sim_posemath_cartesian *end,
                            double fraction, sim_posemath_cartesian *output) {
    PmCartesian begin;
    PmCartesian finish;
    PmCartesian result;
    PmCartesian delta;
    if (!finite_cart(start) || !finite_cart(end) || output == 0) return SIM_POSEMATH_ERR_ARGUMENT;
    if (!isfinite(fraction) || fraction < 0.0 || fraction > 1.0) return SIM_POSEMATH_ERR_RANGE;
    begin = to_pm(start);
    finish = to_pm(end);
    if (pmCartCartSub(&finish, &begin, &delta) != 0 || pmCartScalMult(&delta, fraction, &delta) != 0 || pmCartCartAdd(&begin, &delta, &result) != 0) return SIM_POSEMATH_ERR_DEGENERATE;
    from_pm(&result, output);
    return SIM_POSEMATH_OK;
}