/* VectorNav HSI Calibration Library v1.0.0.2
 *
 * Copyright (c) 2018 VectorNav Technologies, LLC
 *
 * This source code is proprietary to and copyrighted by VectorNav Technologies, LLC and is
 * available solely under license from VectorNav. All rights reserved. If you do not have a
 * license to use this source code from VectorNav, any use hereof is strictly prohibited by
 * U.S. law and other applicable law. This source code is restricted for use solely with the
 * products of VectorNav and as otherwise set forth in any agreement with VectorNav. Any
 * disclosure of this source code to the public or to any third party is also prohibited.
 */
#if defined(_MSC_VER)
  #pragma warning(push)
  #pragma warning(disable:4255)
#endif
#include <stdio.h>
#if defined(_MSC_VER)
  #pragma warning(pop)
#endif
#include "vnvector.h"
#include "vnenum.h"
#include "vnstring.h"

union vn_vec3f
create_v3f(
    float x,
    float y,
    float z)
{
  union vn_vec3f v;

  init_v3f(&v, x, y, z);

  return v;
}

union vn_vec3f
zero_v3f(void)
{
  return create_v3f(0.f, 0.f, 0.f);
}

void
init_v3f(
    union vn_vec3f *v,
    float x,
    float y,
    float z)
{
  v->c[0] = x;
  v->c[1] = y;
  v->c[2] = z;
}

void
init_v3f_s(
    union vn_vec3f *v,
    float s)
{
  init_v3f(v, s, s, s);
}

void
init_v3f_fa(
    union vn_vec3f *v,
    const float *fa)
{
  init_v3f(v, fa[0], fa[1], fa[2]);
}

union vn_vec3f
add_v3f_v3f(
    union vn_vec3f lhs,
    union vn_vec3f rhs)
{
  return create_v3f(
    lhs.c[0] + rhs.c[0],
    lhs.c[1] + rhs.c[1],
    lhs.c[2] + rhs.c[2]);
}

union vn_vec3d
create_v3d(
    double x,
    double y,
    double z)
{
  union vn_vec3d v;

  v.c[0] = x;
  v.c[1] = y;
  v.c[2] = z;

  return v;
}

union vn_vec3d
zero_v3d(void)
{
  return create_v3d(0., 0., 0.);
}

union vn_vec4f
create_v4f(
    float x,
    float y,
    float z,
    float w)
{
  union vn_vec4f v;

  v.c[0] = x;
  v.c[1] = y;
  v.c[2] = z;
  v.c[3] = w;

  return v;
}

union vn_vec4f
zero_v4f(void)
{
  return create_v4f(0.f, 0.f, 0.f, 0.f);
}

enum VnError
to_string_vec3f(
    char* out,
    size_t outSize,
    union vn_vec3f v)
{
  return sprintf_x(out, outSize, "(%f; %f; %f)", v.c[0], v.c[1], v.c[2]);
}

enum VnError
to_string_vec3d(
    char* out,
    size_t outSize,
    union vn_vec3d v)
{
  return sprintf_x(out, outSize, "(%f; %f; %f)", v.c[0], v.c[1], v.c[2]);
}

enum VnError
to_string_vec4f(
    char* out,
    size_t outSize,
    union vn_vec4f v)
{
  return sprintf_x(out, outSize, "(%f; %f; %f; %f)", v.c[0], v.c[1], v.c[2], v.c[3]);
}