#include "printing_utils.h"
#include "vector3_pair_difference.h"

// ======== Float Pointer 32-bit version ======== //

static void _test_fp32_no_turn()
{
    BGC_FP32_Vector3 initial_main = { 1.0f, 0.0f, 0.0f };
    BGC_FP32_Vector3 initial_branch = { 0.0f, 1.0f, 0.0f };
    BGC_FP32_Vector3 final_main = { 1.0f, 0.0f, 0.0f };
    BGC_FP32_Vector3 final_branch = { 0.0f, 1.0f, 0.0f };
    BGC_FP32_Turn3 turn;

    bgc_fp32_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);

    printf("\nNo turn:\n");
    print_fp32_quaternion(&turn._versor);
}

static void _test_fp32_turn_at_180deg_v1()
{
    BGC_FP32_Vector3 initial_main = { 1.0f, 0.0f, 0.0f };
    BGC_FP32_Vector3 initial_branch = { 0.0f, 1.0f, 0.0f };
    BGC_FP32_Vector3 final_main = { 0.0f, 1.0f, 0.0f };
    BGC_FP32_Vector3 final_branch = { 1.0f, 0.0f, 0.0f };
    BGC_FP32_Turn3 turn;

    bgc_fp32_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);

    printf("\nTurn around (1, 1, 0) axis on 180 degrees:\n");
    print_fp32_quaternion(&turn._versor);
}

static void _test_fp32_turn_at_180deg_v2()
{
    BGC_FP32_Vector3 initial_main = { 1.0f, 0.0f, 0.0f };
    BGC_FP32_Vector3 initial_branch = { 0.0f, 1.0f, 0.0f };
    BGC_FP32_Vector3 final_main = { -1.0f, 0.0f, 0.0f };
    BGC_FP32_Vector3 final_branch = { 0.0f, 1.0f, 0.0f };
    BGC_FP32_Turn3 turn;

    bgc_fp32_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);

    printf("\nTurn around (0, 1, 0) axis on 180 degrees:\n");
    print_fp32_quaternion(&turn._versor);
}

static void _test_fp32_turn_at_90deg_v1()
{
    BGC_FP32_Vector3 initial_main = { 2.0f, 0.0f, 0.0f };
    BGC_FP32_Vector3 initial_branch = { 0.0f, 3.1f, 0.0f };
    BGC_FP32_Vector3 final_main = { 0.0f, 10.0f, 0.0f };
    BGC_FP32_Vector3 final_branch = { -1.0f, 0.0f, 0.0f };
    BGC_FP32_Turn3 turn;

    bgc_fp32_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);

    printf("\n90 degree turn around (0, 0, 1):\n");
    print_fp32_quaternion(&turn._versor);
}

static void _test_fp32_turn_at_90deg_v2()
{
    BGC_FP32_Vector3 initial_main = { 2.0f, 0.0f, 0.0f };
    BGC_FP32_Vector3 initial_branch = { 0.0f, 3.1f, 0.0f };
    BGC_FP32_Vector3 final_main = { 0.0f, 10.0f, 0.0f };
    BGC_FP32_Vector3 final_branch = { -1.0f, 5.0f, 0.0f };
    BGC_FP32_Turn3 turn;

    bgc_fp32_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);

    printf("\nUnorthogonal pairs turn at 90 degrees around (0, 0, 1):\n");
    print_fp32_quaternion(&turn._versor);
}

static void _test_fp32_zero_vectors()
{
    BGC_FP32_Vector3 initial_main = { 0.0f, 0.0f, 0.0f };
    BGC_FP32_Vector3 initial_branch = { 0.0f, 1.0f, 0.0f };
    BGC_FP32_Vector3 final_main = { 1.0f, 0.0f, 0.0f };
    BGC_FP32_Vector3 final_branch = { 0.0f, 1.0f, 0.0f };
    BGC_FP32_Turn3 turn;

    bgc_fp32_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);

    const int code = bgc_fp32_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);

    if (code == BGC_SUCCESS) {
        printf("\nZero vectors: this cannot be!\n");
        print_fp32_quaternion(&turn._versor);
    }
    else {
        printf("\nZero vector validation works fine\n");
    }
}

static void _test_fp32_parallel_vectors()
{
    BGC_FP32_Vector3 initial_main = { 1.0f, 0.0f, 0.0f };
    BGC_FP32_Vector3 initial_branch = { 2.0f, 0.0f, 0.0f };
    BGC_FP32_Vector3 final_main = { 1.0f, 0.0f, 0.0f };
    BGC_FP32_Vector3 final_branch = { 0.0f, 1.0f, 0.0f };
    BGC_FP32_Turn3 turn;

    bgc_fp32_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);

    const int code = bgc_fp32_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);

    if (code == BGC_SUCCESS) {
        printf("\nParallel vectors: this cannot be!\n");
        print_fp32_quaternion(&turn._versor);
    }
    else {
        printf("\nParallelism validation works fine\n");
    }
}

static void _test_fp32_small_angle_turn()
{
    BGC_FP32_Vector3 initial_main = { 1.0f, 0.0f, 0.0f };
    BGC_FP32_Vector3 initial_branch = { 0.0f, 1.0f, 0.0f };
    BGC_FP32_Vector3 final_main = { 0.999848f, 0.017452f, 0.0f };
    BGC_FP32_Vector3 final_branch = { -0.017452f, 0.999848f, 0.0f };
    BGC_FP32_Turn3 turn;

    bgc_fp32_turn3_find_pair_difference(&turn , &initial_main, &initial_branch, &final_main, &final_branch);

    printf("\nSmall angle turn (about 1 degree):\n");
    print_fp32_quaternion(&turn._versor);
}

static void _test_fp32_turn_at_179deg()
{
    BGC_FP32_Vector3 initial_main = { 1.0f, 0.0f, 0.0f };
    BGC_FP32_Vector3 initial_branch = { 0.0f, 1.0f, 0.0f };
    BGC_FP32_Vector3 final_main = { -0.999848f, -0.017452f, 0.0f };
    BGC_FP32_Vector3 final_branch = { 0.017452f, -0.999848f, 0.0f };
    BGC_FP32_Turn3 turn;

    bgc_fp32_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);

    printf("\nTurn around (0, 1, 0) axis on 179 degrees:\n");
    print_fp32_quaternion(&turn._versor);
}

static void _test_fp32_turn_at_120deg_around()
{
    // 120 degrees around (-1, -1, 1)
    BGC_FP32_Vector3 initial_main = { 1.0f, 0.0f, 0.0f };
    BGC_FP32_Vector3 initial_branch = { 0.0f, 1.0f, 0.0f };
    BGC_FP32_Vector3 final_main = { 0.0f, 1.0f, 0.0f };
    BGC_FP32_Vector3 final_branch = { 0.0f, 0.0f, -1.0f };
    BGC_FP32_Turn3 turn;

    bgc_fp32_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);

    printf("\n120 degees turn:\n");
    print_fp32_quaternion(&turn._versor);
}

static void _test_fp32_1deg_difference()
{
    BGC_FP32_Vector3 initial_main = { 1.0f, 0.0f, 0.0f };
    BGC_FP32_Vector3 initial_branch = { 0.999848f, 0.017452f, 0.0f };
    BGC_FP32_Vector3 final_main = { 0.0f, 1.0f, 0.0f };
    BGC_FP32_Vector3 final_branch = { -1.0f, 0.0f, 0.0f };
    BGC_FP32_Turn3 turn;

    bgc_fp32_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);

    printf("\nAbout 1 degree turn difference between initial_main and initial_branch directions:\n");
    print_fp32_quaternion(&turn._versor);
}

static void _test_fp32_0_01deg_difference()
{
    BGC_FP32_Vector3 initial_main = { 1.0f, 0.0f, 0.0f };
    BGC_FP32_Vector3 initial_branch = { 1.0f, 0.000001f, 0.0f };
    BGC_FP32_Vector3 final_main = { 0.0f, -1.0f, 0.0f };
    BGC_FP32_Vector3 final_branch = { 1.0f, 0.0f, 0.0f };
    BGC_FP32_Turn3 turn;

    bgc_fp32_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);

    printf("\nAbout 0.01 degree turn difference between initial_main and initial_branch directions:\n");
    print_fp32_quaternion(&turn._versor);
}

static void _test_fp32_little_z_influence()
{
    // почти (0,1,0), но крошечный z
    BGC_FP32_Vector3 initial_main = { 1.0f, 0.0f, 0.0f };
    BGC_FP32_Vector3 initial_branch = { 0.0f, 0.999999f, 0.00014142f };
    BGC_FP32_Vector3 final_main = { -0.999999f, 0.0f, 0.00014142f };
    BGC_FP32_Vector3 final_branch = { 0.0f, 0.999999f, -0.00014142f };
    BGC_FP32_Turn3 turn;

    bgc_fp32_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);

    printf("\nNear 180° with tiny branch deviation:\n");
    print_fp32_quaternion(&turn._versor);
}

static void _test_fp32_turn_restoring()
{
    BGC_FP32_Vector3 initial_main = { -0.999999f, 0.0f, 0.00014142f };
    BGC_FP32_Vector3 initial_branch = { 0.0f, 0.999999f, -0.00014142f };
    BGC_FP32_Vector3 final_main;
    BGC_FP32_Vector3 final_branch;
    BGC_FP32_Turn3 known, turn;

    bgc_fp32_turn3_set_rotation(&known, 0.0f, 0.0f, 1.0f, 90.0f, BGC_ANGLE_UNIT_DEGREES);

    bgc_fp32_turn3_vector(&final_main,   &known, &initial_main);
    bgc_fp32_turn3_vector(&final_branch, &known, &initial_branch);

    bgc_fp32_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);
    printf("\nRecover known 90° Z rotation:\n");
    print_fp32_quaternion(&turn._versor);

    printf("Known was: ");
    print_fp32_quaternion(&known._versor);
}

void test_fp32_pair_difference()
{
    _test_fp32_no_turn();

    _test_fp32_turn_at_180deg_v1();

    _test_fp32_turn_at_180deg_v2();

    _test_fp32_turn_at_90deg_v1();

    _test_fp32_turn_at_90deg_v2();

    _test_fp32_zero_vectors();

    _test_fp32_parallel_vectors();

    _test_fp32_small_angle_turn();

    _test_fp32_turn_at_179deg();

    _test_fp32_turn_at_120deg_around();

    _test_fp32_1deg_difference();

    _test_fp32_0_01deg_difference();

    _test_fp32_little_z_influence();

    _test_fp32_turn_restoring();
}

// ======== Float Pointer 64-bit version ======== //

static void _test_fp64_no_turn()
{
    BGC_FP64_Vector3 initial_main = { 1.0f, 0.0f, 0.0f };
    BGC_FP64_Vector3 initial_branch = { 0.0f, 1.0f, 0.0f };
    BGC_FP64_Vector3 final_main = { 1.0f, 0.0f, 0.0f };
    BGC_FP64_Vector3 final_branch = { 0.0f, 1.0f, 0.0f };
    BGC_FP64_Turn3 turn;

    bgc_fp64_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);

    printf("\nNo turn:\n");
    print_fp64_quaternion(&turn._versor);
}

static void _test_fp64_turn_at_180deg_v1()
{
    BGC_FP64_Vector3 initial_main = { 1.0f, 0.0f, 0.0f };
    BGC_FP64_Vector3 initial_branch = { 0.0f, 1.0f, 0.0f };
    BGC_FP64_Vector3 final_main = { 0.0f, 1.0f, 0.0f };
    BGC_FP64_Vector3 final_branch = { 1.0f, 0.0f, 0.0f };
    BGC_FP64_Turn3 turn;

    bgc_fp64_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);

    printf("\nTurn around (1, 1, 0) axis on 180 degrees:\n");
    print_fp64_quaternion(&turn._versor);
}

static void _test_fp64_turn_at_180deg_v2()
{
    BGC_FP64_Vector3 initial_main = { 1.0f, 0.0f, 0.0f };
    BGC_FP64_Vector3 initial_branch = { 0.0f, 1.0f, 0.0f };
    BGC_FP64_Vector3 final_main = { -1.0f, 0.0f, 0.0f };
    BGC_FP64_Vector3 final_branch = { 0.0f, 1.0f, 0.0f };
    BGC_FP64_Turn3 turn;

    bgc_fp64_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);

    printf("\nTurn around (0, 1, 0) axis on 180 degrees:\n");
    print_fp64_quaternion(&turn._versor);
}

static void _test_fp64_turn_at_90deg_v1()
{
    BGC_FP64_Vector3 initial_main = { 2.0f, 0.0f, 0.0f };
    BGC_FP64_Vector3 initial_branch = { 0.0f, 3.1f, 0.0f };
    BGC_FP64_Vector3 final_main = { 0.0f, 10.0f, 0.0f };
    BGC_FP64_Vector3 final_branch = { -1.0f, 0.0f, 0.0f };
    BGC_FP64_Turn3 turn;

    bgc_fp64_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);

    printf("\n90 degree turn around (0, 0, 1):\n");
    print_fp64_quaternion(&turn._versor);
}

static void _test_fp64_turn_at_90deg_v2()
{
    BGC_FP64_Vector3 initial_main = { 2.0f, 0.0f, 0.0f };
    BGC_FP64_Vector3 initial_branch = { 0.0f, 3.1f, 0.0f };
    BGC_FP64_Vector3 final_main = { 0.0f, 10.0f, 0.0f };
    BGC_FP64_Vector3 final_branch = { -1.0f, 5.0f, 0.0f };
    BGC_FP64_Turn3 turn;

    bgc_fp64_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);

    printf("\nUnorthogonal pairs turn at 90 degrees around (0, 0, 1):\n");
    print_fp64_quaternion(&turn._versor);
}

static void _test_fp64_zero_vectors()
{
    BGC_FP64_Vector3 initial_main = { 0.0f, 0.0f, 0.0f };
    BGC_FP64_Vector3 initial_branch = { 0.0f, 1.0f, 0.0f };
    BGC_FP64_Vector3 final_main = { 1.0f, 0.0f, 0.0f };
    BGC_FP64_Vector3 final_branch = { 0.0f, 1.0f, 0.0f };
    BGC_FP64_Turn3 turn;

    bgc_fp64_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);

    const int code = bgc_fp64_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);

    if (code == BGC_SUCCESS) {
        printf("\nZero vectors: this cannot be!\n");
        print_fp64_quaternion(&turn._versor);
    }
    else {
        printf("\nZero vector validation works fine\n");
    }
}

static void _test_fp64_parallel_vectors()
{
    BGC_FP64_Vector3 initial_main = { 1.0f, 0.0f, 0.0f };
    BGC_FP64_Vector3 initial_branch = { 2.0f, 0.0f, 0.0f };
    BGC_FP64_Vector3 final_main = { 1.0f, 0.0f, 0.0f };
    BGC_FP64_Vector3 final_branch = { 0.0f, 1.0f, 0.0f };
    BGC_FP64_Turn3 turn;

    bgc_fp64_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);

    const int code = bgc_fp64_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);

    if (code == BGC_SUCCESS) {
        printf("\nParallel vectors: this cannot be!\n");
        print_fp64_quaternion(&turn._versor);
    }
    else {
        printf("\nParallelism validation works fine\n");
    }
}

static void _test_fp64_small_angle_turn()
{
    BGC_FP64_Vector3 initial_main = { 1.0f, 0.0f, 0.0f };
    BGC_FP64_Vector3 initial_branch = { 0.0f, 1.0f, 0.0f };
    BGC_FP64_Vector3 final_main = { 0.999848f, 0.017452f, 0.0f };
    BGC_FP64_Vector3 final_branch = { -0.017452f, 0.999848f, 0.0f };
    BGC_FP64_Turn3 turn;

    bgc_fp64_turn3_find_pair_difference(&turn , &initial_main, &initial_branch, &final_main, &final_branch);

    printf("\nSmall angle turn (about 1 degree):\n");
    print_fp64_quaternion(&turn._versor);
}

static void _test_fp64_turn_at_179deg()
{
    BGC_FP64_Vector3 initial_main = { 1.0f, 0.0f, 0.0f };
    BGC_FP64_Vector3 initial_branch = { 0.0f, 1.0f, 0.0f };
    BGC_FP64_Vector3 final_main = { -0.999848f, -0.017452f, 0.0f };
    BGC_FP64_Vector3 final_branch = { 0.017452f, -0.999848f, 0.0f };
    BGC_FP64_Turn3 turn;

    bgc_fp64_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);

    printf("\nTurn around (0, 1, 0) axis on 179 degrees:\n");
    print_fp64_quaternion(&turn._versor);
}

static void _test_fp64_turn_at_120deg_around()
{
    // 120 degrees around (-1, -1, 1)
    BGC_FP64_Vector3 initial_main = { 1.0f, 0.0f, 0.0f };
    BGC_FP64_Vector3 initial_branch = { 0.0f, 1.0f, 0.0f };
    BGC_FP64_Vector3 final_main = { 0.0f, 1.0f, 0.0f };
    BGC_FP64_Vector3 final_branch = { 0.0f, 0.0f, -1.0f };
    BGC_FP64_Turn3 turn;

    bgc_fp64_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);

    printf("\n120 degees turn:\n");
    print_fp64_quaternion(&turn._versor);
}

static void _test_fp64_1deg_difference()
{
    BGC_FP64_Vector3 initial_main = { 1.0f, 0.0f, 0.0f };
    BGC_FP64_Vector3 initial_branch = { 0.999848f, 0.017452f, 0.0f };
    BGC_FP64_Vector3 final_main = { 0.0f, 1.0f, 0.0f };
    BGC_FP64_Vector3 final_branch = { -1.0f, 0.0f, 0.0f };
    BGC_FP64_Turn3 turn;

    bgc_fp64_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);

    printf("\nAbout 1 degree turn difference between initial_main and initial_branch directions:\n");
    print_fp64_quaternion(&turn._versor);
}

static void _test_fp64_0_01deg_difference()
{
    BGC_FP64_Vector3 initial_main = { 1.0f, 0.0f, 0.0f };
    BGC_FP64_Vector3 initial_branch = { 1.0f, 0.000001f, 0.0f };
    BGC_FP64_Vector3 final_main = { 0.0f, -1.0f, 0.0f };
    BGC_FP64_Vector3 final_branch = { 1.0f, 0.0f, 0.0f };
    BGC_FP64_Turn3 turn;

    bgc_fp64_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);

    printf("\nAbout 0.01 degree turn difference between initial_main and initial_branch directions:\n");
    print_fp64_quaternion(&turn._versor);
}

static void _test_fp64_little_z_influence()
{
    // почти (0,1,0), но крошечный z
    BGC_FP64_Vector3 initial_main = { 1.0f, 0.0f, 0.0f };
    BGC_FP64_Vector3 initial_branch = { 0.0f, 0.999999f, 0.00014142f };
    BGC_FP64_Vector3 final_main = { -0.999999f, 0.0f, 0.00014142f };
    BGC_FP64_Vector3 final_branch = { 0.0f, 0.999999f, -0.00014142f };
    BGC_FP64_Turn3 turn;

    bgc_fp64_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);

    printf("\nNear 180° with tiny branch deviation:\n");
    print_fp64_quaternion(&turn._versor);
}

static void _test_fp64_turn_restoring()
{
    BGC_FP64_Vector3 initial_main = { -0.999999f, 0.0f, 0.00014142f };
    BGC_FP64_Vector3 initial_branch = { 0.0f, 0.999999f, -0.00014142f };
    BGC_FP64_Vector3 final_main;
    BGC_FP64_Vector3 final_branch;
    BGC_FP64_Turn3 known, turn;

    bgc_fp64_turn3_set_rotation(&known, 0.0f, 0.0f, 1.0f, 90.0f, BGC_ANGLE_UNIT_DEGREES);

    bgc_fp64_turn3_vector(&final_main,   &known, &initial_main);
    bgc_fp64_turn3_vector(&final_branch, &known, &initial_branch);

    bgc_fp64_turn3_find_pair_difference(&turn, &initial_main, &initial_branch, &final_main, &final_branch);
    printf("\nRecover known 90° Z rotation:\n");
    print_fp64_quaternion(&turn._versor);

    printf("Known was: ");
    print_fp64_quaternion(&known._versor);
}

void test_fp64_pair_difference()
{
    _test_fp64_no_turn();

    _test_fp64_turn_at_180deg_v1();

    _test_fp64_turn_at_180deg_v2();

    _test_fp64_turn_at_90deg_v1();

    _test_fp64_turn_at_90deg_v2();

    _test_fp64_zero_vectors();

    _test_fp64_parallel_vectors();

    _test_fp64_small_angle_turn();

    _test_fp64_turn_at_179deg();

    _test_fp64_turn_at_120deg_around();

    _test_fp64_1deg_difference();

    _test_fp64_0_01deg_difference();

    _test_fp64_little_z_influence();

    _test_fp64_turn_restoring();
}