utility_helpers.h

#ifndef CVD_INCLUDE_UTILITY_HELPERS_H
#define CVD_INCLUDE_UTILITY_HELPERS_H

#include <cvd/utility.h>
#include <xmmintrin.h>
using namespace std;

namespace CVD{
    template <class F, class T1, class T2, int A, int M> inline void maybe_aligned_differences(const T1* a, const T1* b, T2* c, size_t count)
    {
	if (count < M*2) {
	    F::unaligned_differences(a,b,c,count);
	    return;
	}
	if (!is_aligned<A>(a)) {	    
	    size_t steps = steps_to_align<A>(a);
	    F::unaligned_differences(a,b,c,steps);
	    count -= steps;
	    a += steps;
	    b += steps;
	    c += steps;
	}
	if (!is_aligned<A>(c) || count < M) {
	    F::unaligned_differences(a,b,c,count);
	    return;
	}	
	size_t block = (count/M)*M;
	F::aligned_differences(a,b,c,block);
	if (count > block) {
	    F::unaligned_differences(a+block,b+block,c+block,count-block);
	}
    }    
    
    template <class F, class T1, class T2, int A, int M> inline void maybe_aligned_add_mul_add(const T1* a, const T1* b, const T1& c, T2* out, size_t count)
    {
	if (count < M*2) {
	    F::unaligned_add_mul_add(a,b,c,out,count);
	    return;
	}
	if (!is_aligned<A>(a)) {      
	    size_t steps = steps_to_align<A>(a);
	    F::unaligned_add_mul_add(a,b,c,out,steps);
	    count -= steps;
	    a += steps;
	    b += steps;
	    out += steps;
	    if (count < M || !is_aligned<16>(out)) {
		F::unaligned_add_mul_add(a,b,c,out,count);
		return;
	    }
	}
	else if (count < M || !is_aligned<16>(out)) {
	    F::unaligned_add_mul_add(a,b,c,out,count);
	    return;
	}
	size_t block = (count/M)*M;
	F::aligned_add_mul_add(a,b,c,out,block);
	if (count > block)
	    F::unaligned_add_mul_add(a+block,b+block,c, out+block,count-block);
    }    

    template <class F, class T1, class T2, int A, int M> inline void maybe_aligned_assign_mul(const T1* a, const T1& c, T2* out, size_t count)
    {
	if (count < M*2) {
	    F::unaligned_assign_mul(a,c,out,count);
	    return;
	}
	if (!is_aligned<A>(a)) {      
	    size_t steps = steps_to_align<A>(a);
	    F::unaligned_assign_mul(a,c,out,steps);
	    count -= steps;
	    a += steps;
	    out += steps;
	    if (count < M) {
		F::unaligned_assign_mul(a,c,out,count);
		return;
	    }
	}
	size_t block = (count/M)*M;
	F::aligned_assign_mul(a,c,out,block);
	if (count > block) {
	    F::unaligned_assign_mul(a+block,c, out+block,count-block);
	}
    }    

    template <class F, class R, class T1, int A, int M> inline R maybe_aligned_inner_product(const T1* a, const T1* b, size_t count)
    {
	if (count < M*2) {
	    return F::unaligned_inner_product(a,b,count);
	}
	R sum = 0;
	if (!is_aligned<A>(a)) {      
	    size_t steps = steps_to_align<A>(a);
	    sum = F::unaligned_inner_product(a,b,steps);
	    count -= steps;
	    a += steps;
	    b += steps;
	    if (count < M) {
		return sum + F::unaligned_inner_product(a,b,count);
	    }
	}
	size_t block = (count/M)*M;
	sum += F::aligned_inner_product(a,b,block);
	if (count > block)
	    sum += F::unaligned_inner_product(a+block,b+block,count-block);
	return sum;
    }    

    template <class F, class R, class T1, int A, int M> inline R maybe_aligned_ssd(const T1* a, const T1* b, size_t count)
    {
	if (count < M*2) {
	    return F::unaligned_ssd(a,b,count);
	}
	R sum = 0;
	if (!is_aligned<A>(a)) {      
	    size_t steps = steps_to_align<A>(a);
	    sum = F::unaligned_ssd(a,b,steps);
	    count -= steps;
	    a += steps;
	    b += steps;
	    if (count < M) {
		return sum + F::unaligned_ssd(a,b,count);
	    }
	}
	size_t block = (count/M)*M;
	sum += F::aligned_ssd(a,b,block);
	if (count > block)
	    sum += F::unaligned_ssd(a+block,b+block,count-block);
	return sum;
    }    

template <class F, class T1, class T2, int A, int M> inline void maybe_aligned_square(const T1* in, T2* out, size_t count)
{
    if (count < M*2) {
	return F::unaligned_square(in,out,count);
    }
    if (!is_aligned<A>(in)) {
	size_t steps = steps_to_align<A>(in);
	F::unaligned_square(in,out,steps);
	count -= steps;
	in += steps;
	out += steps;
	if (count < M) {
	    F::unaligned_square(in,out,count);
	}
    }
    size_t block = (count/M)*M;
    F::aligned_square(in,out,block);
    if (count > block)
	F::unaligned_square(in+block,out+block,count-block);
}    

template <class F, class T1, class T2, int A, int M> inline void maybe_aligned_subtract_square(const T1* in, T2* out, size_t count)
{
    if (count < M*2) {
	return F::unaligned_subtract_square(in,out,count);
    }
    if (!is_aligned<A>(in)) {
	size_t steps = steps_to_align<A>(in);
	F::unaligned_subtract_square(in,out,steps);
	count -= steps;
	in += steps;
	out += steps;
	if (count < M) {
	    F::unaligned_subtract_square(in,out,count);
	}
    }
    size_t block = (count/M)*M;
    F::aligned_subtract_square(in,out,block);
    if (count > block)
	F::unaligned_subtract_square(in+block,out+block,count-block);
}    



}

#endif