#ifndef VECTOROPERATIONS_
#define VECTOROPERATIONS_

#include <stdint.h>

#include <cmath>

template <typename T>
class VectorOperations {
 public:
  virtual ~VectorOperations() {}

  static void cross(const T left[], const T right[], T out[]) {
    T temp[3] = {0, 0, 0};
    temp[0] = left[1] * right[2] - left[2] * right[1];
    temp[1] = left[2] * right[0] - left[0] * right[2];
    temp[2] = left[0] * right[1] - left[1] * right[0];
    out[0] = temp[0];
    out[1] = temp[1];
    out[2] = temp[2];
  }

  static T dot(const T a[], const T b[]) { return a[0] * b[0] + a[1] * b[1] + a[2] * b[2]; }

  static void mulScalar(const T vector[], T scalar, T out[], uint8_t size) {
    for (; size > 0; size--) {
      out[size - 1] = vector[size - 1] * scalar;
    }
  }

  static void add(const T vector1[], const T vector2[], T sum[], uint8_t size = 3) {
    for (; size > 0; size--) {
      sum[size - 1] = vector1[size - 1] + vector2[size - 1];
    }
  }

  static void subtract(const T vector1[], const T vector2[], T sum[], uint8_t size = 3) {
    for (; size > 0; size--) {
      sum[size - 1] = vector1[size - 1] - vector2[size - 1];
    }
  }

  static T norm(const T *vector, uint8_t size) {
    T result = 0;
    for (; size > 0; size--) {
      result += vector[size - 1] * vector[size - 1];
    }
    result = sqrt(result);
    return result;
  }

  static void normalize(const T *vector, T *normalizedVector, uint8_t size) {
    mulScalar(vector, 1 / norm(vector, size), normalizedVector, size);
  }

  static T maxAbsValue(const T *vector, uint8_t size, uint8_t *index = nullptr) {
    T max = vector[size - 1];
    uint8_t foundIndex = size - 1;

    for (; size > 0; size--) {
      T abs = vector[size - 1];
      if (abs < 0) {
        abs = -abs;
      }
      if (abs > max) {
        max = abs;
        if (index != 0) {
          foundIndex = size - 1;
        }
      }
    }

    if (index != nullptr) {
      *index = foundIndex;
    }

    return max;
  }

  static T maxValue(const T *vector, uint8_t size, uint8_t *index = nullptr) {
    T max = vector[size - 1];
    uint8_t foundIndex = size - 1;

    for (; size > 0; size--) {
      if (vector[size - 1] > max) {
        max = vector[size - 1];
        if (index != 0) {
          foundIndex = size - 1;
        }
      }
    }

    if (index != nullptr) {
      *index = foundIndex;
    }

    return max;
  }

  static void copy(const T *in, T *out, uint8_t size) { mulScalar(in, 1, out, size); }

  static bool isFinite(const T *inputVector, uint8_t size) {
    for (uint8_t i = 0; i < size; i++) {
      if (not std::isfinite(inputVector[i])) {
        return false;
      }
    }
    return true;
  }

 private:
  VectorOperations();
};

#endif /* VECTOROPERATIONS_ */