#include "sim_posemath.h"

#include <assert.h>
#include <math.h>

int main(void) {
    sim_posemath_cartesian input = {3.0, 4.0, 0.0};
    sim_posemath_cartesian output = {0};
    sim_posemath_cartesian end = {10.0, 10.0, 10.0};
    double dot = 0;

    assert(sim_posemath_normalize(&input, &output) == SIM_POSEMATH_OK);
    assert(fabs(output.x - 0.6) < 1e-12 && fabs(output.y - 0.8) < 1e-12);
    assert(sim_posemath_dot(&input, &end, &dot) == SIM_POSEMATH_OK && dot == 70.0);
    assert(sim_posemath_line_point(&input, &end, 0.5, &output) == SIM_POSEMATH_OK);
    assert(output.x == 6.5 && output.y == 7.0 && output.z == 5.0);
    assert(sim_posemath_line_point(&input, &end, 1.1, &output) == SIM_POSEMATH_ERR_RANGE);
    input.x = NAN;
    assert(sim_posemath_normalize(&input, &output) == SIM_POSEMATH_ERR_ARGUMENT);
    input.x = 0.0; input.y = 0.0; input.z = 0.0;
    assert(sim_posemath_normalize(&input, &output) == SIM_POSEMATH_ERR_DEGENERATE);
    return 0;
}