utility.h

#ifndef CVD_UTILITY_H
#define CVD_UTILITY_H

#include <cstdint>

#include <cvd/image.h>
#include <cvd/internal/convert_pixel_types.h>
#include <cvd/internal/pixel_traits.h>
#include <type_traits>

namespace CVD
{ //begin namespace

template <class S, class T>
void copy(const BasicImage<S>& in, BasicImage<T>& out, ImageRef size = ImageRef(-1, -1), ImageRef begin = ImageRef(), ImageRef dst = ImageRef())
{
    if(size.x == -1 && size.y == -1)
        size = in.size();
    // FIXME: This should be an exception, but which one do I use?
    // I refuse to define another "ImageRefNotInImage" in this namespace.
    if(!(in.in_image(begin) && out.in_image(dst) && in.in_image(begin + size - ImageRef(1, 1)) && out.in_image(dst + size - ImageRef(1, 1))))
    {
        std::cerr << "bad copy: " << in.size() << " " << out.size() << " " << size << " " << begin << " " << dst << std::endl;
        int* p = 0;
        *p = 1;
    }
    if(in.size() == out.size() && size == in.size() && begin == ImageRef() && dst == ImageRef())
    {
        convert_image(in, out);
        return;
    }

    const S* from = &in[begin];
    T* to = &out[dst];
    int i = 0;
    while(i++ < size.y)
    {
        Pixel::ConvertPixels<S, T>::convert(from, to, size.x);
        from += in.size().x;
        to += out.size().x;
    }
}

template <class T, bool pod = std::is_pod<T>::value>
struct ZeroPixel
{
    static void zero(T& t)
    {
        for(unsigned int c = 0; c < Pixel::Component<T>::count; c++)
            Pixel::Component<T>::get(t, c) = 0;
    }
};

template <class T>
struct ZeroPixel<T, true>
{
    static void zero(T& t) { memset(&t, 0, sizeof(T)); }
};

template <class T, bool pod = std::is_pod<T>::value>
struct ZeroPixels
{
    static void zero(T* pixels, int count)
    {
        if(count)
        {
            ZeroPixel<T>::zero(*pixels);
            std::fill(pixels + 1, pixels + count, *pixels);
        }
    }
};

template <class T>
struct ZeroPixels<T, true>
{
    static void zero(T* pixels, int count)
    {
        memset(pixels, 0, sizeof(T) * count);
    }
};

template <class T>
inline void zeroPixel(T& pixel) { ZeroPixel<T>::zero(pixel); }

template <class T>
inline void zeroPixels(T* pixels, int count) { ZeroPixels<T>::zero(pixels, count); }

template <class T>
void zeroBorders(BasicImage<T>& I)
{
    if(I.size().y == 0)
        return;
    zeroPixels(I[0], I.size().x);
    for(int r = 0; r < I.size().y - 1; r++)
        zeroPixels(I[r] + I.size().x - 1, 2);
    zeroPixels(I[I.size().y - 1], I.size().x);
}

template <class T>
void fillBorders(BasicImage<T>& im, const T pix, int w = 1)
{
    //Fill the top and bottom
    for(int n = 0; n < w; n++)
        for(int x = 0; x < im.size().x; x++)
        {
            im[n][x] = pix;
            im[im.size().y - 1 - n][x] = pix;
        }

    for(int y = w; y < im.size().y - w; y++)
        for(int n = 0; n < w; n++)
        {
            im[y][n] = pix;
            im[y][im.size().x - 1 - n] = pix;
        }
}

template <class A, class B>
inline void differences(const A* a, const A* b, B* diff, size_t count)
{
    while(count--)
        *(diff++) = (B) * (a++) - (B) * (b++);
}

template <class A, class B, class C>
inline void add_multiple_of_sum(const A* a, const A* b, const C& c, B* out, size_t count)
{
    while(count--)
        *(out++) += (*(a++) + *(b++)) * c;
}

template <class A, class B, class C>
inline void assign_multiple(const A* a, const B& c, C* out, size_t count)
{
    while(count--)
        *(out++) = static_cast<C>(*(a++) * c);
}

template <class T>
double inner_product(const T* a, const T* b, size_t count)
{
    double dot = 0;
    while(count--)
        dot += *(a++) * *(b++);
    return dot;
}

template <class R, class D, class T>
struct SumSquaredDifferences
{
    static inline R sum_squared_differences(const T* a, const T* b, size_t count)
    {
        R ssd = 0;
        while(count--)
        {
            D d = *a++ - *b++;
            ssd += d * d;
        }
        return ssd;
    }
};

template <class T1, class T2>
inline void square(const T1* in, T2* out, size_t count)
{
    while(count--)
    {
        *(out++) = static_cast<T2>(*in * *in);
        ++in;
    }
}

template <class T1, class T2>
inline void subtract_square(const T1* in, T2* out, size_t count)
{
    while(count--)
    {
        *(out++) -= static_cast<T2>(*in * *in);
        ++in;
    }
}

template <class T>
inline double sum_squared_differences(const T* a, const T* b, size_t count)
{
    return SumSquaredDifferences<double, double, T>::sum_squared_differences(a, b, count);
}

template <int bytes>
bool is_aligned(const void* ptr);
template <>
inline bool is_aligned<8>(const void* ptr) { return ((reinterpret_cast<size_t>(ptr)) & 0x7) == 0; }
template <>
inline bool is_aligned<16>(const void* ptr) { return ((reinterpret_cast<size_t>(ptr)) & 0xF) == 0; }

template <int A, class T>
inline size_t steps_to_align(const T* ptr)
{
    return is_aligned<A>(ptr) ? 0 : (A - ((reinterpret_cast<size_t>(ptr)) & (A - 1))) / sizeof(T);
}

void differences(const byte* a, const byte* b, short* diff, unsigned int size);
void differences(const short* a, const short* b, short* diff, unsigned int size);

void differences(const float* a, const float* b, float* diff, size_t size);
void add_multiple_of_sum(const float* a, const float* b, const float& c, float* out, size_t count);
void assign_multiple(const float* a, const float& c, float* out, size_t count);
double inner_product(const float* a, const float* b, size_t count);
double sum_squared_differences(const float* a, const float* b, size_t count);
void square(const float* in, float* out, size_t count);
void subtract_square(const float* in, float* out, size_t count);

void differences(const int32_t* a, const int32_t* b, int32_t* diff, size_t size);
void differences(const double* a, const double* b, double* diff, size_t size);
void add_multiple_of_sum(const double* a, const double* b, const double& c, double* out, size_t count);
void assign_multiple(const double* a, const double& c, double* out, size_t count);
double inner_product(const double* a, const double* b, size_t count);
double sum_squared_differences(const double* a, const double* b, size_t count);
long long sum_squared_differences(const byte* a, const byte* b, size_t count);

}

#endif