#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_NONFINITE);
    input.x = 0.0; input.y = 0.0; input.z = 0.0;
    assert(sim_posemath_normalize(&input, &output) == SIM_POSEMATH_ERR_DEGENERATE);

    {
        sim_posemath_pose start = {{0.0, 0.0, 0.0}, 0, 0, 0, 0, 0, 0};
        sim_posemath_pose finish = {{2.0, 0.0, 0.0}, 1, 2, 3, 4, 5, 6};
        sim_posemath_pose point = {0};
        sim_posemath_segment segment = {start, finish, {0, 0, 0}, {0, 0, 1}, 1.0, 2.0, SIM_POSEMATH_LINEAR, 0};
        sim_tp planner = {0};
        sim_tp_state first = {0};
        sim_tp_state second = {0};
        uint32_t id = 0;
        assert(sim_posemath_pose_line_point(&start, &finish, 0.5, &point) == SIM_POSEMATH_OK);
        assert(point.tran.x == 1.0 && point.a == 0.5 && point.w == 3.0);
        assert(sim_posemath_segment_validate(&segment) == SIM_POSEMATH_OK);
        assert(sim_posemath_segment_point(&segment, 0.5, &point) == SIM_POSEMATH_OK);
        assert(sim_tp_init(&planner, 0.1, 10.0, 10.0) == SIM_POSEMATH_OK);
        assert(sim_tp_enqueue(&planner, &segment, &id) == SIM_POSEMATH_OK && id == 1);
        assert(sim_tp_step(&planner) == SIM_POSEMATH_OK);
        assert(sim_tp_read_state(&planner, &first) == SIM_POSEMATH_OK);
        assert(sim_tp_state_hash(&planner) == sim_posemath_hash_state(&first));
        assert(sim_tp_read_state(&planner, &second) == SIM_POSEMATH_OK);
        assert(sim_posemath_hash_state(&first) == sim_posemath_hash_state(&second));
        segment.motion_type = 99;
        assert(sim_posemath_segment_validate(&segment) == SIM_POSEMATH_ERR_MOTION_TYPE);
        segment.motion_type = SIM_POSEMATH_LINEAR;
        segment.velocity = 11.0;
        assert(sim_tp_enqueue(&planner, &segment, 0) == SIM_POSEMATH_ERR_RANGE);
    }
    return 0;
}