#ifndef _BUTIS_MOTOR_H_INCLUDE_
#define _BUTIS_MOTOR_H_INCLUDE_

#include <basic-geometry.h>

// =================== Types ==================== //

typedef struct {
    BgcVector3FP32 point, vector, momentum;
} BtMotorFP32;

typedef struct {
    BgcVector3FP64 point, vector, momentum;
} BtMotorFP64;

// =================== Reset ==================== //

inline void bt_motor_reset_fp32(BtMotorFP32* motor)
{
    bgc_vector3_reset_fp32(&motor->point);
    bgc_vector3_reset_fp32(&motor->vector);
    bgc_vector3_reset_fp32(&motor->momentum);
}

inline void bt_motor_reset_fp64(BtMotorFP64* motor)
{
    bgc_vector3_reset_fp64(&motor->point);
    bgc_vector3_reset_fp64(&motor->vector);
    bgc_vector3_reset_fp64(&motor->momentum);
}

// =============== Reset At Point =============== //

inline void bt_motor_reset_at_point_fp32(const BgcVector3FP32* point, BtMotorFP32* motor)
{
    bgc_vector3_copy_fp32(point, &motor->point);
    bgc_vector3_reset_fp32(&motor->vector);
    bgc_vector3_reset_fp32(&motor->momentum);
}

inline void bt_motor_reset_at_point_fp64(const BgcVector3FP64* point, BtMotorFP64* motor)
{
    bgc_vector3_copy_fp64(point, &motor->point);
    bgc_vector3_reset_fp64(&motor->vector);
    bgc_vector3_reset_fp64(&motor->momentum);
}

// ============== Reset At Values =============== //

inline void bt_motor_reset_with_values_fp32(const float x1, const float x2, const float x3, BtMotorFP32* motor)
{
    bgc_vector3_set_values_fp32(x1, x2, x3, &motor->point);
    bgc_vector3_reset_fp32(&motor->vector);
    bgc_vector3_reset_fp32(&motor->momentum);
}

inline void bt_motor_reset_with_values_fp64(const double x1, const double x2, const double x3, BtMotorFP64* motor)
{
    bgc_vector3_set_values_fp64(x1, x2, x3, &motor->point);
    bgc_vector3_reset_fp64(&motor->vector);
    bgc_vector3_reset_fp64(&motor->momentum);
}

// ==================== Copy ==================== //

inline void bt_motor_copy_fp32(const BtMotorFP32* source, BtMotorFP32* destination)
{
    bgc_vector3_copy_fp32(&source->point, &destination->point);
    bgc_vector3_copy_fp32(&source->vector, &destination->vector);
    bgc_vector3_copy_fp32(&source->momentum, &destination->momentum);
}

inline void bt_motor_copy_fp64(const BtMotorFP64* source, BtMotorFP64* destination)
{
    bgc_vector3_copy_fp64(&source->point, &destination->point);
    bgc_vector3_copy_fp64(&source->vector, &destination->vector);
    bgc_vector3_copy_fp64(&source->momentum, &destination->momentum);
}

// ================== Convert =================== //

inline void bt_motor_convert_fp32_to_fp64(const BtMotorFP32* source, BtMotorFP64* destination)
{
    bgc_vector3_convert_fp32_to_fp64(&source->point, &destination->point);
    bgc_vector3_convert_fp32_to_fp64(&source->vector, &destination->vector);
    bgc_vector3_convert_fp32_to_fp64(&source->momentum, &destination->momentum);
}

inline void bt_motor_convert_fp64_to_fp32(const BtMotorFP64* source, BtMotorFP32* destination)
{
    bgc_vector3_convert_fp64_to_fp32(&source->point, &destination->point);
    bgc_vector3_convert_fp64_to_fp32(&source->vector, &destination->vector);
    bgc_vector3_convert_fp64_to_fp32(&source->momentum, &destination->momentum);
}

// ==================== Swap ==================== //

inline void bt_motor_swap_fp32(BtMotorFP32* motor1, BtMotorFP32* motor2)
{
    bgc_vector3_swap_fp32(&motor2->point, &motor2->point);
    bgc_vector3_swap_fp32(&motor2->vector, &motor2->vector);
    bgc_vector3_swap_fp32(&motor2->momentum, &motor2->momentum);
}

inline void bt_motor_swap_fp64(BtMotorFP64* motor1, BtMotorFP64* motor2)
{
    bgc_vector3_swap_fp64(&motor2->point, &motor2->point);
    bgc_vector3_swap_fp64(&motor2->vector, &motor2->vector);
    bgc_vector3_swap_fp64(&motor2->momentum, &motor2->momentum);
}

// ================== Shift At ================== //

inline void bt_motor_shift_at_fp32(const BtMotorFP32* motor, const BgcVector3FP32* shift, BtMotorFP32* moved)
{
    BgcVector3FP32 momentum_difference;
    bgc_vector3_get_cross_product_fp32(shift, &motor->vector, &momentum_difference);
    bgc_vector3_copy_fp32(&motor->vector, &moved->vector);
    bgc_vector3_subtract_fp32(&motor->momentum, &momentum_difference, &moved->vector);
    bgc_vector3_add_fp32(&motor->point, shift, &moved->point);
}

inline void bt_motor_shift_at_fp64(const BtMotorFP64* motor, const BgcVector3FP64* shift, BtMotorFP64* moved)
{
    BgcVector3FP64 momentum_difference;
    bgc_vector3_get_cross_product_fp64(shift, &motor->vector, &momentum_difference);
    bgc_vector3_subtract_fp64(&motor->momentum, &momentum_difference, &moved->vector);
    bgc_vector3_copy_fp64(&motor->vector, &moved->vector);
    bgc_vector3_add_fp64(&motor->point, shift, &moved->point);
}

// ================== Shift To ================== //

inline void bt_motor_shift_to_fp32(const BtMotorFP32* motor, const BgcVector3FP32* destination, BtMotorFP32* moved)
{
    BgcVector3FP32 shift;
    BgcVector3FP32 momentum_change;
    bgc_vector3_subtract_fp32(&motor->point, destination, &shift);
    bgc_vector3_get_cross_product_fp32(&shift, &motor->vector, &momentum_change);
    bgc_vector3_copy_fp32(destination, &moved->point);
    bgc_vector3_copy_fp32(&motor->vector, &moved->vector);
    bgc_vector3_add_fp32(&motor->momentum, &momentum_change, &moved->momentum);
}

inline void bt_motor_shift_to_fp64(const BtMotorFP64* motor, const BgcVector3FP64* destination, BtMotorFP64* moved)
{
    BgcVector3FP64 shift;
    BgcVector3FP64 momentum_change;
    bgc_vector3_subtract_fp64(&motor->point, destination, &shift);
    bgc_vector3_get_cross_product_fp64(&shift, &motor->vector, &momentum_change);
    bgc_vector3_copy_fp64(destination, &moved->point);
    bgc_vector3_copy_fp64(&motor->vector, &moved->vector);
    bgc_vector3_add_fp64(&motor->momentum, &momentum_change, &moved->momentum);
}

// ================== Multiply ================== //

inline void bt_motor_multiply_at_number_fp32(const BtMotorFP32* multiplicand, const float multiplier, BtMotorFP32* product)
{
    bgc_vector3_copy_fp32(&multiplicand->point, &product->point);
    bgc_vector3_multiply_fp32(&multiplicand->vector, multiplier, &product->vector);
    bgc_vector3_multiply_fp32(&multiplicand->momentum, multiplier, &product->momentum);
}

inline void bt_motor_multiply_at_number_fp64(const BtMotorFP64* multiplicand, const double multiplier, BtMotorFP64* product)
{
    bgc_vector3_copy_fp64(&multiplicand->point, &product->point);
    bgc_vector3_multiply_fp64(&multiplicand->vector, multiplier, &product->vector);
    bgc_vector3_multiply_fp64(&multiplicand->momentum, multiplier, &product->momentum);
}

// =================== Divide =================== //

inline void bt_motor_divide_at_number_fp32(const BtMotorFP32* divident, const float divisor, BtMotorFP32* quotient)
{
    bt_motor_multiply_at_number_fp32(divident, 1.0f / divisor, quotient);
}

inline void bt_motor_divide_at_number_fp64(const BtMotorFP64* divident, const double divisor, BtMotorFP64* quotient)
{
    bt_motor_multiply_at_number_fp64(divident, 1.0 / divisor, quotient);
}

// ==================== Add ===================== //

inline void bt_motor_add_vector_fp32(const BgcVector3FP32* point, const BgcVector3FP32* vector, BtMotorFP32* sum)
{
    BgcVector3FP32 distance, momentum;

    bgc_vector3_subtract_fp32(point, &sum->point, &distance);
    bgc_vector3_get_cross_product_fp32(&distance, vector, &momentum);

    bgc_vector3_add_fp32(&sum->vector, vector, &sum->vector);
    bgc_vector3_add_fp32(&sum->momentum, &momentum, &sum->momentum);
}

inline void bt_motor_add_vector_fp64(const BgcVector3FP64* point, const BgcVector3FP64* vector, BtMotorFP64* sum)
{
    BgcVector3FP64 distance, momentum;

    bgc_vector3_subtract_fp64(point, &sum->point, &distance);
    bgc_vector3_get_cross_product_fp64(&distance, vector, &momentum);

    bgc_vector3_add_fp64(&sum->vector, vector, &sum->vector);
    bgc_vector3_add_fp64(&sum->momentum, &momentum, &sum->momentum);
}

// ================= Add Motor ================== //

inline void bt_motor_add_motor_fp32(const BtMotorFP32* motor, BtMotorFP32* sum)
{
    BgcVector3FP32 distance, momentum;

    bgc_vector3_subtract_fp32(&motor->point, &sum->point, &distance);
    bgc_vector3_get_cross_product_fp32(&distance, &motor->vector, &momentum);

    bgc_vector3_add_fp32(&momentum, &motor->vector, &momentum);

    bgc_vector3_add_fp32(&sum->vector, &motor->vector, &sum->vector);
    bgc_vector3_add_fp32(&sum->momentum, &momentum, &sum->momentum);
}

inline void bt_motor_add_motor_fp64(const BtMotorFP64* motor, BtMotorFP64* sum)
{
    BgcVector3FP64 distance, momentum;

    bgc_vector3_subtract_fp64(&motor->point, &sum->point, &distance);
    bgc_vector3_get_cross_product_fp64(&distance, &motor->vector, &momentum);

    bgc_vector3_add_fp64(&momentum, &motor->vector, &momentum);

    bgc_vector3_add_fp64(&sum->vector, &motor->vector, &sum->vector);
    bgc_vector3_add_fp64(&sum->momentum, &momentum, &sum->momentum);
}

// ================= Add Scaled ================= //

inline void bt_motor_add_scaled_vector_fp32(const BgcVector3FP32* point, const BgcVector3FP32* vector, const float scale, BtMotorFP32* sum)
{
    BgcVector3FP32 distance, momentum;

    bgc_vector3_subtract_fp32(point, &sum->point, &distance);
    bgc_vector3_get_cross_product_fp32(&distance, vector, &momentum);

    bgc_vector3_add_scaled_fp32(&sum->vector, vector, scale, &sum->vector);
    bgc_vector3_add_scaled_fp32(&sum->momentum, &momentum, scale, &sum->momentum);
}

inline void bt_motor_add_scaled_vector_fp64(const BgcVector3FP64* point, const BgcVector3FP64* vector, const double scale, BtMotorFP64* sum)
{
    BgcVector3FP64 distance, momentum;

    bgc_vector3_subtract_fp64(point, &sum->point, &distance);
    bgc_vector3_get_cross_product_fp64(&distance, vector, &momentum);

    bgc_vector3_add_scaled_fp64(&sum->vector, vector, scale, &sum->vector);
    bgc_vector3_add_scaled_fp64(&sum->momentum, &momentum, scale, &sum->momentum);
}

// ================== Subtract ================== //

inline void bt_motor_subtract_vector_fp32(const BgcVector3FP32* point, const BgcVector3FP32* vector, BtMotorFP32* difference)
{
    BgcVector3FP32 distance, momentum;

    bgc_vector3_subtract_fp32(point, &difference->point, &distance);
    bgc_vector3_get_cross_product_fp32(&distance, vector, &momentum);

    bgc_vector3_subtract_fp32(&difference->vector, vector, &difference->vector);
    bgc_vector3_subtract_fp32(&difference->momentum, &momentum, &difference->momentum);
}

inline void bt_motor_subtract_vector_fp64(const BgcVector3FP64* point, const BgcVector3FP64* vector, BtMotorFP64* difference)
{
    BgcVector3FP64 distance, momentum;

    bgc_vector3_subtract_fp64(point, &difference->point, &distance);
    bgc_vector3_get_cross_product_fp64(&distance, vector, &momentum);

    bgc_vector3_subtract_fp64(&difference->vector, vector, &difference->vector);
    bgc_vector3_subtract_fp64(&difference->momentum, &momentum, &difference->momentum);
}

// ============== Subtract Scaled =============== //

inline void bt_motor_subtract_scaled_vector_fp32(const BgcVector3FP32* point, const BgcVector3FP32* vector, const float scale, BtMotorFP32* motor)
{
    BgcVector3FP32 distance, momentum;

    bgc_vector3_subtract_fp32(point, &motor->point, &distance);
    bgc_vector3_get_cross_product_fp32(&distance, vector, &momentum);

    bgc_vector3_subtract_scaled_fp32(&motor->vector, vector, scale, &motor->vector);
    bgc_vector3_subtract_scaled_fp32(&motor->momentum, &momentum, scale, &motor->momentum);
}

inline void bt_motor_subtract_scaled_vector_fp64(const BgcVector3FP64* point, const BgcVector3FP64* vector, const double scale, BtMotorFP64* motor)
{
    BgcVector3FP64 distance, momentum;

    bgc_vector3_subtract_fp64(point, &motor->point, &distance);
    bgc_vector3_get_cross_product_fp64(&distance, vector, &momentum);

    bgc_vector3_subtract_scaled_fp64(&motor->vector, vector, scale, &motor->vector);
    bgc_vector3_subtract_scaled_fp64(&motor->momentum, &momentum, scale, &motor->momentum);
}

// ================= Is Vector ================== //

inline int bt_motor_is_vector_fp32(const BtMotorFP32* motor)
{
    return bgc_vector3_are_orthogonal_fp32(&motor->vector, &motor->momentum);
}

inline int bt_motor_is_vector_fp64(const BtMotorFP64* motor)
{
    return bgc_vector3_are_orthogonal_fp64(&motor->vector, &motor->momentum);
}

// ================== Is Screw ================== //

inline int bt_motor_is_screw_fp32(const BtMotorFP32* motor)
{
    return bgc_vector3_are_parallel_fp32(&motor->vector, &motor->momentum);
}

inline int bt_motor_is_screw_fp64(const BtMotorFP64* motor)
{
    return bgc_vector3_are_parallel_fp64(&motor->vector, &motor->momentum);
}

// ================= Make Screw ================= //

inline void bt_motor_make_screw_fp32(BtMotorFP32* motor)
{
    const float square_vector = bgc_vector3_get_square_modulus_fp32(&motor->vector);

    if (square_vector <= BGC_SQUARE_EPSYLON_FP32) {
        return;
    }

    const float multiplier = 1.0f / square_vector;
    BgcVector3FP32 cross_product;

    bgc_vector3_get_cross_product_fp32(&motor->vector, &motor->momentum, &cross_product);

    bgc_vector3_add_scaled_fp32(&motor->point, &cross_product, multiplier, &motor->point);

    bgc_vector3_multiply_fp32(&motor->vector, multiplier * bgc_vector3_get_scalar_product_fp32(&motor->vector, &motor->momentum), &motor->momentum);
}

inline void bt_motor_make_screw_fp64(BtMotorFP64* motor)
{
    const double square_vector = bgc_vector3_get_square_modulus_fp64(&motor->vector);

    if (square_vector <= BGC_SQUARE_EPSYLON_FP64) {
        return;
    }

    const double multiplier = 1.0 / square_vector;
    BgcVector3FP64 cross_product;

    bgc_vector3_get_cross_product_fp64(&motor->vector, &motor->momentum, &cross_product);

    bgc_vector3_add_scaled_fp64(&motor->point, &cross_product, multiplier, &motor->point);

    bgc_vector3_multiply_fp64(&motor->vector, multiplier * bgc_vector3_get_scalar_product_fp64(&motor->vector, &motor->momentum), &motor->momentum);
}

// ================= Get Screw ================== //

inline void bt_motor_get_screw_fp32(const BtMotorFP32* motor, BtMotorFP32* screw)
{
    const float square_vector = bgc_vector3_get_square_modulus_fp32(&motor->vector);

    bgc_vector3_copy_fp32(&motor->vector, &screw->vector);

    if (square_vector <= BGC_SQUARE_EPSYLON_FP32) {
        bgc_vector3_copy_fp32(&motor->point, &screw->point);
        bgc_vector3_copy_fp32(&motor->momentum, &screw->momentum);
        return;
    }

    const float multiplier = 1.0f / square_vector;
    BgcVector3FP32 cross_product;

    bgc_vector3_get_cross_product_fp32(&motor->vector, &motor->momentum, &cross_product);

    bgc_vector3_add_scaled_fp32(&motor->point, &cross_product, multiplier, &screw->point);

    bgc_vector3_multiply_fp32(&motor->vector, multiplier * bgc_vector3_get_scalar_product_fp32(&motor->vector, &motor->momentum), &screw->momentum);
}

inline void bt_motor_get_screw_fp64(const BtMotorFP64* motor, BtMotorFP64* screw)
{
    const double square_vector = bgc_vector3_get_square_modulus_fp64(&motor->vector);

    bgc_vector3_copy_fp64(&motor->vector, &screw->vector);

    if (square_vector <= BGC_SQUARE_EPSYLON_FP64) {
        bgc_vector3_copy_fp64(&motor->point, &screw->point);
        bgc_vector3_copy_fp64(&motor->momentum, &screw->momentum);
        return;
    }

    const double multiplier = 1.0 / square_vector;
    BgcVector3FP64 cross_product;

    bgc_vector3_get_cross_product_fp64(&motor->vector, &motor->momentum, &cross_product);

    bgc_vector3_add_scaled_fp64(&motor->point, &cross_product, multiplier, &screw->point);

    bgc_vector3_multiply_fp64(&motor->vector, multiplier * bgc_vector3_get_scalar_product_fp64(&motor->vector, &motor->momentum), &screw->momentum);
}

#endif