DistanceUtils.h

#pragma once

#include <opencv2/core/types_c.h>
#include <type_traits>

template<typename T> static inline typename std::enable_if<std::is_integral<T>::value,size_t>::type L1dist(T a, T b) {
    return (size_t)abs((int)a-b);
}

template<typename T> static inline typename std::enable_if<std::is_floating_point<T>::value,float>::type L1dist(T a, T b) {
    return fabs((float)a-(float)b);
}

template<size_t nChannels, typename T> static inline auto L1dist(const T* a, const T* b) -> decltype(L1dist(*a,*b)) {
    decltype(L1dist(*a,*b)) oResult = 0;
    for(size_t c=0; c<nChannels; ++c)
        oResult += L1dist(a[c],b[c]);
    return oResult;
}

template<size_t nChannels, typename T> static inline auto L1dist(const T* a, const T* b, size_t nElements, const uchar* m=NULL) -> decltype(L1dist<nChannels>(a,b)) {
    decltype(L1dist<nChannels>(a,b)) oResult = 0;
    size_t nTotElements = nElements*nChannels;
    if(m) {
        for(size_t n=0,i=0; n<nTotElements; n+=nChannels,++i)
            if(m[i])
                oResult += L1dist<nChannels>(a+n,b+n);
    }
    else {
        for(size_t n=0; n<nTotElements; n+=nChannels)
            oResult += L1dist<nChannels>(a+n,b+n);
    }
    return oResult;
}

template<typename T> static inline auto L1dist(const T* a, const T* b, size_t nElements, size_t nChannels, const uchar* m=NULL) -> decltype(L1dist<3>(a,b,nElements,m)) {
    CV_Assert(nChannels>0 && nChannels<=4);
    switch(nChannels) {
        case 1: return L1dist<1>(a,b,nElements,m);
        case 2: return L1dist<2>(a,b,nElements,m);
        case 3: return L1dist<3>(a,b,nElements,m);
        case 4: return L1dist<4>(a,b,nElements,m);
        default: return 0;
    }
}

template<size_t nChannels, typename T> static inline auto L1dist_(const cv::Vec<T,nChannels>& a, const cv::Vec<T,nChannels>& b) -> decltype(L1dist<nChannels,T>((T*)(0),(T*)(0))) {
    T a_array[nChannels], b_array[nChannels];
    for(size_t c=0; c<nChannels; ++c) {
        a_array[c] = a[(int)c];
        b_array[c] = b[(int)c];
    }
    return L1dist<nChannels>(a_array,b_array);
}


template<typename T> static inline auto L2sqrdist(T a, T b) -> decltype(L1dist(a,b)) {
    auto oResult = L1dist(a,b);
    return oResult*oResult;
}

template<size_t nChannels, typename T> static inline auto L2sqrdist(const T* a, const T* b) -> decltype(L2sqrdist(*a,*b)) {
    decltype(L2sqrdist(*a,*b)) oResult = 0;
    for(size_t c=0; c<nChannels; ++c)
        oResult += L2sqrdist(a[c],b[c]);
    return oResult;
}

template<size_t nChannels, typename T> static inline auto L2sqrdist(const T* a, const T* b, size_t nElements, const uchar* m=NULL) -> decltype(L2sqrdist<nChannels>(a,b)) {
    decltype(L2sqrdist<nChannels>(a,b)) oResult = 0;
    size_t nTotElements = nElements*nChannels;
    if(m) {
        for(size_t n=0,i=0; n<nTotElements; n+=nChannels,++i)
            if(m[i])
                oResult += L2sqrdist<nChannels>(a+n,b+n);
    }
    else {
        for(size_t n=0; n<nTotElements; n+=nChannels)
            oResult += L2sqrdist<nChannels>(a+n,b+n);
    }
    return oResult;
}

template<typename T> static inline auto L2sqrdist(const T* a, const T* b, size_t nElements, size_t nChannels, const uchar* m=NULL) -> decltype(L2sqrdist<3>(a,b,nElements,m)) {
    CV_Assert(nChannels>0 && nChannels<=4);
    switch(nChannels) {
        case 1: return L2sqrdist<1>(a,b,nElements,m);
        case 2: return L2sqrdist<2>(a,b,nElements,m);
        case 3: return L2sqrdist<3>(a,b,nElements,m);
        case 4: return L2sqrdist<4>(a,b,nElements,m);
        default: return 0;
    }
}

template<size_t nChannels, typename T> static inline auto L2sqrdist_(const cv::Vec<T,nChannels>& a, const cv::Vec<T,nChannels>& b) -> decltype(L2sqrdist<nChannels,T>((T*)(0),(T*)(0))) {
    T a_array[nChannels], b_array[nChannels];
    for(size_t c=0; c<nChannels; ++c) {
        a_array[c] = a[(int)c];
        b_array[c] = b[(int)c];
    }
    return L2sqrdist<nChannels>(a_array,b_array);
}

template<size_t nChannels, typename T> static inline float L2dist(const T* a, const T* b) {
    decltype(L2sqrdist(*a,*b)) oResult = 0;
    for(size_t c=0; c<nChannels; ++c)
        oResult += L2sqrdist(a[c],b[c]);
    return sqrt((float)oResult);
}

template<size_t nChannels, typename T> static inline float L2dist(const T* a, const T* b, size_t nElements, const uchar* m=NULL) {
    decltype(L2sqrdist<nChannels>(a,b)) oResult = 0;
    size_t nTotElements = nElements*nChannels;
    if(m) {
        for(size_t n=0,i=0; n<nTotElements; n+=nChannels,++i)
            if(m[i])
                oResult += L2sqrdist<nChannels>(a+n,b+n);
    }
    else {
        for(size_t n=0; n<nTotElements; n+=nChannels)
            oResult += L2sqrdist<nChannels>(a+n,b+n);
    }
    return sqrt((float)oResult);
}

template<typename T> static inline float L2dist(const T* a, const T* b, size_t nElements, size_t nChannels, const uchar* m=NULL) {
    CV_Assert(nChannels>0 && nChannels<=4);
    switch(nChannels) {
        case 1: return L2dist<1>(a,b,nElements,m);
        case 2: return L2dist<2>(a,b,nElements,m);
        case 3: return L2dist<3>(a,b,nElements,m);
        case 4: return L2dist<4>(a,b,nElements,m);
        default: return 0;
    }
}

template<size_t nChannels, typename T> static inline float L2dist_(const cv::Vec<T,nChannels>& a, const cv::Vec<T,nChannels>& b) {
    T a_array[nChannels], b_array[nChannels];
    for(size_t c=0; c<nChannels; ++c) {
        a_array[c] = a[(int)c];
        b_array[c] = b[(int)c];
    }
    return L2dist<nChannels>(a_array,b_array);
}


template<size_t nChannels, typename T> static inline typename std::enable_if<std::is_integral<T>::value,size_t>::type cdist(const T* curr, const T* bg) {
    static_assert(nChannels>1,"cdist: requires more than one channel");
    bool bNonConstDist = false;
    bool bNonNullDist = (curr[0]!=bg[0]);
    bool bNonNullBG = (bg[0]>0);
    for(size_t c=1; c<nChannels; ++c) {
        bNonConstDist |= (curr[c]!=curr[c-1]) || (bg[c]!=bg[c-1]);
        bNonNullDist |= (curr[c]!=bg[c]);
        bNonNullBG |= (bg[c]>0);
    }
    if(!bNonConstDist || !bNonNullDist)
        return 0;
    if(!bNonNullBG) {
        size_t nulldist = 0;
        for(size_t c=0; c<nChannels; ++c)
            nulldist += curr[c];
        return nulldist;
    }
    size_t curr_sqr = 0;
    size_t bg_sqr = 0;
    size_t mix = 0;
    for(size_t c=0; c<nChannels; ++c) {
        curr_sqr += curr[c]*curr[c];
        bg_sqr += bg[c]*bg[c];
        mix += curr[c]*bg[c];
    }
    return (size_t)sqrt((float)(curr_sqr-(mix*mix)/bg_sqr));
}

template<size_t nChannels, typename T> static inline typename std::enable_if<std::is_floating_point<T>::value,float>::type cdist(const T* curr, const T* bg) {
    static_assert(nChannels>1,"cdist: requires more than one channel");
    bool bNonConstDist = false;
    bool bNonNullDist = (curr[0]!=bg[0]);
    bool bNonNullBG = (bg[0]>0);
    for(size_t c=1; c<nChannels; ++c) {
        bNonConstDist |= (curr[c]!=curr[c-1]) || (bg[c]!=bg[c-1]);
        bNonNullDist |= (curr[c]!=bg[c]);
        bNonNullBG |= (bg[c]>0);
    }
    if(!bNonConstDist || !bNonNullDist)
        return 0;
    if(!bNonNullBG) {
        float nulldist = 0;
        for(size_t c=0; c<nChannels; ++c)
            nulldist += curr[c];
        return nulldist;
    }
    float curr_sqr = 0;
    float bg_sqr = 0;
    float mix = 0;
    for(size_t c=0; c<nChannels; ++c) {
        curr_sqr += (float)curr[c]*curr[c];
        bg_sqr += (float)bg[c]*bg[c];
        mix += (float)curr[c]*bg[c];
    }
    if(curr_sqr<(mix*mix)/bg_sqr)
        return 0;
    else
        return (float)sqrt(curr_sqr-(mix*mix)/bg_sqr);
}

template<size_t nChannels, typename T> static inline auto cdist(const T* a, const T* b, size_t nElements, const uchar* m=NULL) -> decltype(cdist<nChannels>(a,b)) {
    decltype(cdist<nChannels>(a,b)) oResult = 0;
    size_t nTotElements = nElements*nChannels;
    if(m) {
        for(size_t n=0,i=0; n<nTotElements; n+=nChannels,++i)
            if(m[i])
                oResult += cdist<nChannels>(a+n,b+n);
    }
    else {
        for(size_t n=0; n<nTotElements; n+=nChannels)
            oResult += cdist<nChannels>(a+n,b+n);
    }
    return oResult;
}

template<typename T> static inline auto cdist(const T* a, const T* b, size_t nElements, size_t nChannels, const uchar* m=NULL) -> decltype(cdist<3>(a,b,nElements,m)) {
    CV_Assert(nChannels>0 && nChannels<=4);
    switch(nChannels) {
        case 2: return cdist<2>(a,b,nElements,m);
        case 3: return cdist<3>(a,b,nElements,m);
        case 4: return cdist<4>(a,b,nElements,m);
        default: return 0;
    }
}

template<size_t nChannels, typename T> static inline auto cdist_(const cv::Vec<T,nChannels>& a, const cv::Vec<T,nChannels>& b) -> decltype(cdist<nChannels,T>((T*)(0),(T*)(0))) {
    T a_array[nChannels], b_array[nChannels];
    for(size_t c=0; c<nChannels; ++c) {
        a_array[c] = a[(int)c];
        b_array[c] = b[(int)c];
    }
    return cdist<nChannels>(a_array,b_array);
}

template<typename T> static inline T cmixdist(T oL1Distance, T oCDistortion) {
    return (oL1Distance/2+oCDistortion*4);
}

template<size_t nChannels, typename T> static inline typename std::enable_if<std::is_integral<T>::value,size_t>::type cmixdist(const T* curr, const T* bg) {
    return cmixdist(L1dist<nChannels>(curr,bg),cdist<nChannels>(curr,bg));
}

template<size_t nChannels, typename T> static inline typename std::enable_if<std::is_floating_point<T>::value,float>::type cmixdist(const T* curr, const T* bg) {
    return cmixdist(L1dist<nChannels>(curr,bg),cdist<nChannels>(curr,bg));
}


static const uchar popcount_LUT8[256] = {
    0, 1, 1, 2, 1, 2, 2, 3, 1, 2, 2, 3, 2, 3, 3, 4,
    1, 2, 2, 3, 2, 3, 3, 4, 2, 3, 3, 4, 3, 4, 4, 5,
    1, 2, 2, 3, 2, 3, 3, 4, 2, 3, 3, 4, 3, 4, 4, 5,
    2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6,
    1, 2, 2, 3, 2, 3, 3, 4, 2, 3, 3, 4, 3, 4, 4, 5,
    2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6,
    2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6,
    3, 4, 4, 5, 4, 5, 5, 6, 4, 5, 5, 6, 5, 6, 6, 7,
    1, 2, 2, 3, 2, 3, 3, 4, 2, 3, 3, 4, 3, 4, 4, 5,
    2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6,
    2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6,
    3, 4, 4, 5, 4, 5, 5, 6, 4, 5, 5, 6, 5, 6, 6, 7,
    2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6,
    3, 4, 4, 5, 4, 5, 5, 6, 4, 5, 5, 6, 5, 6, 6, 7,
    3, 4, 4, 5, 4, 5, 5, 6, 4, 5, 5, 6, 5, 6, 6, 7,
    4, 5, 5, 6, 5, 6, 6, 7, 5, 6, 6, 7, 6, 7, 7, 8,
};

template<typename T> static inline size_t popcount(T x) {
    size_t nBytes = sizeof(T);
    size_t nResult = 0;
    for(size_t l=0; l<nBytes; ++l)
        nResult += popcount_LUT8[(uchar)(x>>l*8)];
    return nResult;
}

template<typename T> static inline size_t hdist(T a, T b) {
    return popcount(a^b);
}

template<typename T> static inline size_t gdist(T a, T b) {
    return L1dist(popcount(a),popcount(b));
}

template<size_t nChannels, typename T> static inline size_t popcount(const T* x) {
    size_t nBytes = sizeof(T);
    size_t nResult = 0;
    for(size_t c=0; c<nChannels; ++c)
        for(size_t l=0; l<nBytes; ++l)
            nResult += popcount_LUT8[(uchar)(*(x+c)>>l*8)];
    return nResult;
}

template<size_t nChannels, typename T> static inline size_t hdist(const T* a, const T* b) {
    T xor_array[nChannels];
    for(size_t c=0; c<nChannels; ++c)
        xor_array[c] = a[c]^b[c];
    return popcount<nChannels>(xor_array);
}

template<size_t nChannels, typename T> static inline size_t gdist(const T* a, const T* b) {
    return L1dist(popcount<nChannels>(a),popcount<nChannels>(b));
}