/**
 * test_divround.cpp
 * Tests the following functions for all valid inputs:
 * divround_i8, divround<int8_t>, divround_u8, divround<uint8_t>,
 * divround_i16, divround<int16_t>, divround_u16, and divround<uint16_t>.
 *
 * Tests divround_i32 and divround<int32_t> for all valid combinations
 * of dividend and divisor on the ranges
 * [-2147483648, -2147418112],
 * [-65536, 65536], and
 * [2147418111, 2147483647].
 * Approximately 2^36 tests are conducted on each function.
 *
 * Tests divround_u32 and divround<uint32_t> for all valid combinations
 * of dividend and divisor on the ranges
 * [1, 131072] and
 * [4294836223, 4294967295].
 * Approximately 2^36 tests are conducted on each function.
 *
 * Tests divround_i64 and divround<int64_t> for all valid combinations
 * of dividend and divisor on the ranges
 * [-9223372036854775808, -9223372036854710272],
 * [-65536, 65536], and
 * [9223372036854710271, 9223372036854775807].
 * Approximately 2^36 tests are conducted on each function.
 *
 * Tests divround_u64 and divround<uint64_t> for all valid combinations
 * of dividend and divisor on the ranges
 * [1, 131072] and
 * [18446744073709420543, 18446744073709551615].
 * Approximately 2^36 tests are conducted on each function.
 *
 * Written in 2018 by Ben Tesch.
 * Originally distributed at https://github.com/slugrustle/numerical_routines
 *
 * To the extent possible under law, the author has dedicated all copyright
 * and related and neighboring rights to this software to the public domain
 * worldwide.This software is distributed without any warranty.
 * The text of the CC0 Public Domain Dedication should be reproduced at the
 * end of this file. If not, see http ://creativecommons.org/publicdomain/zero/1.0/
 */

#include <cinttypes>
#include <cmath>
#include <cstdio>
#include <limits>
#include <thread>
#include <mutex>
#include <vector>
#ifdef __cplusplus
  extern "C"
  {
#endif
    #include "divround.h"
#ifdef __cplusplus
  }
#endif
#include "divround.hpp"
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/math/special_functions/round.hpp>

typedef boost::multiprecision::number<boost::multiprecision::backends::cpp_bin_float<80, boost::multiprecision::backends::digit_base_2, void, boost::int16_t, -16382, 16383>, boost::multiprecision::et_off> cpp_bin_float_80;

/**
 * Mutex for stdout when running multithreaded.
 */
std::mutex print_mutex;

/**
 * Test c style int16_t divround for dividend on
 * [dividend_start, dividend_end]
 * and divisor on [-32768, 32767]
 * for each dividend value.
 */
void test_divround_i16_c(int16_t dividend_start, int16_t dividend_end) {
  /**
   * The dividend loop is written this way in case
   * dividend_end is std::numeric_limits<int16_t>::max().
   */
  int16_t dividend_i16 = dividend_start;
  while (true) {
    int16_t divisor = std::numeric_limits<int16_t>::lowest();
    while (true) {
      /* This would cause overflow in the result. Skip it. */
      if ((dividend_i16 == std::numeric_limits<int16_t>::lowest()) & (divisor == static_cast<int16_t>(-1))) divisor = static_cast<int16_t>(1);
      /* Don't divide by 0. */
      if (divisor == static_cast<int16_t>(0)) divisor = static_cast<int16_t>(1);
      int16_t dr = divround_i16(dividend_i16, divisor);
      int16_t dbl_dr = static_cast<int16_t>(std::round(static_cast<double>(dividend_i16) / static_cast<double>(divisor)));
      if (dr != dbl_dr) {
        int16_t quotient = dividend_i16 / divisor;
        int16_t remainder = dividend_i16 - (quotient * divisor);
        int16_t div_half = divisor >> 1;
        if ((divisor & static_cast<uint16_t>(0x8001)) == static_cast<int16_t>(1)) div_half++;
        std::lock_guard<std::mutex> print_lock(print_mutex);
        std::printf("\nERROR: ROUND(%i / %i) = %i, but divround_i16 returns %i\n  remainder = %i, div_half = %i\n\n", dividend_i16, divisor, dbl_dr, dr, remainder, div_half);
      }
      if (divisor == std::numeric_limits<int16_t>::max()) break;
      divisor++;
    }
    if (dividend_i16 == dividend_end) break;
    dividend_i16++;
  }
}

/**
 * Test c++ style int16_t divround for dividend on
 * [dividend_start, dividend_end]
 * and divisor on [-32768, 32767]
 * for each dividend value.
 */
void test_divround_i16_cpp(int16_t dividend_start, int16_t dividend_end) {
  /**
   * The dividend loop is written this way in case
   * dividend_end is std::numeric_limits<int16_t>::max().
   */
  int16_t dividend_i16 = dividend_start;
  while (true) {
    int16_t divisor = std::numeric_limits<int16_t>::lowest();
    while (true) {
      /* This would cause overflow in the result. Skip it. */
      if ((dividend_i16 == std::numeric_limits<int16_t>::lowest()) & (divisor == static_cast<int16_t>(-1))) divisor = static_cast<int16_t>(1);
      /* Don't divide by 0. */
      if (divisor == static_cast<int16_t>(0)) divisor = static_cast<int16_t>(1);
      int16_t dr = divround<int16_t>(dividend_i16, divisor);
      int16_t dbl_dr = static_cast<int16_t>(std::round(static_cast<double>(dividend_i16) / static_cast<double>(divisor)));
      if (dr != dbl_dr) {
        int16_t quotient = dividend_i16 / divisor;
        int16_t remainder = dividend_i16 - (quotient * divisor);
        int16_t div_half = divisor >> 1;
        if ((divisor & static_cast<uint16_t>(0x8001)) == static_cast<int16_t>(1)) div_half++;
        std::lock_guard<std::mutex> print_lock(print_mutex);
        std::printf("\nERROR: ROUND(%i / %i) = %i, but divround<int16_t> returns %i\n  remainder = %i, div_half = %i\n\n", dividend_i16, divisor, dbl_dr, dr, remainder, div_half);
      }
      if (divisor == std::numeric_limits<int16_t>::max()) break;
      divisor++;
    }
    if (dividend_i16 == dividend_end) break;
    dividend_i16++;
  }
}

/**
 * Test c style uint16_t divround for dividend on
 * [dividend_start, dividend_end]
 * and divisor on [1, 65535]
 * for each dividend value.
 */
void test_divround_u16_c(uint16_t dividend_start, uint16_t dividend_end) {
  /**
   * The dividend loop is written this way in case
   * dividend_end is std::numeric_limits<uint16_t>::max().
   */
  uint16_t dividend_u16 = dividend_start;
  while (true) {
    uint16_t divisor = static_cast<uint16_t>(1);
    while (true) {
      uint16_t dr = divround_u16(dividend_u16, divisor);
      uint16_t dbl_dr = static_cast<uint16_t>(std::round(static_cast<double>(dividend_u16) / static_cast<double>(divisor)));
      if (dr != dbl_dr) {
        uint16_t quotient = dividend_u16 / divisor;
        uint16_t remainder = dividend_u16 - (quotient * divisor);
        uint16_t div_half = divisor >> 1;
        if (divisor & static_cast<uint16_t>(0x0001)) div_half++;
        std::lock_guard<std::mutex> print_lock(print_mutex);
        std::printf("\nERROR: ROUND(%u / %u) = %u, but divround_u16 returns %u\n  remainder = %u, div_half = %u\n\n", dividend_u16, divisor, dbl_dr, dr, remainder, div_half);
      }
      if (divisor == std::numeric_limits<uint16_t>::max()) break;
      divisor++;
    }
    if (dividend_u16 == dividend_end) break;
    dividend_u16++;
  }
}

/**
 * Test c++ style uint16_t divround for dividend on
 * [dividend_start, dividend_end]
 * and divisor on [1, 65535]
 * for each dividend value.
 */
void test_divround_u16_cpp(uint16_t dividend_start, uint16_t dividend_end) {
  /**
   * The dividend loop is written this way in case
   * dividend_end is std::numeric_limits<uint16_t>::max().
   */
  uint16_t dividend_u16 = dividend_start;
  while (true) {
    uint16_t divisor = static_cast<uint16_t>(1);
    while (true) {
      uint16_t dr = divround<uint16_t>(dividend_u16, divisor);
      uint16_t dbl_dr = static_cast<uint16_t>(std::round(static_cast<double>(dividend_u16) / static_cast<double>(divisor)));
      if (dr != dbl_dr) {
        uint16_t quotient = dividend_u16 / divisor;
        uint16_t remainder = dividend_u16 - (quotient * divisor);
        uint16_t div_half = divisor >> 1;
        if (divisor & static_cast<uint16_t>(0x0001)) div_half++;
        std::lock_guard<std::mutex> print_lock(print_mutex);
        std::printf("\nERROR: ROUND(%u / %u) = %u, but divround<uint16_t> returns %u\n  remainder = %u, div_half = %u\n\n", dividend_u16, divisor, dbl_dr, dr, remainder, div_half);
      }
      if (divisor == std::numeric_limits<uint16_t>::max()) break;
      divisor++;
    }
    if (dividend_u16 == dividend_end) break;
    dividend_u16++;
  }
}

/**
 * Test c style int32_t divround for dividend on
 * [dividend_start, dividend_end]
 * and divisor on
 * [-2147483648, -2147418112],
 * [-65536, 65536], and
 * [2147418111, 2147483647]
 * for each dividend value.
 */
void test_divround_i32_c(int32_t dividend_start, int32_t dividend_end) {
  /**
   * The dividend loop is written this way in case
   * dividend_end is std::numeric_limits<int32_t>::max().
   */
  int32_t dividend_i32 = dividend_start;
  while (true) {
    int32_t divisor = std::numeric_limits<int32_t>::lowest();
    while (true) {
      /* This would cause overflow in the result. Skip it. */
      if ((dividend_i32 == std::numeric_limits<int32_t>::lowest()) & (divisor == -1)) divisor = 1;
      /* Don't divide by 0. */
      if (divisor == 0) divisor = 1;
      int32_t dr = divround_i32(dividend_i32, divisor);
      int32_t dbl_dr = static_cast<int32_t>(std::round(static_cast<double>(dividend_i32) / static_cast<double>(divisor)));
      if (dr != dbl_dr) {
        int32_t quotient = dividend_i32 / divisor;
        int32_t remainder = dividend_i32 - (quotient * divisor);
        int32_t div_half = divisor >> 1;
        if ((divisor & 0x80000001u) == 1) div_half++;
        std::lock_guard<std::mutex> print_lock(print_mutex);
        std::printf("\nERROR: ROUND(%i / %i) = %i, but divround_i32 returns %i\n  remainder = %i, div_half = %i\n\n", dividend_i32, divisor, dbl_dr, dr, remainder, div_half);
      }
      if (divisor == std::numeric_limits<int32_t>::lowest() + (1 << 16)) divisor = -(1 << 16);
      else if (divisor == (1 << 16)) divisor = std::numeric_limits<int32_t>::max() - (1 << 16);
      else if (divisor == std::numeric_limits<int32_t>::max()) break;
      else divisor++;
    }
    if (dividend_i32 == dividend_end) break;
    else dividend_i32++;
  }
}

/**
 * Test c++ style int32_t divround for dividend on
 * [dividend_start, dividend_end]
 * and divisor on
 * [-2147483648, -2147418112],
 * [-65536, 65536], and
 * [2147418111, 2147483647]
 * for each dividend value.
 */
void test_divround_i32_cpp(int32_t dividend_start, int32_t dividend_end) {
  /**
   * The dividend loop is written this way in case
   * dividend_end is std::numeric_limits<int32_t>::max().
   */
  int32_t dividend_i32 = dividend_start;
  while (true) {
    int32_t divisor = std::numeric_limits<int32_t>::lowest();
    while (true) {
      /* This would cause overflow in the result. Skip it. */
      if ((dividend_i32 == std::numeric_limits<int32_t>::lowest()) & (divisor == -1)) divisor = 1;
      /* Don't divide by 0. */
      if (divisor == 0) divisor = 1;
      int32_t dr = divround<int32_t>(dividend_i32, divisor);
      int32_t dbl_dr = static_cast<int32_t>(std::round(static_cast<double>(dividend_i32) / static_cast<double>(divisor)));
      if (dr != dbl_dr) {
        int32_t quotient = dividend_i32 / divisor;
        int32_t remainder = dividend_i32 - (quotient * divisor);
        int32_t div_half = divisor >> 1;
        if ((divisor & 0x80000001u) == 1) div_half++;
        std::lock_guard<std::mutex> print_lock(print_mutex);
        std::printf("\nERROR: ROUND(%i / %i) = %i, but divround<int32_t> returns %i\n  remainder = %i, div_half = %i\n\n", dividend_i32, divisor, dbl_dr, dr, remainder, div_half);
      }
      if (divisor == std::numeric_limits<int32_t>::lowest() + (1 << 16)) divisor = -(1 << 16);
      else if (divisor == (1 << 16)) divisor = std::numeric_limits<int32_t>::max() - (1 << 16);
      else if (divisor == std::numeric_limits<int32_t>::max()) break;
      else divisor++;
    }
    if (dividend_i32 == dividend_end) break;
    else dividend_i32++;
  }
}

/**
 * Test c style uint32_t divround for dividend on
 * [dividend_start, dividend_end]
 * and divisor on
 * [1, 131072] and
 * [4294836223, 4294967295]
 * for each dividend value.
 */
void test_divround_u32_c(uint32_t dividend_start, uint32_t dividend_end) {
  /**
   * The dividend loop is written this way in case
   * dividend_end is std::numeric_limits<uint32_t>::max().
   */
  uint32_t dividend_u32 = dividend_start;
  while (true) {
    uint32_t divisor = 1u;
    while (true) {
      uint32_t dr = divround_u32(dividend_u32, divisor);
      uint32_t dbl_dr = static_cast<uint32_t>(std::round(static_cast<double>(dividend_u32) / static_cast<double>(divisor)));
      if (dr != dbl_dr) {
        uint32_t quotient = dividend_u32 / divisor;
        uint32_t remainder = dividend_u32 - (quotient * divisor);
        uint32_t div_half = divisor >> 1;
        if (divisor & 0x00000001u) div_half++;
        std::lock_guard<std::mutex> print_lock(print_mutex);
        std::printf("\nERROR: ROUND(%u / %u) = %u, but divround_u32 returns %u\n  remainder = %u, div_half = %u\n\n", dividend_u32, divisor, dbl_dr, dr, remainder, div_half);
      }
      if (divisor == (1u << 17)) divisor = std::numeric_limits<uint32_t>::max() - (1u << 17);
      else if (divisor == std::numeric_limits<uint32_t>::max()) break;
      else divisor++;
    }
    if (dividend_u32 == dividend_end) break;
    else dividend_u32++;
  }
}

/**
 * Test c++ style uint32_t divround for dividend on
 * [dividend_start, dividend_end]
 * and divisor on
 * [1, 131072] and
 * [4294836223, 4294967295]
 * for each dividend value.
 */
void test_divround_u32_cpp(uint32_t dividend_start, uint32_t dividend_end) {
  /**
   * The dividend loop is written this way in case
   * dividend_end is std::numeric_limits<uint32_t>::max().
   */
  uint32_t dividend_u32 = dividend_start;
  while (true) {
    uint32_t divisor = 1u;
    while (true) {
      uint32_t dr = divround<uint32_t>(dividend_u32, divisor);
      uint32_t dbl_dr = static_cast<uint32_t>(std::round(static_cast<double>(dividend_u32) / static_cast<double>(divisor)));
      if (dr != dbl_dr) {
        uint32_t quotient = dividend_u32 / divisor;
        uint32_t remainder = dividend_u32 - (quotient * divisor);
        uint32_t div_half = divisor >> 1;
        if (divisor & 0x00000001u) div_half++;
        std::lock_guard<std::mutex> print_lock(print_mutex);
        std::printf("\nERROR: ROUND(%u / %u) = %u, but divround<uint32_t> returns %u\n  remainder = %u, div_half = %u\n\n", dividend_u32, divisor, dbl_dr, dr, remainder, div_half);
      }
      if (divisor == (1u << 17)) divisor = std::numeric_limits<uint32_t>::max() - (1u << 17);
      else if (divisor == std::numeric_limits<uint32_t>::max()) break;
      else divisor++;
    }
    if (dividend_u32 == dividend_end) break;
    else dividend_u32++;
  }
}

/**
 * Test c style int64_t divround for dividend on
 * [dividend_start, dividend_end]
 * and divisor on
 * [-9223372036854775808, -9223372036854710272],
 * [-65536, 65536], and
 * [9223372036854710271, 9223372036854775807]
 * for each dividend value.
 */
void test_divround_i64_c(int64_t dividend_start, int64_t dividend_end) {
  /**
   * The dividend loop is written this way in case
   * dividend_end is std::numeric_limits<int64_t>::max().
   */
  int64_t dividend_i64 = dividend_start;
  while (true) {
    int64_t divisor = std::numeric_limits<int64_t>::lowest();
    while (true) {
      /* This would cause overflow in the result. Skip it. */
      if ((dividend_i64 == std::numeric_limits<int64_t>::lowest()) & (divisor == -1ll)) divisor = 1ll;
      /* Don't divide by 0. */
      if (divisor == 0ll) divisor = 1ll;
      int64_t dr = divround_i64(dividend_i64, divisor);
      int64_t dbl_dr = boost::math::round(cpp_bin_float_80(dividend_i64) / cpp_bin_float_80(divisor)).convert_to<int64_t>();
      if (dr != dbl_dr) {
        int64_t quotient = dividend_i64 / divisor;
        int64_t remainder = dividend_i64 - (quotient * divisor);
        int64_t div_half = divisor >> 1;
        if ((divisor & 0x8000000000000001ull) == 1ll) div_half++;
        std::lock_guard<std::mutex> print_lock(print_mutex);
        std::printf("\nERROR: ROUND(%" PRIi64 " / %" PRIi64 ") = %" PRIi64 ", but divround_i64 returns %" PRIi64 "\n  remainder = %" PRIi64 ", div_half = %" PRIi64 "\n\n", dividend_i64, divisor, dbl_dr, dr, remainder, div_half);
      }
      if (divisor == std::numeric_limits<int64_t>::lowest() + (1 << 16)) divisor = -(1 << 16);
      else if (divisor == (1 << 16)) divisor = std::numeric_limits<int64_t>::max() - (1 << 16);
      else if (divisor == std::numeric_limits<int64_t>::max()) break;
      else divisor++;
    }
    if (dividend_i64 == dividend_end) break;
    else dividend_i64++;
  }
}

/**
 * Test c++ style int64_t divround for dividend on
 * [dividend_start, dividend_end]
 * and divisor on
 * [-9223372036854775808, -9223372036854710272],
 * [-65536, 65536], and
 * [9223372036854710271, 9223372036854775807]
 * for each dividend value.
 */
void test_divround_i64_cpp(int64_t dividend_start, int64_t dividend_end) {
  /**
   * The dividend loop is written this way in case
   * dividend_end is std::numeric_limits<int64_t>::max().
   */
  int64_t dividend_i64 = dividend_start;
  while (true) {
    int64_t divisor = std::numeric_limits<int64_t>::lowest();
    while (true) {
      /* This would cause overflow in the result. Skip it. */
      if ((dividend_i64 == std::numeric_limits<int64_t>::lowest()) & (divisor == -1ll)) divisor = 1ll;
      /* Don't divide by 0. */
      if (divisor == 0ll) divisor = 1ll;
      int64_t dr = divround<int64_t>(dividend_i64, divisor);
      int64_t dbl_dr = boost::math::round(cpp_bin_float_80(dividend_i64) / cpp_bin_float_80(divisor)).convert_to<int64_t>();
      if (dr != dbl_dr) {
        int64_t quotient = dividend_i64 / divisor;
        int64_t remainder = dividend_i64 - (quotient * divisor);
        int64_t div_half = divisor >> 1;
        if ((divisor & 0x8000000000000001ull) == 1ll) div_half++;
        std::lock_guard<std::mutex> print_lock(print_mutex);
        std::printf("\nERROR: ROUND(%" PRIi64 " / %" PRIi64 ") = %" PRIi64 ", but divround<int64_t> returns %" PRIi64 "\n  remainder = %" PRIi64 ", div_half = %" PRIi64 "\n\n", dividend_i64, divisor, dbl_dr, dr, remainder, div_half);
      }
      if (divisor == std::numeric_limits<int64_t>::lowest() + (1 << 16)) divisor = -(1 << 16);
      else if (divisor == (1 << 16)) divisor = std::numeric_limits<int64_t>::max() - (1 << 16);
      else if (divisor == std::numeric_limits<int64_t>::max()) break;
      else divisor++;
    }
    if (dividend_i64 == dividend_end) break;
    else dividend_i64++;
  }
}

/**
 * Test c style uint64_t divround for dividend on
 * [dividend_start, dividend_end]
 * and divisor on
 * [1, 131072] and
 * [18446744073709420543, 18446744073709551615]
 * for each dividend value.
 */
void test_divround_u64_c(uint64_t dividend_start, uint64_t dividend_end) {
  /**
   * The dividend loop is written this way in case
   * dividend_end is std::numeric_limits<uint64_t>::max().
   */
  uint64_t dividend_u64 = dividend_start;
  while (true) {
    uint64_t divisor = 1ull;
    while (true) {
      uint64_t dr = divround_u64(dividend_u64, divisor);
      uint64_t dbl_dr = boost::math::round(cpp_bin_float_80(dividend_u64) / cpp_bin_float_80(divisor)).convert_to<uint64_t>();
      if (dr != dbl_dr) {
        uint64_t quotient = dividend_u64 / divisor;
        uint64_t remainder = dividend_u64 - (quotient * divisor);
        uint64_t div_half = divisor >> 1;
        if (divisor & 0x0000000000000001ull) div_half++;
        std::lock_guard<std::mutex> print_lock(print_mutex);
        std::printf("\nERROR: ROUND(%" PRIu64 " / %" PRIu64 ") = %" PRIu64 ", but divround_u64 returns %" PRIu64 "\n  remainder = %" PRIu64 ", div_half = %" PRIu64 "\n\n", dividend_u64, divisor, dbl_dr, dr, remainder, div_half);
      }
      if (divisor == (1ull << 17)) divisor = std::numeric_limits<uint64_t>::max() - (1ull << 17);
      else if (divisor == std::numeric_limits<uint64_t>::max()) break;
      else divisor++;
    }
    if (dividend_u64 == dividend_end) break;
    else dividend_u64++;
  }
}

/**
 * Test c++ style uint64_t divround for dividend on
 * [dividend_start, dividend_end]
 * and divisor on
 * [1, 131072] and
 * [18446744073709420543, 18446744073709551615]
 * for each dividend value.
 */
void test_divround_u64_cpp(uint64_t dividend_start, uint64_t dividend_end) {
  /**
   * The dividend loop is written this way in case
   * dividend_end is std::numeric_limits<uint64_t>::max().
   */
  uint64_t dividend_u64 = dividend_start;
  while (true) {
    uint64_t divisor = 1ull;
    while (true) {
      uint64_t dr = divround<uint64_t>(dividend_u64, divisor);
      uint64_t dbl_dr = boost::math::round(cpp_bin_float_80(dividend_u64) / cpp_bin_float_80(divisor)).convert_to<uint64_t>();
      if (dr != dbl_dr) {
        uint64_t quotient = dividend_u64 / divisor;
        uint64_t remainder = dividend_u64 - (quotient * divisor);
        uint64_t div_half = divisor >> 1;
        if (divisor & 0x0000000000000001ull) div_half++;
        std::lock_guard<std::mutex> print_lock(print_mutex);
        std::printf("\nERROR: ROUND(%" PRIu64 " / %" PRIu64 ") = %" PRIu64 ", but divround<uint64_t> returns %" PRIu64 "\n  remainder = %" PRIu64 ", div_half = %" PRIu64 "\n\n", dividend_u64, divisor, dbl_dr, dr, remainder, div_half);
      }
      if (divisor == (1ull << 17)) divisor = std::numeric_limits<uint64_t>::max() - (1ull << 17);
      else if (divisor == std::numeric_limits<uint64_t>::max()) break;
      else divisor++;
    }
    if (dividend_u64 == dividend_end) break;
    else dividend_u64++;
  }
}

int main() {
  std::printf("\nTesting divround_i8\n");
  int8_t dividend_i8 = std::numeric_limits<int8_t>::lowest();
  while (true) {
    int8_t divisor = std::numeric_limits<int8_t>::lowest();
    while (true) {
      /* This would cause overflow in the result. Skip it. */
      if ((dividend_i8 == std::numeric_limits<int8_t>::lowest()) & (divisor == static_cast<int8_t>(-1))) divisor = static_cast<int8_t>(1);
      /* Don't divide by 0. */
      if (divisor == static_cast<int8_t>(0)) divisor = static_cast<int8_t>(1);
      int8_t dr = divround_i8(dividend_i8, divisor);
      int8_t dbl_dr = static_cast<int8_t>(std::round(static_cast<double>(dividend_i8) / static_cast<double>(divisor)));
      if (dr != dbl_dr) {
        int8_t quotient = dividend_i8 / divisor;
        int8_t remainder = dividend_i8 - (quotient * divisor);
        int8_t div_half = divisor >> 1;
        if ((divisor & static_cast<uint8_t>(0x81)) == static_cast<int8_t>(0x01)) div_half++;
        std::printf("\nERROR: ROUND(%i / %i) = %i, but divround_i8 returns %i\n  remainder = %i, div_half = %i\n\n", dividend_i8, divisor, dbl_dr, dr, remainder, div_half);
      }
      if (divisor == std::numeric_limits<int8_t>::max()) break;
      divisor++;
    }
    if (dividend_i8 == std::numeric_limits<int8_t>::max()) break;
    dividend_i8++;
  }

  std::printf("Testing divround_<int8_t>\n");
  dividend_i8 = std::numeric_limits<int8_t>::lowest();
  while (true) {
    int8_t divisor = std::numeric_limits<int8_t>::lowest();
    while (true) {
      /* This would cause overflow in the result. Skip it. */
      if ((dividend_i8 == std::numeric_limits<int8_t>::lowest()) & (divisor == static_cast<int8_t>(-1))) divisor = static_cast<int8_t>(1);
      /* Don't divide by 0. */
      if (divisor == static_cast<int8_t>(0)) divisor = static_cast<int8_t>(1);
      int8_t dr = divround<int8_t>(dividend_i8, divisor);
      int8_t dbl_dr = static_cast<int8_t>(std::round(static_cast<double>(dividend_i8) / static_cast<double>(divisor)));
      if (dr != dbl_dr) {
        int8_t quotient = dividend_i8 / divisor;
        int8_t remainder = dividend_i8 - (quotient * divisor);
        int8_t div_half = divisor >> 1;
        if ((divisor & static_cast<uint8_t>(0x81)) == static_cast<int8_t>(0x01)) div_half++;
        std::printf("\nERROR: ROUND(%i / %i) = %i, but divround<int8_t> returns %i\n  remainder = %i, div_half = %i\n\n", dividend_i8, divisor, dbl_dr, dr, remainder, div_half);
      }
      if (divisor == std::numeric_limits<int8_t>::max()) break;
      divisor++;
    }
    if (dividend_i8 == std::numeric_limits<int8_t>::max()) break;
    dividend_i8++;
  }

  std::printf("Testing divround_u8\n");
  uint8_t dividend_u8 = std::numeric_limits<uint8_t>::lowest();
  while (true) {
    uint8_t divisor = static_cast<uint8_t>(1);
    while (true) {
      uint8_t dr = divround_u8(dividend_u8, divisor);
      uint8_t dbl_dr = static_cast<uint8_t>(std::round(static_cast<double>(dividend_u8) / static_cast<double>(divisor)));
      if (dr != dbl_dr) {
        uint8_t quotient = dividend_u8 / divisor;
        uint8_t remainder = dividend_u8 - (quotient * divisor);
        uint8_t div_half = divisor >> 1;
        if (divisor & static_cast<uint8_t>(0x01)) div_half++;
        std::printf("\nERROR: ROUND(%u / %u) = %u, but divround_u8 returns %u\n  remainder = %u, div_half = %u\n\n", dividend_u8, divisor, dbl_dr, dr, remainder, div_half);
      }
      if (divisor == std::numeric_limits<uint8_t>::max()) break;
      divisor++;
    }
    if (dividend_u8 == std::numeric_limits<uint8_t>::max()) break;
    dividend_u8++;
  }

  std::printf("Testing divround<uint8_t>\n");
  dividend_u8 = std::numeric_limits<uint8_t>::lowest();
  while (true) {
    uint8_t divisor = static_cast<uint8_t>(1);
    while (true) {
      uint8_t dr = divround<uint8_t>(dividend_u8, divisor);
      uint8_t dbl_dr = static_cast<uint8_t>(std::round(static_cast<double>(dividend_u8) / static_cast<double>(divisor)));
      if (dr != dbl_dr) {
        uint8_t quotient = dividend_u8 / divisor;
        uint8_t remainder = dividend_u8 - (quotient * divisor);
        uint8_t div_half = divisor >> 1;
        if (divisor & static_cast<uint8_t>(0x01)) div_half++;
        std::printf("\nERROR: ROUND(%u / %u) = %u, but divround<uint8_t> returns %u\n  remainder = %u, div_half = %u\n\n", dividend_u8, divisor, dbl_dr, dr, remainder, div_half);
      }
      if (divisor == std::numeric_limits<uint8_t>::max()) break;
      divisor++;
    }
    if (dividend_u8 == std::numeric_limits<uint8_t>::max()) break;
    dividend_u8++;
  }

  /**
   * Use one thread if only one hardware thread is available. Otherwise, use
   * one less than the number of available hardware threads.
   */
  uint32_t nThreads = std::thread::hardware_concurrency();
  if (nThreads <= 2u) nThreads = 1u;
  else nThreads--;

  /**
   * Store all threads in vThreads.
   */
  std::vector<std::thread> vThreads(nThreads);

  std::printf("\nStarting multithreaded tests with %u threads.\n\n", nThreads);

  std::printf("Testing divround_i16\n");
  /**
   * Test dividends on the range [-2^15, 2^15-1] multithreaded
   */
  {
    int32_t dividend_start = static_cast<int32_t>(std::numeric_limits<int16_t>::lowest());
    uint32_t nDividends = (1u << 16);
    uint32_t nDividends_per_thread = nDividends / nThreads;
    uint32_t nDividends_remaining = nDividends % nThreads;
    int32_t thread_dividend_start = dividend_start;
    for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
      int32_t thread_dividend_end = thread_dividend_start + static_cast<int32_t>(nDividends_per_thread-1u);
      if (jThread < nDividends_remaining) thread_dividend_end++;
      vThreads.at(jThread) = std::move(std::thread(test_divround_i16_c, static_cast<int16_t>(thread_dividend_start), static_cast<int16_t>(thread_dividend_end)));
      thread_dividend_start = thread_dividend_end+1;
    }
  }

  for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
    vThreads.at(jThread).join();
  }

  std::printf("Testing divround<int16_t>\n");
  /**
   * Test dividends on the range [-2^15, 2^15-1] multithreaded
   */
  {
    int32_t dividend_start = static_cast<int32_t>(std::numeric_limits<int16_t>::lowest());
    uint32_t nDividends = (1u << 16);
    uint32_t nDividends_per_thread = nDividends / nThreads;
    uint32_t nDividends_remaining = nDividends % nThreads;
    int32_t thread_dividend_start = dividend_start;
    for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
      int32_t thread_dividend_end = thread_dividend_start + static_cast<int32_t>(nDividends_per_thread-1u);
      if (jThread < nDividends_remaining) thread_dividend_end++;
      vThreads.at(jThread) = std::move(std::thread(test_divround_i16_cpp, static_cast<int16_t>(thread_dividend_start), static_cast<int16_t>(thread_dividend_end)));
      thread_dividend_start = thread_dividend_end+1;
    }
  }

  for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
    vThreads.at(jThread).join();
  }

  std::printf("Testing divround_u16\n");
  /**
   * Test dividends on the range [0, 2^16-1] multithreaded
   */
  {
    uint32_t dividend_start = static_cast<uint32_t>(std::numeric_limits<uint16_t>::lowest());
    uint32_t nDividends = (1u << 16);
    uint32_t nDividends_per_thread = nDividends / nThreads;
    uint32_t nDividends_remaining = nDividends % nThreads;
    uint32_t thread_dividend_start = dividend_start;
    for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
      uint32_t thread_dividend_end = thread_dividend_start + (nDividends_per_thread-1u);
      if (jThread < nDividends_remaining) thread_dividend_end++;
      vThreads.at(jThread) = std::move(std::thread(test_divround_u16_c, static_cast<uint16_t>(thread_dividend_start), static_cast<uint16_t>(thread_dividend_end)));
      thread_dividend_start = thread_dividend_end+1;
    }
  }

  for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
    vThreads.at(jThread).join();
  }

  std::printf("Testing divround<uint16_t>\n");
  /**
   * Test dividends on the range [0, 2^16-1] multithreaded
   */
  {
    uint32_t dividend_start = static_cast<uint32_t>(std::numeric_limits<uint16_t>::lowest());
    uint32_t nDividends = (1u << 16);
    uint32_t nDividends_per_thread = nDividends / nThreads;
    uint32_t nDividends_remaining = nDividends % nThreads;
    uint32_t thread_dividend_start = dividend_start;
    for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
      uint32_t thread_dividend_end = thread_dividend_start + (nDividends_per_thread-1u);
      if (jThread < nDividends_remaining) thread_dividend_end++;
      vThreads.at(jThread) = std::move(std::thread(test_divround_u16_cpp, static_cast<uint16_t>(thread_dividend_start), static_cast<uint16_t>(thread_dividend_end)));
      thread_dividend_start = thread_dividend_end+1;
    }
  }

  for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
    vThreads.at(jThread).join();
  }

  std::printf("Testing divround_i32\n");
  /**
   * Test dividends on the range [-2^31, -2^31+2^16] multithreaded
   */
  {
    int32_t dividend_start = std::numeric_limits<int32_t>::lowest();
    uint32_t nDividends = (1u << 16) + 1u;
    uint32_t nDividends_per_thread = nDividends / nThreads;
    uint32_t nDividends_remaining = nDividends % nThreads;
    int32_t thread_dividend_start = dividend_start;
    for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
      int32_t thread_dividend_end = thread_dividend_start + static_cast<int32_t>(nDividends_per_thread-1u);
      if (jThread < nDividends_remaining) thread_dividend_end++;
      vThreads.at(jThread) = std::move(std::thread(test_divround_i32_c, thread_dividend_start, thread_dividend_end));
      thread_dividend_start = thread_dividend_end+1;
    }
  }

  for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
    vThreads.at(jThread).join();
  }

  /**
   * Test dividends on the range [-2^16, 2^16] multithreaded
   */
  {
    int32_t dividend_start = -(1 << 16);
    uint32_t nDividends = (1u << 17) + 1u;
    uint32_t nDividends_per_thread = nDividends / nThreads;
    uint32_t nDividends_remaining = nDividends % nThreads;
    int32_t thread_dividend_start = dividend_start;
    for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
      int32_t thread_dividend_end = thread_dividend_start + static_cast<int32_t>(nDividends_per_thread-1u);
      if (jThread < nDividends_remaining) thread_dividend_end++;
      vThreads.at(jThread) = std::move(std::thread(test_divround_i32_c, thread_dividend_start, thread_dividend_end));
      thread_dividend_start = thread_dividend_end+1;
    }
  }

  for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
    vThreads.at(jThread).join();
  }

  /**
   * Test dividends on the range [2^31-2^16-1, 2^31-1] multithreaded
   */
  {
    int32_t dividend_start = std::numeric_limits<int32_t>::max() - (1 << 16);
    uint32_t nDividends = (1u << 16) + 1u;
    uint32_t nDividends_per_thread = nDividends / nThreads;
    uint32_t nDividends_remaining = nDividends % nThreads;
    int32_t thread_dividend_start = dividend_start;
    for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
      int32_t thread_dividend_end = thread_dividend_start + static_cast<int32_t>(nDividends_per_thread-1u);
      if (jThread < nDividends_remaining) thread_dividend_end++;
      vThreads.at(jThread) = std::move(std::thread(test_divround_i32_c, thread_dividend_start, thread_dividend_end));
      thread_dividend_start = thread_dividend_end+1;
    }
  }

  for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
    vThreads.at(jThread).join();
  }

  std::printf("Testing divround<int32_t>\n");
  /**
   * Test dividends on the range [-2^31, -2^31+2^16] multithreaded
   */
  {
    int32_t dividend_start = std::numeric_limits<int32_t>::lowest();
    uint32_t nDividends = (1u << 16) + 1u;
    uint32_t nDividends_per_thread = nDividends / nThreads;
    uint32_t nDividends_remaining = nDividends % nThreads;
    int32_t thread_dividend_start = dividend_start;
    for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
      int32_t thread_dividend_end = thread_dividend_start + static_cast<int32_t>(nDividends_per_thread-1u);
      if (jThread < nDividends_remaining) thread_dividend_end++;
      vThreads.at(jThread) = std::move(std::thread(test_divround_i32_cpp, thread_dividend_start, thread_dividend_end));
      thread_dividend_start = thread_dividend_end+1;
    }
  }

  for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
    vThreads.at(jThread).join();
  }

  /**
   * Test dividends on the range [-2^16, 2^16] multithreaded
   */
  {
    int32_t dividend_start = -(1 << 16);
    uint32_t nDividends = (1u << 17) + 1u;
    uint32_t nDividends_per_thread = nDividends / nThreads;
    uint32_t nDividends_remaining = nDividends % nThreads;
    int32_t thread_dividend_start = dividend_start;
    for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
      int32_t thread_dividend_end = thread_dividend_start + static_cast<int32_t>(nDividends_per_thread-1u);
      if (jThread < nDividends_remaining) thread_dividend_end++;
      vThreads.at(jThread) = std::move(std::thread(test_divround_i32_cpp, thread_dividend_start, thread_dividend_end));
      thread_dividend_start = thread_dividend_end+1;
    }
  }

  for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
    vThreads.at(jThread).join();
  }

  /**
   * Test dividends on the range [2^31-2^16-1, 2^31-1] multithreaded
   */
  {
    int32_t dividend_start = std::numeric_limits<int32_t>::max() - (1 << 16);
    uint32_t nDividends = (1u << 16) + 1u;
    uint32_t nDividends_per_thread = nDividends / nThreads;
    uint32_t nDividends_remaining = nDividends % nThreads;
    int32_t thread_dividend_start = dividend_start;
    for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
      int32_t thread_dividend_end = thread_dividend_start + static_cast<int32_t>(nDividends_per_thread-1u);
      if (jThread < nDividends_remaining) thread_dividend_end++;
      vThreads.at(jThread) = std::move(std::thread(test_divround_i32_cpp, thread_dividend_start, thread_dividend_end));
      thread_dividend_start = thread_dividend_end+1;
    }
  }

  for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
    vThreads.at(jThread).join();
  }

  std::printf("Testing divround_u32\n");
  /**
   * Test dividends on the range [0, 2^17] multithreaded
   */
  {
    uint32_t dividend_start = std::numeric_limits<uint32_t>::lowest();
    uint32_t nDividends = (1u << 17) + 1u;
    uint32_t nDividends_per_thread = nDividends / nThreads;
    uint32_t nDividends_remaining = nDividends % nThreads;
    uint32_t thread_dividend_start = dividend_start;
    for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
      uint32_t thread_dividend_end = thread_dividend_start + (nDividends_per_thread-1u);
      if (jThread < nDividends_remaining) thread_dividend_end++;
      vThreads.at(jThread) = std::move(std::thread(test_divround_u32_c, thread_dividend_start, thread_dividend_end));
      thread_dividend_start = thread_dividend_end+1u;
    }
  }

  for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
    vThreads.at(jThread).join();
  }

  /**
   * Test dividends on the range [2^32-2^17-1, 2^32-1] multithreaded
   */
  {
    uint32_t dividend_start = std::numeric_limits<uint32_t>::max() - (1u << 17);
    uint32_t nDividends = (1u << 17) + 1u;
    uint32_t nDividends_per_thread = nDividends / nThreads;
    uint32_t nDividends_remaining = nDividends % nThreads;
    uint32_t thread_dividend_start = dividend_start;
    for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
      uint32_t thread_dividend_end = thread_dividend_start + (nDividends_per_thread-1u);
      if (jThread < nDividends_remaining) thread_dividend_end++;
      vThreads.at(jThread) = std::move(std::thread(test_divround_u32_c, thread_dividend_start, thread_dividend_end));
      thread_dividend_start = thread_dividend_end+1u;
    }
  }

  for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
    vThreads.at(jThread).join();
  }

  std::printf("Testing divround<uint32_t>\n");
  /**
   * Test dividends on the range [0, 2^17] multithreaded
   */
  {
    uint32_t dividend_start = std::numeric_limits<uint32_t>::lowest();
    uint32_t nDividends = (1u << 17) + 1u;
    uint32_t nDividends_per_thread = nDividends / nThreads;
    uint32_t nDividends_remaining = nDividends % nThreads;
    uint32_t thread_dividend_start = dividend_start;
    for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
      uint32_t thread_dividend_end = thread_dividend_start + (nDividends_per_thread-1u);
      if (jThread < nDividends_remaining) thread_dividend_end++;
      vThreads.at(jThread) = std::move(std::thread(test_divround_u32_cpp, thread_dividend_start, thread_dividend_end));
      thread_dividend_start = thread_dividend_end+1u;
    }
  }

  for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
    vThreads.at(jThread).join();
  }

  /**
   * Test dividends on the range [2^32-2^17-1, 2^32-1] multithreaded
   */
  {
    uint32_t dividend_start = std::numeric_limits<uint32_t>::max() - (1u << 17);
    uint32_t nDividends = (1u << 17) + 1u;
    uint32_t nDividends_per_thread = nDividends / nThreads;
    uint32_t nDividends_remaining = nDividends % nThreads;
    uint32_t thread_dividend_start = dividend_start;
    for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
      uint32_t thread_dividend_end = thread_dividend_start + (nDividends_per_thread-1u);
      if (jThread < nDividends_remaining) thread_dividend_end++;
      vThreads.at(jThread) = std::move(std::thread(test_divround_u32_cpp, thread_dividend_start, thread_dividend_end));
      thread_dividend_start = thread_dividend_end+1u;
    }
  }

  for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
    vThreads.at(jThread).join();
  }

  std::printf("Testing divround_i64\n");
  /**
   * Test dividends on the range [-2^63, -2^63+2^16] multithreaded
   */
  {
    int64_t dividend_start = std::numeric_limits<int64_t>::lowest();
    uint64_t nDividends = (1ull << 16) + 1ull;
    uint64_t nDividends_per_thread = nDividends / nThreads;
    uint64_t nDividends_remaining = nDividends % nThreads;
    int64_t thread_dividend_start = dividend_start;
    for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
      int64_t thread_dividend_end = thread_dividend_start + static_cast<int64_t>(nDividends_per_thread-1ull);
      if (jThread < nDividends_remaining) thread_dividend_end++;
      vThreads.at(jThread) = std::move(std::thread(test_divround_i64_c, thread_dividend_start, thread_dividend_end));
      thread_dividend_start = thread_dividend_end+1ll;
    }
  }

  for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
    vThreads.at(jThread).join();
  }

  /**
   * Test dividends on the range [-2^16, 2^16] multithreaded
   */
  {
    int64_t dividend_start = -(1ll << 16);
    uint64_t nDividends = (1ull << 17) + 1ull;
    uint64_t nDividends_per_thread = nDividends / nThreads;
    uint64_t nDividends_remaining = nDividends % nThreads;
    int64_t thread_dividend_start = dividend_start;
    for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
      int64_t thread_dividend_end = thread_dividend_start + static_cast<int64_t>(nDividends_per_thread-1ull);
      if (jThread < nDividends_remaining) thread_dividend_end++;
      vThreads.at(jThread) = std::move(std::thread(test_divround_i64_c, thread_dividend_start,thread_dividend_end));
      thread_dividend_start = thread_dividend_end+1ll;
    }
  }

  for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
    vThreads.at(jThread).join();
  }

  /**
   * Test dividends on the range [2^63-2^16-1, 2^63-1] multithreaded
   */
  {
    int64_t dividend_start = std::numeric_limits<int64_t>::max() - (1ll << 16);
    uint64_t nDividends = (1ull << 16) + 1ull;
    uint64_t nDividends_per_thread = nDividends / nThreads;
    uint64_t nDividends_remaining = nDividends % nThreads;
    int64_t thread_dividend_start = dividend_start;
    for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
      int64_t thread_dividend_end = thread_dividend_start + static_cast<int64_t>(nDividends_per_thread-1ull);
      if (jThread < nDividends_remaining) thread_dividend_end++;
      vThreads.at(jThread) = std::move(std::thread(test_divround_i64_c, thread_dividend_start, thread_dividend_end));
      thread_dividend_start = thread_dividend_end+1ll;
    }
  }

  for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
    vThreads.at(jThread).join();
  }

  std::printf("Testing divround<int64_t>\n");
    /**
   * Test dividends on the range [-2^63, -2^63+2^16] multithreaded
   */
  {
    int64_t dividend_start = std::numeric_limits<int64_t>::lowest();
    uint64_t nDividends = (1ull << 16) + 1ull;
    uint64_t nDividends_per_thread = nDividends / nThreads;
    uint64_t nDividends_remaining = nDividends % nThreads;
    int64_t thread_dividend_start = dividend_start;
    for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
      int64_t thread_dividend_end = thread_dividend_start + static_cast<int64_t>(nDividends_per_thread-1ull);
      if (jThread < nDividends_remaining) thread_dividend_end++;
      vThreads.at(jThread) = std::move(std::thread(test_divround_i64_cpp, thread_dividend_start, thread_dividend_end));
      thread_dividend_start = thread_dividend_end+1ll;
    }
  }

  for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
    vThreads.at(jThread).join();
  }

  /**
   * Test dividends on the range [-2^16, 2^16] multithreaded
   */
  {
    int64_t dividend_start = -(1ll << 16);
    uint64_t nDividends = (1ull << 17) + 1ull;
    uint64_t nDividends_per_thread = nDividends / nThreads;
    uint64_t nDividends_remaining = nDividends % nThreads;
    int64_t thread_dividend_start = dividend_start;
    for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
      int64_t thread_dividend_end = thread_dividend_start + static_cast<int64_t>(nDividends_per_thread-1ull);
      if (jThread < nDividends_remaining) thread_dividend_end++;
      vThreads.at(jThread) = std::move(std::thread(test_divround_i64_cpp, thread_dividend_start, thread_dividend_end));
      thread_dividend_start = thread_dividend_end+1ll;
    }
  }

  for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
    vThreads.at(jThread).join();
  }

  /**
   * Test dividends on the range [2^63-2^16-1, 2^63-1] multithreaded
   */
  {
    int64_t dividend_start = std::numeric_limits<int64_t>::max() - (1ll << 16);
    uint64_t nDividends = (1ull << 16) + 1ull;
    uint64_t nDividends_per_thread = nDividends / nThreads;
    uint64_t nDividends_remaining = nDividends % nThreads;
    int64_t thread_dividend_start = dividend_start;
    for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
      int64_t thread_dividend_end = thread_dividend_start + static_cast<int64_t>(nDividends_per_thread-1ull);
      if (jThread < nDividends_remaining) thread_dividend_end++;
      vThreads.at(jThread) = std::move(std::thread(test_divround_i64_cpp, thread_dividend_start, thread_dividend_end));
      thread_dividend_start = thread_dividend_end+1ll;
    }
  }

  for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
    vThreads.at(jThread).join();
  }

  std::printf("Testing divround_u64\n");
  /**
   * Test dividends on the range [0, 2^17] multithreaded
   */
  {
    uint64_t dividend_start = std::numeric_limits<uint64_t>::lowest();
    uint64_t nDividends = (1ull << 17) + 1ull;
    uint64_t nDividends_per_thread = nDividends / nThreads;
    uint64_t nDividends_remaining = nDividends % nThreads;
    uint64_t thread_dividend_start = dividend_start;
    for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
      uint64_t thread_dividend_end = thread_dividend_start + (nDividends_per_thread-1ull);
      if (jThread < nDividends_remaining) thread_dividend_end++;
      vThreads.at(jThread) = std::move(std::thread(test_divround_u64_c, thread_dividend_start, thread_dividend_end));
      thread_dividend_start = thread_dividend_end+1ull;
    }
  }

  for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
    vThreads.at(jThread).join();
  }

  /**
   * Test dividends on the range [2^64-2^17-1, 2^64-1] multithreaded
   */
  {
    uint64_t dividend_start = std::numeric_limits<uint64_t>::max() - (1ull << 17);
    uint64_t nDividends = (1ull << 17) + 1ull;
    uint64_t nDividends_per_thread = nDividends / nThreads;
    uint64_t nDividends_remaining = nDividends % nThreads;
    uint64_t thread_dividend_start = dividend_start;
    for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
      uint64_t thread_dividend_end = thread_dividend_start + (nDividends_per_thread-1ull);
      if (jThread < nDividends_remaining) thread_dividend_end++;
      vThreads.at(jThread) = std::move(std::thread(test_divround_u64_c, thread_dividend_start, thread_dividend_end));
      thread_dividend_start = thread_dividend_end+1ull;
    }
  }

  for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
    vThreads.at(jThread).join();
  }

  std::printf("Testing divround<uint64_t>\n");
  /**
   * Test dividends on the range [0, 2^17] multithreaded
   */
  {
    uint64_t dividend_start = std::numeric_limits<uint64_t>::lowest();
    uint64_t nDividends = (1ull << 17) + 1ull;
    uint64_t nDividends_per_thread = nDividends / nThreads;
    uint64_t nDividends_remaining = nDividends % nThreads;
    uint64_t thread_dividend_start = dividend_start;
    for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
      uint64_t thread_dividend_end = thread_dividend_start + (nDividends_per_thread-1ull);
      if (jThread < nDividends_remaining) thread_dividend_end++;
      vThreads.at(jThread) = std::move(std::thread(test_divround_u64_cpp, thread_dividend_start, thread_dividend_end));
      thread_dividend_start = thread_dividend_end+1ull;
    }
  }

  for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
    vThreads.at(jThread).join();
  }

  /**
   * Test dividends on the range [2^64-2^17-1, 2^64-1] multithreaded
   */
  {
    uint64_t dividend_start = std::numeric_limits<uint64_t>::max() - (1ull << 17);
    uint64_t nDividends = (1ull << 17) + 1ull;
    uint64_t nDividends_per_thread = nDividends / nThreads;
    uint64_t nDividends_remaining = nDividends % nThreads;
    uint64_t thread_dividend_start = dividend_start;
    for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
      uint64_t thread_dividend_end = thread_dividend_start + (nDividends_per_thread-1ull);
      if (jThread < nDividends_remaining) thread_dividend_end++;
      vThreads.at(jThread) = std::move(std::thread(test_divround_u64_cpp, thread_dividend_start, thread_dividend_end));
      thread_dividend_start = thread_dividend_end+1ull;
    }
  }

  for (uint32_t jThread = 0u; jThread < nThreads; jThread++) {
    vThreads.at(jThread).join();
  }

  std::printf("If there are no errors above, the tests were successful.\n\n");
  return 0;
}

/*
Creative Commons Legal Code

CC0 1.0 Universal

    CREATIVE COMMONS CORPORATION IS NOT A LAW FIRM AND DOES NOT PROVIDE
    LEGAL SERVICES. DISTRIBUTION OF THIS DOCUMENT DOES NOT CREATE AN
    ATTORNEY-CLIENT RELATIONSHIP. CREATIVE COMMONS PROVIDES THIS
    INFORMATION ON AN "AS-IS" BASIS. CREATIVE COMMONS MAKES NO WARRANTIES
    REGARDING THE USE OF THIS DOCUMENT OR THE INFORMATION OR WORKS
    PROVIDED HEREUNDER, AND DISCLAIMS LIABILITY FOR DAMAGES RESULTING FROM
    THE USE OF THIS DOCUMENT OR THE INFORMATION OR WORKS PROVIDED
    HEREUNDER.

Statement of Purpose

The laws of most jurisdictions throughout the world automatically confer
exclusive Copyright and Related Rights (defined below) upon the creator
and subsequent owner(s) (each and all, an "owner") of an original work of
authorship and/or a database (each, a "Work").

Certain owners wish to permanently relinquish those rights to a Work for
the purpose of contributing to a commons of creative, cultural and
scientific works ("Commons") that the public can reliably and without fear
of later claims of infringement build upon, modify, incorporate in other
works, reuse and redistribute as freely as possible in any form whatsoever
and for any purposes, including without limitation commercial purposes.
These owners may contribute to the Commons to promote the ideal of a free
culture and the further production of creative, cultural and scientific
works, or to gain reputation or greater distribution for their Work in
part through the use and efforts of others.

For these and/or other purposes and motivations, and without any
expectation of additional consideration or compensation, the person
associating CC0 with a Work (the "Affirmer"), to the extent that he or she
is an owner of Copyright and Related Rights in the Work, voluntarily
elects to apply CC0 to the Work and publicly distribute the Work under its
terms, with knowledge of his or her Copyright and Related Rights in the
Work and the meaning and intended legal effect of CC0 on those rights.

1. Copyright and Related Rights. A Work made available under CC0 may be
protected by copyright and related or neighboring rights ("Copyright and
Related Rights"). Copyright and Related Rights include, but are not
limited to, the following:

  i. the right to reproduce, adapt, distribute, perform, display,
     communicate, and translate a Work;
 ii. moral rights retained by the original author(s) and/or performer(s);
iii. publicity and privacy rights pertaining to a person's image or
     likeness depicted in a Work;
 iv. rights protecting against unfair competition in regards to a Work,
     subject to the limitations in paragraph 4(a), below;
  v. rights protecting the extraction, dissemination, use and reuse of data
     in a Work;
 vi. database rights (such as those arising under Directive 96/9/EC of the
     European Parliament and of the Council of 11 March 1996 on the legal
     protection of databases, and under any national implementation
     thereof, including any amended or successor version of such
     directive); and
vii. other similar, equivalent or corresponding rights throughout the
     world based on applicable law or treaty, and any national
     implementations thereof.

2. Waiver. To the greatest extent permitted by, but not in contravention
of, applicable law, Affirmer hereby overtly, fully, permanently,
irrevocably and unconditionally waives, abandons, and surrenders all of
Affirmer's Copyright and Related Rights and associated claims and causes
of action, whether now known or unknown (including existing as well as
future claims and causes of action), in the Work (i) in all territories
worldwide, (ii) for the maximum duration provided by applicable law or
treaty (including future time extensions), (iii) in any current or future
medium and for any number of copies, and (iv) for any purpose whatsoever,
including without limitation commercial, advertising or promotional
purposes (the "Waiver"). Affirmer makes the Waiver for the benefit of each
member of the public at large and to the detriment of Affirmer's heirs and
successors, fully intending that such Waiver shall not be subject to
revocation, rescission, cancellation, termination, or any other legal or
equitable action to disrupt the quiet enjoyment of the Work by the public
as contemplated by Affirmer's express Statement of Purpose.

3. Public License Fallback. Should any part of the Waiver for any reason
be judged legally invalid or ineffective under applicable law, then the
Waiver shall be preserved to the maximum extent permitted taking into
account Affirmer's express Statement of Purpose. In addition, to the
extent the Waiver is so judged Affirmer hereby grants to each affected
person a royalty-free, non transferable, non sublicensable, non exclusive,
irrevocable and unconditional license to exercise Affirmer's Copyright and
Related Rights in the Work (i) in all territories worldwide, (ii) for the
maximum duration provided by applicable law or treaty (including future
time extensions), (iii) in any current or future medium and for any number
of copies, and (iv) for any purpose whatsoever, including without
limitation commercial, advertising or promotional purposes (the
"License"). The License shall be deemed effective as of the date CC0 was
applied by Affirmer to the Work. Should any part of the License for any
reason be judged legally invalid or ineffective under applicable law, such
partial invalidity or ineffectiveness shall not invalidate the remainder
of the License, and in such case Affirmer hereby affirms that he or she
will not (i) exercise any of his or her remaining Copyright and Related
Rights in the Work or (ii) assert any associated claims and causes of
action with respect to the Work, in either case contrary to Affirmer's
express Statement of Purpose.

4. Limitations and Disclaimers.

 a. No trademark or patent rights held by Affirmer are waived, abandoned,
    surrendered, licensed or otherwise affected by this document.
 b. Affirmer offers the Work as-is and makes no representations or
    warranties of any kind concerning the Work, express, implied,
    statutory or otherwise, including without limitation warranties of
    title, merchantability, fitness for a particular purpose, non
    infringement, or the absence of latent or other defects, accuracy, or
    the present or absence of errors, whether or not discoverable, all to
    the greatest extent permissible under applicable law.
 c. Affirmer disclaims responsibility for clearing rights of other persons
    that may apply to the Work or any use thereof, including without
    limitation any person's Copyright and Related Rights in the Work.
    Further, Affirmer disclaims responsibility for obtaining any necessary
    consents, permissions or other rights required for any use of the
    Work.
 d. Affirmer understands and acknowledges that Creative Commons is not a
    party to this document and has no duty or obligation with respect to
    this CC0 or use of the Work.
*/


#include <stan/math/prim/arr.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <gtest/gtest.h>

TEST(MathFunctions, sumZeroSize) {
  std::vector<double> x;
  EXPECT_FLOAT_EQ(0.0, stan::math::sum(x));
}

TEST(MathFunctions, sum) {
  std::vector<double> x(3);

  x[0] = 1.0;
  x[1] = 2.0;
  x[2] = 3.0;

  EXPECT_FLOAT_EQ(6.0, stan::math::sum(x));
}

TEST(MathFunctions, sub_nan) {
  double nan = std::numeric_limits<double>::quiet_NaN();
  std::vector<double> x(3);

  x[0] = 1.0;
  x[1] = 2.0;
  x[2] = nan;

  EXPECT_PRED1(boost::math::isnan<double>,
               stan::math::sum(x));
}
TEST(MathMatrix,sum_vector_int) {
  std::vector<int> x(3);
  EXPECT_EQ(0,stan::math::sum(x));
  x[0] = 1;
  x[1] = 2;
  x[2] = 3;
  EXPECT_EQ(6,stan::math::sum(x));
}


#include <iostream>
#include <armadillo>
#include <cmath>
#include <cstdlib>
#include <time.h>
#include <fstream>
using namespace std;
using namespace arma;

void RHO_A_FILL(vec &rho, mat &A, int N,double rhoN); //rho, kind of like a linespace
                                                 //A, Tridiagonal matrix
void Maxoff(mat &A, int N,int &k, int &l, double &max);        //Finds the max element of a matrix

void Maxoff_test(double &max,int &k,int &l);

void Ortho_test(mat &S,int N);

int main(){
    int N = 400; //matrix size; N x N
    double rhoN = 60;
    double max;
    int k,l;
    Maxoff_test(max,k,l);

    mat A = mat(N,N,fill::zeros); //indexes go from (0) to (N-1)
    mat S = mat(N,N,fill::eye);
    vec rho = vec(N,fill::zeros);
    vec eigen = vec(N);

    RHO_A_FILL(rho,A,N,rhoN);
    Maxoff(A,N,k,l,max);
    int iterations = 0;

    double eps = 1E-10;

    double tau, t, s, c, il, ik, kk, ll, s_ik, s_il;
    double start, finish;

    start = clock();                      //clock value before eigen solve
    while(max > eps){
        tau = (A(l,l)-A(k,k))/(double(2)*A(k,l));
        if(tau>0){
             t = (-tau - sqrt(1.0 + tau*tau));}
        else{t = ( -tau + sqrt(1.0 + tau*tau));}

        //cosine and sine
        c = double(1)/sqrt(1.+t*t);
        s = t*c;

        //Jacobi rotating A round theta in N-dim space
        for(int i = 0; i<N; i++){
            if ((i != k) && (i !=l)){
                ik = A(i,k)*c - A(i,l)*s;
                il = A(i,l)*c + A(i,k)*s;
                A(i,k) = ik;
                A(k,i) = ik;
                A(i,l) = il;
                A(l,i) = il;
                }
            s_ik = S(i,k);
            s_il = S(i,l);
            S(i,k) = c*s_ik - s*s_il;
            S(i,l) = c*s_il + s*s_ik;
            }

        kk = A(k,k)*c*c - 2.*A(k,l)*c*s + A(l,l)*s*s;
        ll = A(l,l)*c*c + 2.*A(k,l)*c*s + A(k,k)*s*s;

        A(k,k) = kk;
        A(l,l) = ll;
        A(k,l) = 0;
        A(l,k) = 0;

        iterations++;
        Maxoff(A,N,k,l,max);
        //Ortho test
        if (iterations%N*2==0){
            Ortho_test(S,N);
            }

        } //end of while
    finish = clock();                    //clock value after eigen solve

    //[eigen] is now eigenvalues
    for(int i = 0; i<N;i++){
        eigen(i) = A(i,i);
        }

    eigen.print();

    double time = (finish -start)/CLOCKS_PER_SEC/iterations;

    fstream outfile;
    outfile.open("eigenvectors5.dat",ios::out);

    for (int i = 0; i<N; i++){
        for (int j = 0; j<N; j++){
            if(j%N == 0){outfile<<endl;}
            outfile << S(i,j)<<" ";
            }
        }
    outfile.close();

    fstream utfile;
    utfile.open("eigenvalues5.dat",ios::out);

    for (int i = 0; i<N; i++){
        utfile << eigen(i)<<endl;
        }
    utfile.close();

    //fstream Outfile;
    //Outfile.open("steps.dat",ios::out);
    //Outfile << "N     epsilon    steps  step_time"<<endl;

    //Outfile <<N <<" "<<eps<<" "<<iterations<<" "<<time;
    //Outfile<<endl;

    //Outfile.close();

} //end of main

void RHO_A_FILL(vec &rho, mat &A, int N,double rhoN){
    double h = rhoN/(N);
    rho(0) = 0.000001;
    for(int i = 0; i < N;i++){
        if(i > 0) rho(i) = i*h;
        A(i,i) = 2./(h*h)+ double(25)*rho(i)*rho(i)+(double(1)/rho(i));
        if(i<N-1){
            A(i+1,i) = -1./(h*h);
            A(i,i+1) = -1./(h*h);
}
        }
    }

void Maxoff(mat &A, int N,int &k, int &l, double &max){
    N = A.n_rows;
    max = 0;
    for(int i = 0; i < N ; i++){
        for(int j = i+1; j < N ; j++){
            if ( A(i,j)*A(i,j) > max){
                max =A(i,j)*A(i,j);
                k = i;
                l = j;
                }
            }
        }

    }
////////////Unit tests
void Maxoff_test(double &max,int &k,int &l){
    mat matrix = mat(5,5,fill::zeros);
    matrix(0,3) = -9;
    matrix(3,0) = -9;
    matrix(4,4) = -3;
    matrix(3,4) = -1;
    matrix(4,3) = -1;
    Maxoff(matrix,5,k,l,max);
    if(abs(max-81) < 1E-13){cout << "Maxoff function passes the test"<<endl;}
    else{cout<<"Maxoff might not be working properly"<<endl;}
    }
void Ortho_test(mat &S,int N){
    double error = 0;
    error = dot(S.col(int(double(rand())/RAND_MAX*N)),S.col(int(double(rand())/RAND_MAX*N)));
    if (abs(error) < 1e-10){
        cout << "ortho pass" << endl;}
    else{cout<<"ortho fail" << endl;}
    }


#include <Eigen/Dense>
#include <unsupported/Eigen/MatrixFunctions>
#include <pybind11/eigen.h>
#include <pybind11/operators.h>
#include <pybind11/pybind11.h>

namespace py = pybind11;

PYBIND11_PLUGIN(linear_solver) {
  py::module m("linear_solver", "linear_solver");

  m.def("solve",
        [](Eigen::Ref<Eigen::MatrixXd> H, Eigen::Ref<Eigen::VectorXd> g) {
          Eigen::MatrixXd solution = H.fullPivHouseholderQr().solve(g);
          return solution;
        });
  m.def("solve",
        [](Eigen::Ref<Eigen::MatrixXd> H, Eigen::Ref<Eigen::MatrixXd> g) {
          Eigen::MatrixXd solution = H.fullPivHouseholderQr().solve(g);
          return solution;
        });

  m.def("expm", [](Eigen::Ref<Eigen::MatrixXd> A) {
    Eigen::MatrixXd solution = A.exp();
    return solution;

  });

  m.def("logm", [](Eigen::Ref<Eigen::MatrixXd> A) {
      Eigen::MatrixXd solution = A.log();
      return solution;

    });

  return m.ptr();
}


//          Copyright Carl Philipp Reh 2009 - 2016.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <fcppt/math/ceil_div.hpp>
#include <fcppt/math/ceil_div_signed.hpp>
#include <fcppt/preprocessor/disable_gcc_warning.hpp>
#include <fcppt/preprocessor/pop_warning.hpp>
#include <fcppt/preprocessor/push_warning.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/test/unit_test.hpp>
#include <fcppt/config/external_end.hpp>

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

BOOST_AUTO_TEST_CASE(
	ceil_div
)
{
FCPPT_PP_POP_WARNING

	BOOST_CHECK_EQUAL(
		fcppt::math::ceil_div(
			0u,
			1u
		),
		0u
	);

	BOOST_CHECK_EQUAL(
		fcppt::math::ceil_div(
			1u,
			1u
		),
		1u
	);

	BOOST_CHECK_EQUAL(
		fcppt::math::ceil_div(
			5u,
			3u
		),
		2u
	);

	BOOST_CHECK_EQUAL(
		fcppt::math::ceil_div(
			6u,
			3u
		),
		2u
	);
}

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

BOOST_AUTO_TEST_CASE(
	ceil_div_signed
)
{
FCPPT_PP_POP_WARNING

	BOOST_CHECK_EQUAL(
		fcppt::math::ceil_div_signed(
			-3,
			2
		),
		-1
	);

	BOOST_CHECK_EQUAL(
		fcppt::math::ceil_div_signed(
			5,
			3
		),
		2
	);
}


#pragma once
// hprblas.hpp :  include file containing templated C++ interfaces to High-Performance Reproducible BLAS routines
//
// Copyright (C) 2017-2021 Stillwater Supercomputing, Inc.
//
// This file is part of the HPRBLAS project, which is released under an MIT Open Source license.
#include <universal/number/posit/posit>
#include <boost/numeric/mtl/mtl.hpp>

namespace sw {
namespace hprblas {

// LEVEL 1 BLAS operators

// 1-norm of a vector: sum of magnitudes of the vector elements, default increment stride is 1
template<typename Vector>
typename Vector::value_type asum(size_t n, const Vector& x, size_t incx = 1) {
	typename Vector::value_type sum = 0;
	size_t ix;
	for (ix = 0; ix < n; ix += incx) {
		sum += (x[ix] < 0 ? -x[ix] : x[ix]);
	}
	return sum;
}

// sum of the vector elements, default increment stride is 1
template<typename Vector>
typename Vector::value_type sum(size_t n, const Vector& x, size_t incx = 1) {
	typename Vector::value_type sum = 0;
	size_t ix;
	for (ix = 0; ix < n; ix += incx) {
		sum += x[ix];
	}
	return sum;
}

// a time x plus y
template<typename Scalar, typename Vector>
void axpy(size_t n, Scalar a, const Vector& x, size_t incx, Vector& y, size_t incy) {
	using namespace mtl;
	size_t cnt, ix, iy;
	for (cnt = 0, ix = 0, iy = 0; cnt < n && ix < size(x) && iy < size(y); ++cnt, ix += incx, iy += incy) {
		y[iy] += a * x[ix];
	}
}

// vector copy
template<typename Vector>
void copy(size_t n, const Vector& x, size_t incx, Vector& y, size_t incy) {
	using namespace mtl;
	size_t cnt, ix, iy;
	for (cnt = 0, ix = 0, iy = 0; cnt < n && ix < size(x) && iy < size(y); ++cnt, ix += incx, iy += incy) {
		y[iy] = x[ix];
	}
}

// adapter for STL vectors
//template<typename Scalar> auto size(const std::vector<Scalar>& v) { return v.size(); }

// dot product: the operator vector::x[index] is limited to uint32_t, so the arguments are limited to uint32_t as well
// The library does support arbitrary posit configuration conversions, but to simplify the
// behavior of the dot product, the element type of the vectors x and y are declared to be the same.
// TODO: investigate if the vector<> index is always a 32bit entity?
template<typename Vector>
typename Vector::value_type dot(size_t n, const Vector& x, size_t incx, const Vector& y, size_t incy) {
	using namespace mtl;
	typename Vector::value_type product = 0;
	size_t cnt, ix, iy;
	for (cnt = 0, ix = 0, iy = 0; cnt < n && ix < mtl::size(x) && iy < mtl::size(y); ++cnt, ix += incx, iy += incy) {
		product += x[ix] * y[iy];
	}
	return product;
}
// specialized dot product
template<typename Vector>
typename Vector::value_type dot(const Vector& x, const Vector& y) {
	using namespace mtl;
	typename Vector::value_type product = 0;
	size_t cnt, ix, iy;
	for (cnt = 0, ix = 0, iy = 0; cnt < mtl::size(x); ++cnt, ++ix, ++iy) {
		product += x[ix] * y[iy];
	}
	return product;
}
///
/// fused dot product operators

// Fused dot product with quire continuation
template<typename Quire, typename Vector>
void fdp_qr(Quire& sum_of_products, size_t n, const Vector& x, size_t incx, const Vector& y, size_t incy) {
	size_t ix, iy;
	for (ix = 0, iy = 0; ix < n && iy < n; ix = ix + incx, iy = iy + incy) {
		sum_of_products += sw::universal::quire_mul(x[ix], y[iy]);
	}
}
// Resolved fused dot product, with the option to control capacity bits in the quire
template<typename Vector, size_t capacity = 10>
typename Vector::value_type fdp_stride(size_t n, const Vector& x, size_t incx, const Vector& y, size_t incy) {
	constexpr size_t nbits = Vector::value_type::nbits;
	constexpr size_t es = Vector::value_type::es;
	sw::universal::quire<nbits, es, capacity> q(0);
	size_t ix, iy;
	for (ix = 0, iy = 0; ix < n && iy < n; ix = ix + incx, iy = iy + incy) {
		q += sw::universal::quire_mul(x[ix], y[iy]);
		if (sw::universal::_trace_quire_add) std::cout << q << '\n';
	}
	typename Vector::value_type sum;
	sw::universal::convert(q.to_value(), sum);     // one and only rounding step of the fused-dot product
	return sum;
}
// Specialized resolved fused dot product that assumes unit stride and a standard vector,
// with the option to control capacity bits in the quire
template<typename Vector, size_t capacity = 10>
typename Vector::value_type fdp(const Vector& x, const Vector& y) {
	using namespace mtl;
	constexpr size_t nbits = Vector::value_type::nbits;
	constexpr size_t es = Vector::value_type::es;
	sw::universal::quire<nbits, es, capacity> q(0);
	size_t ix, iy, n = mtl::size(x);
	for (ix = 0, iy = 0; ix < n && iy < n; ++ix, ++iy) {
		q += sw::universal::quire_mul(x[ix], y[iy]);
	}
	typename Vector::value_type sum;
	sw::universal::convert(q.to_value(), sum);     // one and only rounding step of the fused-dot product
	return sum;
}

// rotation of points in the plane
template<typename Rotation, typename Vector>
void rot(size_t n, Vector& x, size_t incx, Vector& y, size_t incy, Rotation c, Rotation s) {
	using namespace mtl;
	// x_i = c*x_i + s*y_i
	// y_i = c*y_i - s*x_i
	size_t cnt, ix, iy;
	for (cnt = 0, ix = 0, iy = 0; cnt < n && ix < size(x) && iy < size(y); ++cnt, ix += incx, iy += incy) {
		Rotation x_i = c*x[ix] + s*y[iy];
		Rotation y_i = c*y[iy] - s*x[ix];
		y[iy] = y_i;
		x[ix] = x_i;
	}
}

// compute parameters for a Givens rotation
template<typename T>
void rotg(T& a, T& b, T& c, T&s) {
	// Given Cartesian coordinates (a,b) of a point, return parameters c,s,r, and z associated with the Givens rotation.
}

// scale a vector
template<typename Scalar, typename Vector>
void scale(size_t n, Scalar a, Vector& x, size_t incx) {
	using namespace mtl;
	size_t cnt, ix;
	for (cnt = 0, ix = 0; cnt < n && ix < size(x); ix += incx) {
		x[ix] *= a;
	}
}

// swap two vectors
template<typename Vector>
void swap(size_t n, Vector& x, size_t incx, Vector& y, size_t incy) {
	using namespace mtl;
	size_t cnt, ix, iy;
	for (cnt = 0, ix = 0, iy = 0; cnt < n && ix < size(x) && iy < size(y); ++cnt, ix += incx, iy += incy) {
		typename Vector::value_type tmp = x[ix];
		x[ix] = y[iy];
		y[iy] = tmp;
	}
}

// find the index of the element with maximum absolute value
template<typename Vector>
size_t amax(size_t n, const Vector& x, size_t incx) {
	using namespace mtl;
	typename Vector::value_type running_max = -INFINITY;
	size_t ix, index;
	for (ix = 0; ix < size(x); ix += incx) {
		if (x[ix] > running_max) {
			index = ix;
			running_max = x[ix];
		}
	}
	return index;
}

// find the index of the element with minimum absolute value
template<typename Vector>
size_t amin(size_t n, const Vector& x, size_t incx) {
	using namespace mtl;
	typename Vector::value_type running_min = INFINITY;
	size_t ix, index;
	for (ix = 0; ix < size(x); ix += incx) {
		if (x[ix] < running_min) {
			index = ix;
			running_min = x[ix];
		}
	}
	return index;
}

// absolute value of a complex number
template<typename T>
T cabs(T z) {
}

// print a vector
template<typename Vector>
void strided_print(std::ostream& ostr, size_t n, Vector& x, size_t incx = 1) {
	using namespace mtl;
	size_t cnt, ix;
	for (cnt = 0, ix = 0; cnt < n && ix < size(x); ++cnt, ix += incx) {
		cnt == 0 ? ostr << "[" << x[ix] : ostr << ", " << x[ix];
	}
	ostr << "]";
}

// LEVEL 2 BLAS operators

// Matrix-vector product: b = A * x
template<typename Matrix, typename Vector>
void matvec(Vector& b, const Matrix& A, const Vector& x) {
	b = A * x;
}

// Matrix-vector product: b = A * x, posit specialized
template<size_t nbits, size_t es>
void matvec(mtl::vec::dense_vector< sw::universal::posit<nbits, es> >& b, const mtl::mat::dense2D< sw::universal::posit<nbits, es> >& A, const mtl::vec::dense_vector< sw::universal::posit<nbits, es> >& x) {
	using namespace mtl;
	// preconditions
	assert(A.num_cols() == size(x));
	assert(size(b) == size(x));

#if HPRBLAS_TRACE_ROUNDING_EVENTS
	unsigned errors = 0;
#endif
	size_t nr = size(b);
	size_t nc = size(x);
	for (size_t i = 0; i < nr; ++i) {
		sw::universal::quire<nbits, es> q(0);
		for (size_t j = 0; j < nc; ++j) {
			q += sw::universal::quire_mul(A[i][j], x[j]);
		}
		sw::universal::convert(q.to_value(), b[i]);     // one and only rounding step of the fused-dot product
#if HPRBLAS_TRACE_ROUNDING_EVENTS
		sw::universal::quire<nbits, es> qdiff = q;
		sw::universal::quire<nbits, es> qsum = b[i];
		qdiff -= qsum;
		if (!qdiff.iszero()) {
			++errors;
			std::cout << "q    : " << q << std::endl;
			std::cout << "qsum : " << qsum << std::endl;
			std::cout << "qdiff: " << qdiff << std::endl;
			sw::universal::posit<nbits, es> roundingError;
			convert(qdiff.to_value(), roundingError);
			std::cout << "matvec b[" << i << "] = " << posit_format(b[i]) << " rounding error: " << posit_format(roundingError) << " " << roundingError << std::endl;
		}
#endif
	}
#if HPRBLAS_TRACE_ROUNDING_EVENTS
	if (errors) {
		std::cout << "HPR-BLAS: tracing found " << errors << " rounding errors in matvec operation\n";
	}
#endif
}

// A times x = b fused matrix-vector product
template<size_t nbits, size_t es>
mtl::vec::dense_vector< sw::universal::posit<nbits, es> > fmv(const mtl::mat::dense2D< sw::universal::posit<nbits, es> >& A, const mtl::vec::dense_vector< sw::universal::posit<nbits, es> >& x) {
	using namespace mtl;
	// preconditions
	assert(A.num_cols() == size(x));
	mtl::vec::dense_vector< sw::universal::posit<nbits, es> > b(size(x));

#if HPRBLAS_TRACE_ROUNDING_EVENTS
	unsigned errors = 0;
#endif
	size_t nr = size(b);
	size_t nc = size(x);
	for (size_t i = 0; i < nr; ++i) {
		sw::universal::quire<nbits, es> q(0);
		for (size_t j = 0; j < nc; ++j) {
			q += sw::universal::quire_mul(A[i][j], x[j]);
		}
		sw::universal::convert(q.to_value(), b[i]);     // one and only rounding step of the fused-dot product
#if HPRBLAS_TRACE_ROUNDING_EVENTS
		sw::universal::quire<nbits, es> qdiff = q;
		sw::universal::quire<nbits, es> qsum = b[i];
		qdiff -= qsum;
		if (!qdiff.iszero()) {
			++errors;
			std::cout << "q    : " << q << std::endl;
			std::cout << "qsum : " << qsum << std::endl;
			std::cout << "qdiff: " << qdiff << std::endl;
			sw::universal::posit<nbits, es> roundingError;
			convert(qdiff.to_value(), roundingError);
			std::cout << "matvec b[" << i << "] = " << posit_format(b[i]) << " rounding error: " << posit_format(roundingError) << " " << roundingError << std::endl;
		}
#endif
	}
#if HPRBLAS_TRACE_ROUNDING_EVENTS
	if (errors) {
		std::cout << "HPR-BLAS: tracing found " << errors << " rounding errors in matvec operation\n";
	}
#endif
	return b;
}

// LEVEL 3 BLAS operators

template<typename Matrix>
void matmul(Matrix& C, const Matrix& A, const Matrix& B) {
	C = A * B;
}

// C = A * B fused matrix-matrix product when posits are used
template<size_t nbits, size_t es>
void matmul(mtl::mat::dense2D< sw::universal::posit<nbits, es> >& C, const mtl::mat::dense2D< sw::universal::posit<nbits, es> >& A, const mtl::mat::dense2D< sw::universal::posit<nbits, es> >& B) {
	// precondition
	assert(A.num_cols() == B.num_rows());
	size_t nr = A.num_rows();
	size_t nc = B.num_cols();
	size_t nk = A.num_cols();
	// TODO: add asserts to make certain that C is the right size

	for (size_t i = 0; i < nr; ++i) {
		for (size_t j = 0; j < nc; ++j) {
			sw::universal::quire<nbits, es> q(0);
			for (size_t k = 0; k < nk; ++k) {
				q += sw::universal::quire_mul(A[i][k], B[k][j]);
			}
			sw::universal::convert(q.to_value(), C[i][j]);     // one and only rounding step of the fused-dot product
		}
	}
}

// C = A * B fused matrix-matrix product when posits are used
template<size_t nbits, size_t es>
mtl::mat::dense2D< sw::universal::posit<nbits, es> > fmm(const mtl::mat::dense2D< sw::universal::posit<nbits, es> >& A, const mtl::mat::dense2D< sw::universal::posit<nbits, es> >& B) {
	// precondition
	assert(A.num_cols() == B.num_rows());
	size_t nr = A.num_rows();
	size_t nc = B.num_cols();
	size_t nk = A.num_cols();
	mtl::mat::dense2D< sw::universal::posit<nbits, es> > C(nr, nc);

	for (size_t i = 0; i < nr; ++i) {
		for (size_t j = 0; j < nc; ++j) {
			sw::universal::quire<nbits, es> q(0);
			for (size_t k = 0; k < nk; ++k) {
				q += sw::universal::quire_mul(A[i][k], B[k][j]);
			}
			sw::universal::convert(q.to_value(), C[i][j]);     // one and only rounding step of the fused-dot product
		}
	}
	return C;
}

template<typename Scalar>
inline Scalar minimum(const Scalar& a, const Scalar& b) {
	return (a < b ? a : b);
}

// if you only specify one set of block parameters in the specification then by
// the virtue of doing block matrix multiplication you generate the invariant : blockHeight == blockWidth
// Thus, no need to specify blockHeight and blockWidth: we can simplify to blockSize
// blockHeight = blockWidth = blockSize

// subBlockMM generates the partial sums of a sub-block matrix multiply
// the QuireMatrix is [blockHeight][blockWidth] submatrix
// A and B matrices are full [n][m] and [m][n] matrices
template<typename Matrix>
void subBlockMM(Matrix& C_partial, const Matrix& A, unsigned Ai, unsigned Aj, const Matrix& B, unsigned Bi, unsigned Bj) {
	assert(mtl::mat::num_rows(C_partial) == mtl::mat::num_cols(C_partial));
	using Scalar = typename Matrix::value_type;
//	constexpr size_t nbits = Scalar::nbits;
//	constexpr size_t es = Scalar::es;

	unsigned aRows = unsigned(mtl::mat::num_rows(A));
	unsigned aCols = unsigned(mtl::mat::num_cols(A));
	unsigned bRows = unsigned(mtl::mat::num_rows(B));
	unsigned bCols = unsigned(mtl::mat::num_cols(B));

	unsigned blockSize = unsigned(mtl::mat::num_rows(C_partial));

	unsigned aRow = Ai * blockSize;
	unsigned aCol = Aj * blockSize;
	unsigned bRow = Bi * blockSize;
	unsigned bCol = Bj * blockSize;

	// calculate the shape of submatrix A
	unsigned ar = (aRow + blockSize < aRows) ? blockSize : aRows - aRow;
	unsigned ac = (aCol + blockSize < aCols) ? blockSize : aCols - aCol;
	unsigned br = (bRow + blockSize < bRows) ? blockSize : bRows - bRow;
	unsigned bc = (bCol + blockSize < bCols) ? blockSize : bCols - bCol;

//	std::cout << "A=" << ar << "x" << ac << "  B=" << br << "x" << bc << std::endl;
	assert(ac == br);
	// A(ar,ac) x B(br,bc) = C(ar,bc) if ac == br
	for (unsigned i = 0; i < ar; ++i) {
		for (unsigned j = 0; j < bc; ++j) {
			for (unsigned k = 0; k < ac; ++k) {
				C_partial[i][j] += A[aRow + i][aCol + k] * B[bRow + k][bCol + j];
			}
//			std::cout << "A(" << Ai << "," << Aj << ") B(" << Bi << "," << Bj << ") C(" << i << "," << j << ")\n";
//			printMatrix(std::cout, "partial", C_partial);
		}
	}
}

// copySubBlockInto copies a subblock matrix into of the mother matrix
template<typename Matrix>
void copySubBlockInto(Matrix& A, unsigned ai, unsigned aj, const Matrix& SubBlock) {
	unsigned blockHeight = unsigned(mtl::mat::num_rows(SubBlock));
	unsigned blockWidth = unsigned(mtl::mat::num_cols(SubBlock));

	unsigned aRows = unsigned(mtl::mat::num_rows(A));
	unsigned aCols = unsigned(mtl::mat::num_cols(A));

	unsigned aRow = ai * blockHeight;
	unsigned aCol = aj * blockWidth;

	unsigned maxRow = (aRow + blockHeight < aRows) ? blockHeight : aRows - aRow;
	unsigned maxCol = (aCol + blockWidth < aCols) ? blockWidth : aCols - aCol;

	for (unsigned i = 0; i < maxRow; ++i) {
		for (unsigned j = 0; j < maxCol; ++j) {
			A[aRow + i][aCol + j] = SubBlock[i][j];
		}
	}
}

// bmm is a blocked matrix multply: C = A * B
template<typename Matrix>
Matrix bmm(const Matrix& A, const Matrix& B, unsigned blockSize) {
	// precondition
	assert(A.num_cols() == B.num_rows());
	unsigned nr = unsigned(A.num_rows());
	unsigned nc = unsigned(B.num_cols());
	unsigned nk = unsigned(A.num_cols());
	Matrix C(nr, nc);

	using Scalar = typename Matrix::value_type;
	Matrix C_partial(blockSize, blockSize);

	unsigned nrRowBlocks = nr % blockSize ? nr / blockSize + 1 : nr / blockSize;
	unsigned nrColBlocks = nc % blockSize ? nr / blockSize + 1 : nc / blockSize;
	for (unsigned bi = 0; bi < nrRowBlocks; ++bi) {			// row block index
		for (unsigned bj = 0; bj < nrColBlocks; ++bj) {		// col block index
			C_partial = Scalar(0);
			for (unsigned bk = 0; bk < nrRowBlocks; ++bk) { // block iterator
				subBlockMM(C_partial, A, bi, bk, B, bk, bj);
			}
			copySubBlockInto(C, bi, bj, C_partial);
		}
	}
	return C;
}

// subBlockMM generates the partial sums of a sub-block matrix multiply
// the QuireMatrix is a square matrix
// A and B matrices are full [n][m] and [m][n] matrices
template<typename QuireMatrix, typename Matrix>
void subBlockMM(QuireMatrix& C, const Matrix& A, unsigned Ai, unsigned Aj, const Matrix& B, unsigned Bi, unsigned Bj) {
	assert(mtl::mat::num_rows(C) == mtl::mat::num_cols(C));
	using Scalar = typename Matrix::value_type;
	constexpr size_t nbits = Scalar::nbits;
	constexpr size_t es = Scalar::es;

	unsigned aRows = unsigned(mtl::mat::num_rows(A));
	unsigned aCols = unsigned(mtl::mat::num_cols(A));
	unsigned bRows = unsigned(mtl::mat::num_rows(B));
	unsigned bCols = unsigned(mtl::mat::num_cols(B));

	unsigned blockSize = unsigned(mtl::mat::num_rows(C));

	unsigned aRow = Ai * blockSize;
	unsigned aCol = Aj * blockSize;
	unsigned bRow = Bi * blockSize;
	unsigned bCol = Bj * blockSize;

	// calculate the shape of submatrix A
	unsigned ar = (aRow + blockSize < aRows) ? blockSize : aRows - aRow;
	unsigned ac = (aCol + blockSize < aCols) ? blockSize : aCols - aCol;
	unsigned br = (bRow + blockSize < bRows) ? blockSize : bRows - bRow;
	unsigned bc = (bCol + blockSize < bCols) ? blockSize : bCols - bCol;

	//	std::cout << "A=" << ar << "x" << ac << "  B=" << br << "x" << bc << std::endl;
	assert(ac == br);
	// A(ar,ac) x B(br,bc) = C(ar,bc) if ac == br
	for (unsigned i = 0; i < ar; ++i) {
		for (unsigned j = 0; j < bc; ++j) {
			for (unsigned k = 0; k < ac; ++k) {
				C[i][j] += sw::universal::quire_mul<nbits,es>(A[aRow+i][aCol+k], B[bRow+k][bCol+j]);
			}
		}
	}
}

// subBlockRound takes a sub-block address and a QuireMatrix and rounds the partial sums

template<typename Matrix, typename QuireMatrix>
void subBlockRound(Matrix& C, unsigned ci, unsigned cj, const QuireMatrix& C_partial) {
	unsigned blockHeight = unsigned(mtl::mat::num_rows(C_partial));
	unsigned blockWidth = unsigned(mtl::mat::num_cols(C_partial));

	unsigned cRows = unsigned(mtl::mat::num_rows(C));
	unsigned cCols = unsigned(mtl::mat::num_cols(C));

	unsigned cRow = ci * blockHeight;
	unsigned cCol = cj * blockWidth;

	unsigned maxRow = (cRow + blockHeight < cRows) ? blockHeight : cRows - cRow;
	unsigned maxCol = (cCol + blockWidth < cCols) ? blockWidth : cCols - cCol;

	for (unsigned i = 0; i < maxRow; ++i) {
		for (unsigned j = 0; j < maxCol; ++j) {
			sw::universal::convert(C_partial[i][j].to_value(), C[cRow + i][cCol + j]);
		}
	}
}

// copySubBlock copies a subblock matrix out of the mother matrix
// This function is more generic than used in block matmul, as this can take non-square matrices
template<typename Matrix>
void copySubBlock(Matrix& SubBlock, const Matrix& M, unsigned bi, unsigned bj) {
	unsigned blockHeight = unsigned(mtl::mat::num_rows(SubBlock));
	unsigned blockWidth = unsigned(mtl::mat::num_cols(SubBlock));
	for (unsigned i = 0; i < blockHeight; ++i) {
		for (unsigned j = 0; j < blockWidth; ++j) {
			SubBlock[i][j] = M[bi*blockHeight + i][bj*blockWidth + j];
		}
	}
}

// bfmm is a blocked fused matrix multply: C = A * B
template<typename Matrix>
Matrix bfmm(const Matrix& A, const Matrix& B, unsigned blockSize) {
	// precondition
	assert(A.num_cols() == B.num_rows());
	unsigned nr = unsigned(A.num_rows());
	unsigned nc = unsigned(B.num_cols());
	unsigned nk = unsigned(A.num_cols());
	Matrix C(nr, nc);

	using Scalar = typename Matrix::value_type;
	constexpr size_t nbits = Scalar::nbits;
	constexpr size_t es = Scalar::es;
	using Quire = typename sw::universal::quire<nbits, es>;
	using QuireMatrix = typename mtl::mat::dense2D<Quire>;
	QuireMatrix C_partial(blockSize, blockSize);

	unsigned nrRowBlocks = nr % blockSize ? nr / blockSize + 1 : nr / blockSize;
	unsigned nrColBlocks = nc % blockSize ? nr / blockSize + 1  : nc / blockSize;
	for (unsigned bi = 0; bi < nrRowBlocks; ++bi) {			// row block index
		for (unsigned bj = 0; bj < nrColBlocks; ++bj) {		// col block index
			C_partial = Scalar(0);
			for (unsigned bk = 0; bk < nrRowBlocks; ++bk) { // block iterator
				subBlockMM(C_partial, A, bi, bk, B, bk, bj);
			}
			subBlockRound(C, bi, bj, C_partial);  // C_sub(i,j) = round(QuireMatrix)
		}
	}
	return C;
}

}} // namespace sw::hprblas


/**
 * \file
 * \copyright
 * Copyright (c) 2012-2019, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 */

#include <logog/include/logog.hpp>

#include <Eigen/Dense>

#include "Point3d.h"
#include "Vector3.h"

#include "GeometricBasics.h"

namespace MathLib
{
double orientation3d(MathLib::Point3d const& p,
                     MathLib::Point3d const& a,
                     MathLib::Point3d const& b,
                     MathLib::Point3d const& c)
{
    MathLib::Vector3 const ap (a, p);
    MathLib::Vector3 const bp (b, p);
    MathLib::Vector3 const cp (c, p);
    return MathLib::scalarTriple(bp,cp,ap);
}

double calcTetrahedronVolume(MathLib::Point3d const& a,
                             MathLib::Point3d const& b,
                             MathLib::Point3d const& c,
                             MathLib::Point3d const& d)
{
    const MathLib::Vector3 ab(a, b);
    const MathLib::Vector3 ac(a, c);
    const MathLib::Vector3 ad(a, d);
    return std::abs(MathLib::scalarTriple(ac, ad, ab)) / 6.0;
}

double calcTriangleArea(MathLib::Point3d const& a, MathLib::Point3d const& b,
                        MathLib::Point3d const& c)
{
    MathLib::Vector3 const u(a, c);
    MathLib::Vector3 const v(a, b);
    MathLib::Vector3 const w(MathLib::crossProduct(u, v));
    return 0.5 * w.getLength();
}

bool isPointInTetrahedron(MathLib::Point3d const& p, MathLib::Point3d const& a,
                          MathLib::Point3d const& b, MathLib::Point3d const& c,
                          MathLib::Point3d const& d, double eps)
{
    double const d0 (MathLib::orientation3d(d,a,b,c));
    // if tetrahedron is not coplanar
    if (std::abs(d0) > std::numeric_limits<double>::epsilon())
    {
        bool const d0_sign (d0>0);
        // if p is on the same side of bcd as a
        double const d1 (MathLib::orientation3d(d, p, b, c));
        if (!(d0_sign == (d1 >= 0) || std::abs(d1) < eps))
        {
            return false;
        }
        // if p is on the same side of acd as b
        double const d2 (MathLib::orientation3d(d, a, p, c));
        if (!(d0_sign == (d2 >= 0) || std::abs(d2) < eps))
        {
            return false;
        }
        // if p is on the same side of abd as c
        double const d3 (MathLib::orientation3d(d, a, b, p));
        if (!(d0_sign == (d3 >= 0) || std::abs(d3) < eps))
        {
            return false;
        }
        // if p is on the same side of abc as d
        double const d4 (MathLib::orientation3d(p, a, b, c));
        return d0_sign == (d4 >= 0) || std::abs(d4) < eps;
    }
    return false;
}

bool isPointInTriangle(MathLib::Point3d const& p,
                       MathLib::Point3d const& a,
                       MathLib::Point3d const& b,
                       MathLib::Point3d const& c,
                       double eps_pnt_out_of_plane,
                       double eps_pnt_out_of_tri,
                       MathLib::TriangleTest algorithm)
{
    switch (algorithm)
    {
        case MathLib::GAUSS:
            return gaussPointInTriangle(p, a, b, c, eps_pnt_out_of_plane,
                                        eps_pnt_out_of_tri);
        case MathLib::BARYCENTRIC:
            return barycentricPointInTriangle(p, a, b, c, eps_pnt_out_of_plane,
                                              eps_pnt_out_of_tri);
        default:
            ERR("Selected algorithm for point in triangle testing not found, "
                "falling back on default.");
    }
    return gaussPointInTriangle(p, a, b, c, eps_pnt_out_of_plane,
                                eps_pnt_out_of_tri);
}

bool gaussPointInTriangle(MathLib::Point3d const& q,
                          MathLib::Point3d const& a,
                          MathLib::Point3d const& b,
                          MathLib::Point3d const& c,
                          double eps_pnt_out_of_plane,
                          double eps_pnt_out_of_tri)
{
    MathLib::Vector3 const v(a, b);
    MathLib::Vector3 const w(a, c);

    Eigen::Matrix2d mat;
    mat(0, 0) = v.getSqrLength();
    mat(0, 1) = v[0] * w[0] + v[1] * w[1] + v[2] * w[2];
    mat(1, 0) = mat(0, 1);
    mat(1, 1) = w.getSqrLength();
    Eigen::Vector2d y;
    y << v[0] * (q[0] - a[0]) + v[1] * (q[1] - a[1]) + v[2] * (q[2] - a[2]),
        w[0] * (q[0] - a[0]) + w[1] * (q[1] - a[1]) + w[2] * (q[2] - a[2]);

    y = mat.partialPivLu().solve(y);

    const double lower(eps_pnt_out_of_tri);
    const double upper(1 + lower);

    if (-lower <= y[0] && y[0] <= upper && -lower <= y[1] && y[1] <= upper &&
        y[0] + y[1] <= upper)
    {
        MathLib::Point3d const q_projected(std::array<double, 3>{
            {a[0] + y[0] * v[0] + y[1] * w[0], a[1] + y[0] * v[1] + y[1] * w[1],
             a[2] + y[0] * v[2] + y[1] * w[2]}});
        if (MathLib::sqrDist(q, q_projected) <= eps_pnt_out_of_plane)
        {
            return true;
        }
    }

    return false;
}

bool barycentricPointInTriangle(MathLib::Point3d const& p,
                                MathLib::Point3d const& a,
                                MathLib::Point3d const& b,
                                MathLib::Point3d const& c,
                                double eps_pnt_out_of_plane,
                                double eps_pnt_out_of_tri)
{
    if (std::abs(MathLib::orientation3d(p, a, b, c)) > eps_pnt_out_of_plane)
    {
        return false;
    }

    MathLib::Vector3 const pa(p, a);
    MathLib::Vector3 const pb(p, b);
    MathLib::Vector3 const pc(p, c);
    double const area_x_2(calcTriangleArea(a, b, c) * 2);

    double const alpha((MathLib::crossProduct(pb, pc).getLength()) / area_x_2);
    if (alpha < -eps_pnt_out_of_tri || alpha > 1 + eps_pnt_out_of_tri)
    {
        return false;
    }
    double const beta((MathLib::crossProduct(pc, pa).getLength()) / area_x_2);
    if (beta < -eps_pnt_out_of_tri || beta > 1 + eps_pnt_out_of_tri)
    {
        return false;
    }
    double const gamma(1 - alpha - beta);
    return !(gamma < -eps_pnt_out_of_tri || gamma > 1 + eps_pnt_out_of_tri);
}

bool isPointInTriangleXY(MathLib::Point3d const& p,
                         MathLib::Point3d const& a,
                         MathLib::Point3d const& b,
                         MathLib::Point3d const& c)
{
    // criterion: p-a = u0 * (b-a) + u1 * (c-a); 0 <= u0, u1 <= 1, u0+u1 <= 1
    Eigen::Matrix2d mat;
    mat(0, 0) = b[0] - a[0];
    mat(0, 1) = c[0] - a[0];
    mat(1, 0) = b[1] - a[1];
    mat(1, 1) = c[1] - a[1];
    Eigen::Vector2d y;
    y << p[0] - a[0], p[1] - a[1];

    y = mat.partialPivLu().solve(y);

    // check if u0 and u1 fulfills the condition
    return 0 <= y[0] && y[0] <= 1 && 0 <= y[1] && y[1] <= 1 && y[0] + y[1] <= 1;
}

bool dividedByPlane(const MathLib::Point3d& a, const MathLib::Point3d& b,
                    const MathLib::Point3d& c, const MathLib::Point3d& d)
{
    for (unsigned x = 0; x < 3; ++x)
    {
        const unsigned y = (x + 1) % 3;
        const double abc =
            (b[x] - a[x]) * (c[y] - a[y]) - (b[y] - a[y]) * (c[x] - a[x]);
        const double abd =
            (b[x] - a[x]) * (d[y] - a[y]) - (b[y] - a[y]) * (d[x] - a[x]);

        if ((abc > 0 && abd < 0) || (abc < 0 && abd > 0))
        {
            return true;
        }
    }
    return false;
}

bool isCoplanar(const MathLib::Point3d& a, const MathLib::Point3d& b,
                const MathLib::Point3d& c, const MathLib::Point3d& d)
{
    const MathLib::Vector3 ab(a, b);
    const MathLib::Vector3 ac(a, c);
    const MathLib::Vector3 ad(a, d);

    if (ab.getSqrLength() < pow(std::numeric_limits<double>::epsilon(), 2) ||
        ac.getSqrLength() < pow(std::numeric_limits<double>::epsilon(), 2) ||
        ad.getSqrLength() < pow(std::numeric_limits<double>::epsilon(), 2))
    {
        return true;
    }

    // In exact arithmetic <ac*ad^T, ab> should be zero
    // if all four points are coplanar.
    const double sqr_scalar_triple(
        pow(MathLib::scalarProduct(MathLib::crossProduct(ac, ad), ab), 2));
    // Due to evaluating the above numerically some cancellation or rounding
    // can occur. For this reason a normalisation factor is introduced.
    const double normalisation_factor =
        (ab.getSqrLength() * ac.getSqrLength() * ad.getSqrLength());

    // tolerance 1e-11 is choosen such that
    // a = (0,0,0), b=(1,0,0), c=(0,1,0) and d=(1,1,1e-6) are considered as
    // coplanar
    // a = (0,0,0), b=(1,0,0), c=(0,1,0) and d=(1,1,1e-5) are considered as not
    // coplanar
    return (sqr_scalar_triple / normalisation_factor < 1e-11);
}

}  // end namespace MathLib


#include "stdafx.h"
#include "EigenParameterEstimator.h"

#include <iostream>
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;

EigenParameterEstimator::EigenParameterEstimator()
{
}

EigenParameterEstimator::EigenParameterEstimator(MeasurementDB * mdb) : _mdb(mdb)
{
}

EigenParameterEstimator::~EigenParameterEstimator()
{
}

void EigenParameterEstimator::estimateParameters(AbstractSolution * sol, double newrelerr)
{
	int m = _mdb->get_size();
	int n = 2;

	MatrixXd A = MatrixXd(m, n);
	VectorXd b = VectorXd(m);
	VectorXd x = VectorXd(n);

	// Fill values in vector of right-hand side b
	for (int i = 0; i < m; i++)
		b[i] = _mdb->getPairAt(i).second;

	// Zero the solution vector x
	for (int i = 0; i < n; i++)
		x[i] = 0.0;

	// Fill the coefficient matrix
	for (int row = 0; row < m; row++) {
		A(row, 0) = 1.0;
		A(row, 1) = sol->evaluateConstantTermAt(_mdb->getPairAt(row).first);
	}

	x = A.jacobiSvd(ComputeThinU | ComputeThinV).solve(b);

	// Update Solution
	for (int i = 1; i <= n; ++i) {
		sol->updateAt(i - 1, x[i - 1]);
	}
}


#ifndef NONLINEAR_BASIS_HPP
#define NONLINEAR_BASIS_HPP

#include <armadillo>
#include <math.h>

class NonLinearBasisFunction : public Basis {

public:

    int _nX;
    int _nK;
    int _nU;
    int _nM;
    int _nKU;

    NonLinearBasisFunction(int _nX = 7, int _nK = 25, int _nU = 2 , int _nM = 6, int _nKU = 18) : Basis(_nX,_nK,_nU,_nM,_nKU) {
        this->_nX = _nX;
        this->_nK = _nK;
        this->_nU = _nU;
        this->_nM = _nM;
        this->_nKU = _nKU;
    }

    arma::vec fk(const arma::vec & x, const arma::vec & u ) {
        return arma::join_cols( fkx(x), fku(x, u) );
    }

    arma::vec fkx( const arma::vec& x ) {
        return arma::vec({
          x[0],
          x[1],
          x[2],
          x[3],
          x[4],
          x[5],
          1
        });
    }

    arma::vec fku( const arma::vec & x, const arma::vec& u ) {

        return arma::vec({
            u[0],
            u[1],
            u[0]*x[0],
            u[0]*x[1],
            u[0]*x[2],
            u[0]*x[3],
            u[0]*x[4],
            u[0]*x[5],
            u[1]*x[0],
            u[1]*x[1],
            u[1]*x[2],
            u[1]*x[3],
            u[1]*x[4],
            u[1]*x[5],
            u[0]*cos(x[2]),
            u[0]*sin(x[2]),
            u[1]*cos(x[2]),
            u[1]*sin(x[2])
          });

    }

    arma::mat fkudu( const arma::vec & x, const arma::vec & u) {
        return arma::mat({
            {1,0},
            {0,1},
            {x[0],0},
            {x[1],0},
            {x[2],0},
            {x[3],0},
            {x[4],0},
            {x[5],0},
            {0,x[0]},
            {0,x[1]},
            {0,x[2]},
            {0,x[3]},
            {0,x[4]},
            {0,x[5]},
            {cos(x[2]), 0},
            {sin(x[2]), 0},
            {0, cos(x[2])},
            {0, sin(x[2])}
        });
    }

};

#endif


#include "utils.h"

#include <cmath>
#include <fstream>

#include <boost/format.hpp>
#include <boost/algorithm/string.hpp>

#include <Eigen/Dense>

namespace masterthesis {
    double expit(double x) {
        return x > 0 ? 1 / (1 + std::exp(-x)) : 1 - 1 / (1 + std::exp(x));
    }

    double log1exp(double x) {
        return x > 0 ? x + log1p(exp(-x)) : log1p(exp(x));
    }

    void readFeatureFile(std::string path,
                         size_t* const nItems, size_t* const mFeatures,
                         std::vector<double>* const features) {
        std::fstream features_file_input;
        features_file_input.open(path, std::ios::in);

        std::string line;
        std::vector<std::string> tokens;
        getline(features_file_input, line);
        boost::split(tokens, line, boost::is_any_of(","));
        *nItems = stoul(tokens[0]);
        *mFeatures = stoul(tokens[1]);

        features->reserve(*nItems * *mFeatures);
        for (size_t i = 0; i < *nItems; ++i) {
            getline(features_file_input, line);
            boost::split(tokens, line, boost::is_any_of(","));
            for (size_t j = 0; j < *mFeatures; ++j) {
                (*features)[(i * *mFeatures) + j] = stod(tokens[j]);
            }
        }
    }

    void readFeatureFileBoost(std::string path,
                              size_t &nItems, size_t &mFeatures,
                              Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> &features) {
        std::fstream features_file_input;
        features_file_input.open(path, std::ios::in);

        std::string line;
        std::vector<std::string> tokens;
        getline(features_file_input, line);
        boost::split(tokens, line, boost::is_any_of(","));
        nItems = stoul(tokens[0]);
        mFeatures = stoul(tokens[1]);

        features.resize(nItems, mFeatures);
        for (size_t i = 0; i < nItems; ++i) {
            getline(features_file_input, line);
            boost::split(tokens, line, boost::is_any_of(","));
            for (size_t j = 0; j < mFeatures; ++j) {
                features(i, j) = stod(tokens[j]);
            }
        }
    }
}


#include <iostream>
#include <cmath>
#include <random>
#include <chrono>
#include <dlib/matrix.h>

using namespace dlib;
using namespace std;

double nonlin(double x, bool deriv)
{
    if (deriv)
        return x * (1 - x);
    else
        return 1/(1+exp(-x));
}
/*
template<class T> T nonlinT(T x, bool deriv)
{
    if (deriv)
        return x * (1 - x);
    else
        return 1/(1+exp(-x));
}

void foo(std::vector<int> x);

foo({1,2,3,4});
*/

int main()
{
    std::default_random_engine generator;
    std::uniform_real_distribution<double> distribution(-1.0,1.0);

    matrix<double,4,3> X, l0;
    matrix<double,0,1> y, l1, l1_error, l1_delta, l1_tmp;
    matrix<double,3,1> syn;

    y.set_size(4);
    l1.set_size(4);
    l1_error.set_size(4);
    l1_delta.set_size(4);
    l1_tmp.set_size(4);

    X = 0, 0, 1,
        0, 1, 1,
        1, 0, 1,
        1, 1, 1;

    y = 0,
        0,
        1,
        1;

    chrono::steady_clock::time_point begin = chrono::steady_clock::now();

    for (int i=0; i<3; i++)
        syn(i,1) = distribution(generator);

    cout << syn;

    for (int i=0; i<60000; i++)
    {
        l0 = X;
        l1 = l0 * syn;

        for (int i=0; i<4; i++)
            l1(i) = nonlin(l1(i), false);

        l1_error = y - l1;

        for (int i=0; i<4; i++)
            l1_tmp(i) = nonlin(l1(i), true);

        l1_delta = pointwise_multiply(l1_error, l1_tmp);

        syn += trans(l0) * l1_delta;
    }

    chrono::steady_clock::time_point end = chrono::steady_clock::now();
    cout << "Exec: " << chrono::duration_cast<chrono::microseconds> (end - begin).count() << endl;

    cout << "syn: \n" << syn << endl;
    cout << "l1: \n" << l1 << endl;

}