C++的各种用法展示（一）

// A number is called as Armstrong number if sum of cubes of digits of number is
// equal to the number itself.
// For Example 153 is an Armstrong number because 153 = 1³+5³+3³.
#include <stdio.h>
// Function to calculate x raised to the power y
int power(int x, unsigned int y)
{
    if (y == 0)
        return 1;
    if (y % 2 == 0)
        return power(x, y / 2) * power(x, y / 2);
    return x * power(x, y / 2) * power(x, y / 2);
}
// Function to calculate order of the number
int order(int x)
{
    int n = 0;
    while (x)
    {
        n++;
        x = x / 10;
    }
    return n;
}
// Function to check whether the given number is
// Armstrong number or not
int isArmstrong(int x)
{
    // Calling order function
    int n = order(x);
    int temp = x, sum = 0;
    while (temp)
    {
        int r = temp % 10;
        sum += power(r, n);
        temp = temp / 10;
    }
    // If satisfies Armstrong condition
    if (sum == x)
        return 1;
    else
        return 0;
}
//
int main()
{
    int x = 153;
    if (isArmstrong(x) == 1)
        printf("True\n");
    else
        printf("False\n");
    x = 1253;
    if (isArmstrong(x) == 1)
        printf("True\n");
    else
        printf("False\n");
    return 0;
}

/**
 * @file
 * @brief Program to generate [Cantor ternary
 * set](https://en.wikipedia.org/wiki/Cantor_set)
 */
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
/** structure to define Cantor set */
typedef struct _cantor_set
{
    double start;             /**< start of interval */
    double end;               /**< end of interval */
    struct _cantor_set *next; /**< pointer to next set */
} CantorSet;
/** Iterative constructor of all sets in the current level. This function
 * dynamically allocates memory when creating new sets. These are freed by the
 * function ::free_memory.
 * @param head pointer to interval set instance to update
 */
void propagate(CantorSet *head)
{
    // if input is NULL, ignore the process
    if (head == NULL)
        return;
    CantorSet *temp = head;  // local pointer to track propagation
    // create new node for the new set
    CantorSet *newNode = (CantorSet *)malloc(sizeof(CantorSet));
    // get 1/3rd of interval
    double diff = (((temp->end) - (temp->start)) / 3);
    // update interval ranges
    newNode->end = temp->end;
    temp->end = ((temp->start) + diff);
    newNode->start = (newNode->end) - diff;
    // update pointer to next set in this level
    newNode->next = temp->next;
    // point to next set
    temp->next = newNode;
    // create next set
    propagate(temp->next->next);
}
/** Print sets in the current range to `stdout`
 * @param head pointer to first set in the current level
 */
void print(CantorSet *head)
{
    CantorSet *temp = head;
    while (temp != NULL)  // print while a valid set is found
    {
        printf("\t");
        printf("[%lf] -- ", temp->start);
        printf("[%lf]", temp->end);
        temp = temp->next;
    }
    printf("\n");
}
/** Clear memory allocated by ::propagate function.
 * @param head pointer to first allocated instance.
 */
void free_memory(CantorSet *head)
{
    if (!head)
        return;
    if (head->next)
        free_memory(head->next);
    free(head);
}
/** Main function */
int main(int argc, char const *argv[])
{
    int start_num, end_num, levels;
    if (argc < 2)
    {
        printf("Enter 3 arguments: start_num \t end_num \t levels\n");
        scanf("%d %d %d", &start_num, &end_num, &levels);
    }
    else
    {
        start_num = atoi(argv[1]);
        end_num = atoi(argv[2]);
        levels = atoi(argv[3]);
    }
    if (start_num < 0 || end_num < 0 || levels < 0)
    {
        fprintf(stderr, "All numbers must be positive\n");
        return -1;
    }
    CantorSet head = {.start = start_num, .end = end_num, .next = NULL};
    // loop to propagate each level from top to bottom
    for (int i = 0; i < levels; i++)
    {
        printf("Level %d\t", i);
        print(&head);
        propagate(&head);
        printf("\n");
    }
    printf("Level %d\t", levels);
    print(&head);
    // delete all memory allocated
    free_memory(head.next);
    return 0;
}

/**
 * @file
 * @brief Program to print the nth term of the Fibonacci series.
 * @details 
 * Fibonacci series generally starts from 0 and 1. Every next term in
 * the series is equal to the sum of the two preceding terms.
 * For further info: https://en.wikipedia.org/wiki/Fibonacci_sequence
 *
 * @author [Luiz Carlos Aguiar C](https://github.com/IKuuhakuI)
 * @author [Niranjan](https://github.com/niranjank2022)
 */
#include <assert.h>  /// for assert()
#include <errno.h>  /// for errno - to determine whether there is an error while using strtol()
#include <stdio.h>   /// for input, output
#include <stdlib.h>  /// for exit() - to exit the program
#include <time.h>   /// to calculate time taken by fib()
/**
 * @brief Determines the nth Fibonacci term
 * @param number - n in "nth term" and it can't be negative as well as zero
 * @return nth term in unsigned type
 * @warning
 * Only till 47th and 48th fibonacci element can be stored in `int` and
 * `unsigned int` respectively (takes more than 20 seconds to print)
 */
unsigned int fib(int number)
{
    // Check for negative integers
    if (number <= 0)
    {
        fprintf(stderr, "Illegal Argument Is Passed!\n");
        exit(EXIT_FAILURE);
    }
    // Base conditions
    if (number == 1)
        return 0;
    if (number == 2)
        return 1;
    // Recursive call to the function
    return fib(number - 1) + fib(number - 2);
}
/**
 * @brief Get the input from the user
 * @return valid argument to the fibonacci function
 */
int getInput(void)
{
    int num, excess_len;
    char buffer[3], *endPtr;
    while (1)
    {  // Repeat until a valid number is entered
        printf("Please enter a valid number:");
        fgets(buffer, 3, stdin);  // Inputs the value from user
        excess_len = 0;
        if (!(buffer[0] == '\n' ||
            buffer[1] == '\n' ||
            buffer[2] == '\n')) {
            while (getchar() != '\n') excess_len++;
        }
        num = strtol(buffer, &endPtr,
                     10);  // Attempts to convert the string to integer
        // Checking the input
        if (  // The number is too large
            (excess_len > 0 || num > 48) ||
            // Characters other than digits are included in the input
            (*endPtr != '\0' && *endPtr != '\n') ||
            // No characters are entered
            endPtr == buffer)
        {
            continue;
        }
        break;
    }
    printf("\nEntered digit: %d (it might take sometime)\n", num);
    return num;
}
/**
 * @brief self-test implementation
 * @return void
 */
static void test()
{
    assert(fib(5) == 3);
    assert(fib(2) == 1);
    assert(fib(9) == 21);
}
/**
 * @brief Main function
 * @return 0 on exit
 */
int main()
{
    // Performing the test
    test();
    printf("Tests passed...\n");
    // Getting n
    printf(
        "Enter n to find nth fibonacci element...\n"
        "Note: You would be asked to enter input until valid number ( less "
        "than or equal to 48 ) is entered.\n");
    int number = getInput();
    clock_t start, end;
    start = clock();
    printf("Fibonacci element %d is %u ", number, fib(number));
    end = clock();
    printf("in %.3f seconds.\n", ((double)(end - start)) / CLOCKS_PER_SEC );
    return 0;
}

#include <stdio.h>
int main()
{
    int a[200], n, counter, temp, i;
    a[0] = 1;
    counter = 0;
    printf("Enter a whole number to Find its Factorial: ");
    scanf("%d", &n);
    if (n < 0)
        printf("Cannot Calculate factorials for negative numbers.");
    else
    {
        for (; n >= 2; n--)
        {
            temp = 0;
            for (i = 0; i <= counter; i++)
            {
                temp = (a[i] * n) + temp;
                a[i] = temp % 10;
                temp = temp / 10;
            }
            while (temp > 0)
            {
                a[++counter] = temp % 10;
                temp = temp / 10;
            }
        }
        for (i = counter; i >= 0; i--) printf("%d", a[i]);
    }
    return 0;
}

/**
 * @file
 * @brief Program to identify if a number is [palindrome
 * number](https://en.wikipedia.org/wiki/Palindrome) or not.
 * @see project_euler/problem_4/sol1.c
 */
#include <assert.h>
#include <stdbool.h>
#include <stdio.h>
bool isPalindrome(int number);
/** Driver Code */
int main()
{
    assert(isPalindrome(0));
    assert(isPalindrome(1));
    assert(isPalindrome(12321));
    assert(!isPalindrome(1234));
    return 0;
}
/**
 * Check given number whether is palindrome number or not
 * @param number number to check
 * @return `true` if given number is palindrome number
 * @return `false` if number is not a palindrome number
 */
bool isPalindrome(int number)
{
    int reversedNumber = 0;
    int originalNumber = number;
    while (number != 0)
    {
        int remainder = number % 10;
        reversedNumber = reversedNumber * 10 + remainder;
        number /= 10;
    }
    return originalNumber == reversedNumber;
}

/**
* @file
* @brief Functions related to 3D quaternions and Euler angles.
* @author Krishna Vedala
*/
#include <stdio.h>
#ifdef __arm__  // if compiling for ARM-Cortex processors
#define LIBQUAT_ARM
#include <arm_math.h>
#else
#include <math.h>
#endif
#include <assert.h>
#include "geometry_datatypes.h"
/**
* @addtogroup quats 3D Quaternion operations
* @{
*/
/**
* Function to convert given Euler angles to a quaternion.
* \f{eqnarray*}{
* q_{0} & =
* &\cos\left(\frac{\phi}{2}\right)\cos\left(\frac{\theta}{2}\right)\cos\left(\frac{\psi}{2}\right)
* +
* \sin\left(\frac{\phi}{2}\right)\sin\left(\frac{\theta}{2}\right)\sin\left(\frac{\psi}{2}\right)\\
* q_{1} & =
* &\sin\left(\frac{\phi}{2}\right)\cos\left(\frac{\theta}{2}\right)\cos\left(\frac{\psi}{2}\right)
* -
* \cos\left(\frac{\phi}{2}\right)\sin\left(\frac{\theta}{2}\right)\sin\left(\frac{\psi}{2}\right)\\
* q_{2} & =
* &\cos\left(\frac{\phi}{2}\right)\sin\left(\frac{\theta}{2}\right)\cos\left(\frac{\psi}{2}\right)
* +
* \sin\left(\frac{\phi}{2}\right)\cos\left(\frac{\theta}{2}\right)\sin\left(\frac{\psi}{2}\right)\\
* q_{3} & =
* &\cos\left(\frac{\phi}{2}\right)\cos\left(\frac{\theta}{2}\right)\sin\left(\frac{\psi}{2}\right)
* -
* \sin\left(\frac{\phi}{2}\right)\sin\left(\frac{\theta}{2}\right)\cos\left(\frac{\psi}{2}\right)\\
* \f}
*
* @param [in] in_euler input Euler angles instance
* @returns converted quaternion
*/
quaternion quat_from_euler(const euler *in_euler)
{
quaternion out_quat;
if (!in_euler)  // if null
{
fprintf(stderr, "%s: Invalid input.", __func__);
return out_quat;
}
quaternion temp;
float cy = cosf(in_euler->yaw * 0.5f);
float sy = sinf(in_euler->yaw * 0.5f);
float cp = cosf(in_euler->pitch * 0.5f);
float sp = sinf(in_euler->pitch * 0.5f);
float cr = cosf(in_euler->roll * 0.5f);
float sr = sinf(in_euler->roll * 0.5f);
temp.w = cr * cp * cy + sr * sp * sy;
temp.q1 = sr * cp * cy - cr * sp * sy;
temp.q2 = cr * sp * cy + sr * cp * sy;
temp.q3 = cr * cp * sy - sr * sp * cy;
return temp;
}
/**
* Function to convert given quaternion to Euler angles.
* \f{eqnarray*}{
* \phi & = &
* \tan^{-1}\left[\frac{2\left(q_0q_1+q_2q_3\right)}{1-2\left(q_1^2+q_2^2\right)}\right]\\
* \theta & =
* &-\sin^{-1}\left[2\left(q_0q_2-q_3q_1\right)\right]\\
* \psi & = &
* \tan^{-1}\left[\frac{2\left(q_0q_3+q_1q_2\right)}{1-2\left(q_2^2+q_3^2\right)}\right]\\
* \f}
*
* @param [in] in_quat input quaternion instance
* @returns converted euler angles
*/
euler euler_from_quat(const quaternion *in_quat)
{
euler out_euler;
if (!in_quat)  // if null
{
fprintf(stderr, "%s: Invalid input.", __func__);
return out_euler;
}
out_euler.roll = atan2f(
2.f * (in_quat->w * in_quat->q1 + in_quat->q2 * in_quat->q3),
1.f - 2.f * (in_quat->q1 * in_quat->q1 + in_quat->q2 * in_quat->q2));
out_euler.pitch =
asinf(2.f * (in_quat->w * in_quat->q2 + in_quat->q1 * in_quat->q3));
out_euler.yaw = atan2f(
2.f * (in_quat->w * in_quat->q3 + in_quat->q1 * in_quat->q2),
1.f - 2.f * (in_quat->q2 * in_quat->q2 + in_quat->q3 * in_quat->q3));
return out_euler;
}
/**
* Function to multiply two quaternions.
* \f{eqnarray*}{
* \mathbf{c} & = & \mathbf{a}\otimes\mathbf{b}\\
* & = & \begin{bmatrix}a_{0} & a_{1} & a_{2} &
*  a_{3}\end{bmatrix}\otimes\begin{bmatrix}b_{0} & b_{1} & b_{2} &
*  b_{3}\end{bmatrix}\\
* & = &
* \begin{bmatrix}
*  a_{0}b_{0}-a_{1}b_{1}-a_{2}b_{2}-a_{3}b_{3}\\
*  a_{0}b_{1}+a_{1}b_{0}+a_{2}b_{3}-a_{3}b_{2}\\
*  a_{0}b_{2}-a_{1}b_{3}+a_{2}b_{0}+a_{3}b_{1}\\
*  a_{0}b_{3}+a_{1}b_{2}-a_{2}b_{1}+a_{3}b_{0}
* \end{bmatrix}^{T}
* \f}
*
* @param [in] in_quat1 first input quaternion instance
* @param [in] in_quat2 second input quaternion instance
* @returns resultant quaternion
*/
quaternion quaternion_multiply(const quaternion *in_quat1,
const quaternion *in_quat2)
{
quaternion out_quat;
if (!in_quat1 || !in_quat2)  // if null
{
fprintf(stderr, "%s: Invalid input.", __func__);
return out_quat;
}
out_quat.w = in_quat1->w * in_quat2->w - in_quat1->q1 * in_quat2->q1 -
in_quat1->q2 * in_quat2->q2 - in_quat1->q3 * in_quat2->q3;
out_quat.q1 = in_quat1->w * in_quat2->q1 + in_quat1->q1 * in_quat2->w +
in_quat1->q2 * in_quat2->q3 - in_quat1->q3 * in_quat2->q2;
out_quat.q2 = in_quat1->w * in_quat2->q2 - in_quat1->q1 * in_quat2->q3 +
in_quat1->q2 * in_quat2->w + in_quat1->q3 * in_quat2->q1;
out_quat.q3 = in_quat1->w * in_quat2->q3 + in_quat1->q1 * in_quat2->q2 -
in_quat1->q2 * in_quat2->q1 + in_quat1->q3 * in_quat2->w;
return out_quat;
}
/** @} */
static void test()
{
quaternion quat = {0.7071f, 0.7071f, 0.f, 0.f};
euler eul = euler_from_quat(&quat);
printf("Euler: %.4g, %.4g, %.4g\n", eul.pitch, eul.roll, eul.yaw);
quaternion test_quat = quat_from_euler(&eul);
printf("Quaternion: %.4g %+.4g %+.4g %+.4g\n", test_quat.w,
test_quat.dual.x, test_quat.dual.y, test_quat.dual.z);
assert(fabsf(test_quat.w - quat.w) < .01);
assert(fabsf(test_quat.q1 - quat.q1) < .01);
assert(fabsf(test_quat.q2 - quat.q2) < .01);
assert(fabsf(test_quat.q3 - quat.q3) < .01);
}
int main()
{
test();
return 0;
}

# If necessary, use the RELATIVE flag, otherwise each source file may be listed
# with full pathname. The RELATIVE flag makes it easier to extract an executable's name
# automatically.
file( GLOB APP_SOURCES RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} *.c )
foreach( testsourcefile ${APP_SOURCES} )
    string( REPLACE ".c" "" testname ${testsourcefile} ) # File type. Example: `.c`
    add_executable( ${testname} ${testsourcefile} )
    if(OpenMP_C_FOUND)
        target_link_libraries(${testname} OpenMP::OpenMP_C)
    endif()
    if(MATH_LIBRARY)
        target_link_libraries(${testname} ${MATH_LIBRARY})
    endif()
    install(TARGETS ${testname} DESTINATION "bin/cipher") # Folder name. Do NOT include `<>`
endforeach( testsourcefile ${APP_SOURCES} )

/**
 * @file
 * @brief An [affine cipher](https://en.wikipedia.org/wiki/Affine_cipher) is a
 * letter substitution cipher that uses a linear transformation to substitute
 * letters in a message.
 * @details Given an alphabet of length M with characters with numeric values
 * 0-(M-1), an arbitrary character x can be transformed with the expression (ax
 * + b) % M into our ciphertext character. The only caveat is that a must be
 * relatively prime with M in order for this transformation to be invertible,
 * i.e., gcd(a, M) = 1.
 * @author [Daniel Murrow](https://github.com/dsmurrow)
 */
#include <assert.h>  /// for assertions
#include <stdio.h>   /// for IO
#include <stdlib.h>  /// for div function and div_t struct as well as malloc and free
#include <string.h>  /// for strlen, strcpy, and strcmp
/**
 * @brief number of characters in our alphabet (printable ASCII characters)
 */
#define ALPHABET_SIZE 95
/**
 * @brief used to convert a printable byte (32 to 126) to an element of the
 * group Z_95 (0 to 94)
 */
#define Z95_CONVERSION_CONSTANT 32
/**
 * @brief a structure representing an affine cipher key
 */
typedef struct
{
    int a;  ///< what the character is being multiplied by
    int b;  ///< what is being added after the multiplication with `a`
} affine_key_t;
/**
 * @brief finds the value x such that (a * x) % m = 1
 *
 * @param a number we are finding the inverse for
 * @param m the modulus the inversion is based on
 *
 * @returns the modular multiplicative inverse of `a` mod `m`
 */
int modular_multiplicative_inverse(unsigned int a, unsigned int m)
{
    int x[2] = {1, 0};
    div_t div_result;
    if (m == 0) {
        return 0;
    }
    a %= m;
    if (a == 0) {
        return 0;
    }
    div_result.rem = a;
    while (div_result.rem > 0)
    {
        div_result = div(m, a);
        m = a;
        a = div_result.rem;
        // Calculate value of x for this iteration
        int next = x[1] - (x[0] * div_result.quot);
        x[1] = x[0];
        x[0] = next;
    }
    return x[1];
}
/**
 * @brief Given a valid affine cipher key, this function will produce the
 * inverse key.
 *
 * @param key They key to be inverted
 *
 * @returns inverse of key
 */
affine_key_t inverse_key(affine_key_t key)
{
    affine_key_t inverse;
    inverse.a = modular_multiplicative_inverse(key.a, ALPHABET_SIZE);
    // Turn negative results positive
    inverse.a += ALPHABET_SIZE;
    inverse.a %= ALPHABET_SIZE;
    inverse.b = -(key.b % ALPHABET_SIZE) + ALPHABET_SIZE;
    return inverse;
}
/**
 * @brief Encrypts character string `s` with key
 *
 * @param s string to be encrypted
 * @param key affine key used for encryption
 *
 * @returns void
 */
void affine_encrypt(char *s, affine_key_t key)
{
    for (int i = 0; s[i] != '\0'; i++)
    {
        int c = (int)s[i] - Z95_CONVERSION_CONSTANT;
        c *= key.a;
        c += key.b;
        c %= ALPHABET_SIZE;
        s[i] = (char)(c + Z95_CONVERSION_CONSTANT);
    }
}
/**
 * @brief Decrypts an affine ciphertext
 *
 * @param s string to be decrypted
 * @param key Key used when s was encrypted
 *
 * @returns void
 */
void affine_decrypt(char *s, affine_key_t key)
{
    affine_key_t inverse = inverse_key(key);
    for (int i = 0; s[i] != '\0'; i++)
    {
        int c = (int)s[i] - Z95_CONVERSION_CONSTANT;
        c += inverse.b;
        c *= inverse.a;
        c %= ALPHABET_SIZE;
        s[i] = (char)(c + Z95_CONVERSION_CONSTANT);
    }
}
/**
 * @brief Tests a given string
 *
 * @param s string to be tested
 * @param a value of key.a
 * @param b value of key.b
 *
 * @returns void
 */
void test_string(const char *s, const char *ciphertext, int a, int b)
{
    char *copy = malloc((strlen(s) + 1) * sizeof(char));
    strcpy(copy, s);
    affine_key_t key = {a, b};
    affine_encrypt(copy, key);
    assert(strcmp(copy, ciphertext) == 0);  // assert that the encryption worked
    affine_decrypt(copy, key);
    assert(strcmp(copy, s) ==
           0);  // assert that we got the same string we started with
    free(copy);
}
/**
 * @brief Test multiple strings
 *
 * @returns void
 */
static void tests()
{
    test_string("Hello!", "&3ddy2", 7, 11);
    test_string("TheAlgorithms/C", "DNC}=jHS2zN!7;E", 67, 67);
    test_string("0123456789", "840,($ {ws", 91, 88);
    test_string("7W@;cdeRT9uL", "JDfa*we?z&bL", 77, 76);
    test_string("~Qr%^-+++$leM", "r'qC0$sss;Ahf", 8, 90);
    test_string("The quick brown fox jumps over the lazy dog",
                "K7: .*6<4 =-0(1 90' 5*2/, 0):- +7: 3>%& ;08", 94, 0);
    test_string(
        "One-1, Two-2, Three-3, Four-4, Five-5, Six-6, Seven-7, Eight-8, "
        "Nine-9, Ten-10",
        "H&60>\\2*uY0q\\2*p4660E\\2XYn40x\\2XDB60L\\2VDI0 "
        "\\2V6B6&0S\\2%D=p;0'\\2tD&60Z\\2*6&0>j",
        51, 18);
    printf("All tests have successfully passed!\n");
}
/**
 * @brief main function
 *
 * @returns 0 upon successful program exit
 */
int main()
{
    tests();
    return 0;
}

/**
 * @file
 * @brief [ROT13](https://en.wikipedia.org/wiki/ROT13) is a simple letter 
 * substitution cipher that replaces a letter with the 13th letter after it in 
 * the alphabet.
 * @details ROT13 transforms a piece of text by examining its alphabetic
 * characters and replacing each one with the letter 13 places further along in
 * the alphabet, wrapping back to the beginning if necessary. A becomes N, B
 * becomes O, and so on up to M, which becomes Z, then the sequence continues at
 * the beginning of the alphabet: N becomes A, O becomes B, and so on to Z,
 * which becomes M.
 * @author [Jeremias Moreira Gomes](https://github.com/j3r3mias)
 */
#include <stdio.h>     /// for IO operations
#include <string.h>    /// for string operations
#include <assert.h>    /// for assert
/**
 * @brief Apply the ROT13 cipher
 * @param s contains the string to be processed
 */
void rot13(char *s) {
    for (int i = 0; s[i]; i++) {
        if (s[i] >= 'A' && s[i] <= 'Z') {
            s[i] = 'A' + ((s[i] - 'A' + 13) % 26);
        } else if (s[i] >= 'a' && s[i] <= 'z') {
            s[i] = 'a' + ((s[i] - 'a' + 13) % 26);
        }
    } 
}
/**
 * @brief Self-test implementations
 * @returns void
 */
static void test() {
    char test_01[] = "The more I C, the less I see.";
    rot13(test_01);
    assert(strcmp(test_01, "Gur zber V P, gur yrff V frr.") == 0);
    char test_02[] = "Which witch switched the Swiss wristwatches?";
    rot13(test_02);
    assert(strcmp(test_02, "Juvpu jvgpu fjvgpurq gur Fjvff jevfgjngpurf?") == 0);
    char test_03[] = "Juvpu jvgpu fjvgpurq gur Fjvff jevfgjngpurf?";
    rot13(test_03);
    assert(strcmp(test_03, "Which witch switched the Swiss wristwatches?") == 0);
    printf("All tests have successfully passed!\n");
}
/**
 * @brief Main function
 * @returns 0 on exit
 */
int main() {
    test();  // run self-test implementations
    return 0;
}

/**
 * @file
 * @brief Functions related to 3D vector operations.
 * @author Krishna Vedala
 */
#include <stdio.h>
#ifdef __arm__  // if compiling for ARM-Cortex processors
#define LIBQUAT_ARM
#include <arm_math.h>
#else
#include <math.h>
#endif
#include <assert.h>
#include "geometry_datatypes.h"
/**
 * @addtogroup vec_3d 3D Vector operations
 * @{
 */
/**
 * Subtract one vector from another. @f[
 * \vec{c}=\vec{a}-\vec{b}=\left(a_x-b_x\right)\hat{i}+
 * \left(a_y-b_y\right)\hat{j}+\left(a_z-b_z\right)\hat{k}@f]
 * @param[in] a vector to subtract from
 * @param[in] b vector to subtract
 * @returns resultant vector
 */
vec_3d vector_sub(const vec_3d *a, const vec_3d *b)
{
    vec_3d out;
#ifdef LIBQUAT_ARM
    arm_sub_f32((float *)a, (float *)b, (float *)&out);
#else
    out.x = a->x - b->x;
    out.y = a->y - b->y;
    out.z = a->z - b->z;
#endif
    return out;
}
/**
 * Add one vector to another. @f[
 * \vec{c}=\vec{a}+\vec{b}=\left(a_x+b_x\right)\hat{i}+
 * \left(a_y+b_y\right)\hat{j}+\left(a_z+b_z\right)\hat{k}@f]
 * @param[in] a vector to add to
 * @param[in] b vector to add
 * @returns resultant vector
 */
vec_3d vector_add(const vec_3d *a, const vec_3d *b)
{
    vec_3d out;
#ifdef LIBQUAT_ARM
    arm_add_f32((float *)a, (float *)b, (float *)&out);
#else
    out.x = a->x + b->x;
    out.y = a->y + b->y;
    out.z = a->z + b->z;
#endif
    return out;
}
/**
 * Obtain the dot product of two 3D vectors.
 * @f[
 * \vec{a}\cdot\vec{b}=a_xb_x + a_yb_y + a_zb_z
 * @f]
 * @param[in] a first vector
 * @param[in] b second vector
 * @returns resulting dot product
 */
float dot_prod(const vec_3d *a, const vec_3d *b)
{
    float dot;
#ifdef LIBQUAT_ARM
    arm_dot_prod_f32((float *)a, (float *)b, &dot);
#else
    dot = a->x * b->x;
    dot += a->y * b->y;
    dot += a->z * b->z;
#endif
    return dot;
}
/**
 * Compute the vector product of two 3d vectors.
 * @f[\begin{align*}
 * \vec{a}\times\vec{b} &= \begin{vmatrix}
 *  \hat{i} & \hat{j} & \hat{k}\\
 *  a_x & a_y & a_z\\
 *  b_x & b_y & b_z
 *  \end{vmatrix}\\
 *  &= \left(a_yb_z-b_ya_z\right)\hat{i} - \left(a_xb_z-b_xa_z\right)\hat{j}
 * + \left(a_xb_y-b_xa_y\right)\hat{k} \end{align*}
 * @f]
 * @param[in] a first vector @f$\vec{a}@f$
 * @param[in] b second vector @f$\vec{b}@f$
 * @returns resultant vector @f$\vec{o}=\vec{a}\times\vec{b}@f$
 */
vec_3d vector_prod(const vec_3d *a, const vec_3d *b)
{
    vec_3d out;  // better this way to avoid copying results to input
                 // vectors themselves
    out.x = a->y * b->z - a->z * b->y;
    out.y = -a->x * b->z + a->z * b->x;
    out.z = a->x * b->y - a->y * b->x;
    return out;
}
/**
 * Print formatted vector on stdout.
 * @param[in] a vector to print
 * @param[in] name  name of the vector
 * @returns string representation of vector
 */
const char *print_vector(const vec_3d *a, const char *name)
{
    static char vec_str[100];  // static to ensure the string life extends the
                               // life of function
    snprintf(vec_str, 99, "vec(%s) = (%.3g)i + (%.3g)j + (%.3g)k\n", name, a->x,
             a->y, a->z);
    return vec_str;
}
/**
 * Compute the norm a vector.
 * @f[\lVert\vec{a}\rVert = \sqrt{\vec{a}\cdot\vec{a}} @f]
 * @param[in] a input vector
 * @returns norm of the given vector
 */
float vector_norm(const vec_3d *a)
{
    float n = dot_prod(a, a);
#ifdef LIBQUAT_ARM
    arm_sqrt_f32(*n, n);
#else
    n = sqrtf(n);
#endif
    return n;
}
/**
 * Obtain unit vector in the same direction as given vector.
 * @f[\hat{a}=\frac{\vec{a}}{\lVert\vec{a}\rVert}@f]
 * @param[in] a input vector
 * @returns n unit vector in the direction of @f$\vec{a}@f$
 */
vec_3d unit_vec(const vec_3d *a)
{
    vec_3d n = {0};
    float norm = vector_norm(a);
    if (fabsf(norm) < EPSILON)
    {  // detect possible divide by 0
        return n;
    }
    if (norm != 1.F)  // perform division only if needed
    {
        n.x = a->x / norm;
        n.y = a->y / norm;
        n.z = a->z / norm;
    }
    return n;
}
/**
 * The cross product of vectors can be represented as a matrix
 * multiplication operation. This function obtains the `3x3` matrix
 * of the cross-product operator from the first vector.
 * @f[\begin{align*}
 * \left(\vec{a}\times\right)\vec{b} &= \tilde{A}_a\vec{b}\\
 * \tilde{A}_a &=
 * \begin{bmatrix}0&-a_z&a_y\\a_z&0&-a_x\\-a_y&a_x&0\end{bmatrix}
 * \end{align*}@f]
 * @param[in] a input vector
 * @returns the `3x3` matrix for the cross product operator
 * @f$\left(\vec{a}\times\right)@f$
 */
mat_3x3 get_cross_matrix(const vec_3d *a)
{
    mat_3x3 A = {0., -a->z, a->y, a->z, 0., -a->x, -a->y, a->x, 0.};
    return A;
}
/**
 * Obtain the angle between two given vectors.
 * @f[\alpha=acos\left(\frac{\vec{a} \cdot \vec{b}}{\lVert\vec{a}\rVert \cdot \lVert\vec{b}\rVert}\right)@f]
 * @param[in] a first input vector
 * @param[in] b second input vector
 * @returns angle between @f$\vec{a}@f$ and @f$\vec{b}@f$ in radians
 */
double get_angle(const vec_3d *a, const vec_3d *b)
{
    double alpha, cos_alpha;
    float norm_a = vector_norm(a); ///< The norm of vector a
    float norm_b = vector_norm(b); ///< The norm of vector b
    if (fabsf(norm_a) < EPSILON || fabsf(norm_b) < EPSILON) /// detect possible division by 0 - the angle is not defined in this case
    {  
        return NAN; 
    }
    cos_alpha = dot_prod(a, b) / (norm_a * norm_b);
    alpha = acos(cos_alpha); // delivers the radian
    return alpha; // in range from -1 to 1
}
/** @} */
/**
 * @brief Testing function
 * @returns `void`
 */
static void test()
{
    vec_3d a = {1., 2., 3.};
    vec_3d b = {1., 1., 1.};
    float d;
    // printf("%s", print_vector(&a, "a"));
    // printf("%s", print_vector(&b, "b"));
    d = vector_norm(&a);
    // printf("|a| = %.4g\n", d);
    assert(fabsf(d - 3.742f) < 0.01);
    d = vector_norm(&b);
    // printf("|b| = %.4g\n", d);
    assert(fabsf(d - 1.732f) < 0.01);
    d = dot_prod(&a, &b);
    // printf("Dot product: %f\n", d);
    assert(fabsf(d - 6.f) < 0.01);
    vec_3d c = vector_prod(&a, &b);
    // printf("Vector product ");
    // printf("%s", print_vector(&c, "c"));
    assert(fabsf(c.x - (-1.f)) < 0.01);
    assert(fabsf(c.y - (2.f)) < 0.01);
    assert(fabsf(c.z - (-1.f)) < 0.01);
    double alpha = get_angle(&a, &b);
    // printf("The angle is %f\n", alpha);
    assert(fabsf(alpha - 0.387597) < 0.01);
}
/**
 * @brief Main function
 *
 * @return 0 on exit
 */
int main(void)
{
    test();
    return 0;
}