text: // This file is part of the uSTL library, an STL implementation.
  //
  // Copyright (c) 2005-2009 by Mike Sharov <msharov@users.sourceforge.net>
  // This file is free software, distributed under the MIT License.
  
  #ifndef NDEBUG	// Optimized code here. asserts slow it down, and are checked elsewhere.
  #define NDEBUG
  #endif
  
  #include "ualgo.h"
  
  namespace ustl {
  
  // Generic version for implementing fill_nX_fast on non-i386 architectures.
  template <typename T> static inline void stosv (T*& p, size_t n, T v)
      { while (n--) *p++ = v; }
  
  #if defined(__i386__) || defined(__x86_64__)
  
  //----------------------------------------------------------------------
  // Copy functions
  //----------------------------------------------------------------------
  
  #if __GNUC__ >= 3
  static inline void movsb_dir_up (void) INLINE;
  static inline void movsb_dir_down (void) INLINE;
  static inline void movsb (const void*& src, size_t nBytes, void*& dest) INLINE;
  static inline void movsd (const void*& src, size_t nWords, void*& dest) INLINE;
  #endif
  
  static inline void movsb_dir_up (void) { asm volatile ("cld"); }
  static inline void movsb_dir_down (void) { asm volatile ("std"); }
  
  static inline void movsb (const void*& src, size_t nBytes, void*& dest)
  {
      asm volatile ("rep;\n\tmovsb"
  	: "=&S"(src), "=&D"(dest), "=&c"(nBytes)
  	: "0"(src), "1"(dest), "2"(nBytes)
  	: "memory");
  }
  
  static inline void movsd (const void*& src, size_t nWords, void*& dest)
  {
      asm volatile ("rep;\n\tmovsl"
  	: "=&S"(src), "=&D"(dest), "=&c"(nWords)
  	: "0"(src), "1"(dest), "2"(nWords)
  	: "memory");
  }
  
  template <> inline void stosv (uint8_t*& p, size_t n, uint8_t v)
  { asm volatile ("rep;\n\tstosb" : "=&D"(p), "=c"(n) : "0"(p), "1"(n), "a"(v) : "memory"); }
  template <> inline void stosv (uint16_t*& p, size_t n, uint16_t v)
  { asm volatile ("rep;\n\tstosw" : "=&D"(p), "=c"(n) : "0"(p), "1"(n), "a"(v) : "memory"); }
  template <> inline void stosv (uint32_t*& p, size_t n, uint32_t v)
  { asm volatile ("rep;\n\tstosl" : "=&D"(p), "=c"(n) : "0"(p), "1"(n), "a"(v) : "memory"); }
  
  #if CPU_HAS_MMX
  #define MMX_ALIGN	16U	// Data must be aligned on this grain
  #define MMX_BS		32U	// Assembly routines copy data this many bytes at a time.
  
  static inline void simd_block_copy (const void* src, void* dest) INLINE;
  static inline void simd_block_store (uint8_t* dest) INLINE;
  static inline void simd_block_cleanup (void) INLINE;
  
  static inline void simd_block_copy (const void* src, void* dest)
  {
      const char* csrc ((const char*) src);
      char* cdest ((char*) dest);
      #if CPU_HAS_SSE
      asm (
  	"movaps\t%2, %%xmm0	\n\t"
  	"movaps\t%3, %%xmm1	\n\t"
  	"movntps\t%%xmm0, %0	\n\t"
  	"movntps\t%%xmm1, %1"
  	: "=m"(cdest[0]), "=m"(cdest[16])
  	: "m"(csrc[0]), "m"(csrc[16])
  	: "xmm0", "xmm1", "memory");
      #else
      asm (
  	"movq	%4, %%mm0	\n\t"
  	"movq	%5, %%mm1	\n\t"
  	"movq	%6, %%mm2	\n\t"
  	"movq	%7, %%mm3	\n\t"
  	"movq	%%mm0, %0	\n\t"
  	"movq	%%mm1, %1	\n\t"
  	"movq	%%mm2, %2	\n\t"
  	"movq	%%mm3, %3"
  	: "=m"(cdest[0]), "=m"(cdest[8]), "=m"(cdest[16]), "=m"(cdest[24])
  	: "m"(csrc[0]), "m"(csrc[8]), "m"(csrc[16]), "m"(csrc[24])
  	: "mm0", "mm1", "mm2", "mm3", "st", "st(1)", "st(2)", "st(3)", "memory");
      #endif
  }
  
  static inline void simd_block_store (uint8_t* dest)
  {
      #if CPU_HAS_SSE
      asm volatile (
  	"movntq %%mm0, %0\n\t"
  	"movntq %%mm0, %1\n\t"
  	"movntq %%mm0, %2\n\t"
  	"movntq %%mm0, %3"
  	: "=m"(dest[0]), "=m"(dest[8]), "=m"(dest[16]), "=m"(dest[24])
  	:: "memory");
      #else
      asm volatile (
  	"movq %%mm0, %0	\n\t"
  	"movq %%mm0, %1	\n\t"
  	"movq %%mm0, %2	\n\t"
  	"movq %%mm0, %3"
  	: "=m"(dest[0]), "=m"(dest[8]), "=m"(dest[16]), "=m"(dest[24])
  	:: "memory");
      #endif
  }
  
  static inline void simd_block_cleanup (void)
  {
      #if !CPU_HAS_SSE
  	simd::reset_mmx();
      #endif
      asm volatile ("sfence");
  }
  
  /// The fastest optimized raw memory copy.
  void copy_n_fast (const void* src, size_t nBytes, void* dest) throw()
  {
      movsb_dir_up();
      size_t nHeadBytes = Align(uintptr_t(src), MMX_ALIGN) - uintptr_t(src);
      nHeadBytes = min (nHeadBytes, nBytes);
      movsb (src, nHeadBytes, dest);
      nBytes -= nHeadBytes;
      if (!(uintptr_t(dest) % MMX_ALIGN)) {
  	const size_t nMiddleBlocks = nBytes / MMX_BS;
  	for (uoff_t i = 0; i < nMiddleBlocks; ++ i) {
  	    prefetch (advance (src, 512), 0, 0);
  	    simd_block_copy (src, dest);
  	    src = advance (src, MMX_BS);
  	    dest = advance (dest, MMX_BS);
  	}
  	simd_block_cleanup();
  	nBytes %= MMX_BS;
      }
      movsb (src, nBytes, dest);
  }
  #endif // CPU_HAS_MMX
  
  /// The fastest optimized backwards raw memory copy.
  void copy_backward_fast (const void* first, const void* last, void* result) throw()
  {
      prefetch (first, 0, 0);
      prefetch (result, 1, 0);
      size_t nBytes (distance (first, last));
      movsb_dir_down();
      size_t nHeadBytes = uintptr_t(last) % 4;
      last = advance (last, -1);
      result = advance (result, -1);
      movsb (last, nHeadBytes, result);
      nBytes -= nHeadBytes;
      if (uintptr_t(result) % 4 == 3) {
  	const size_t nMiddleBlocks = nBytes / 4;
  	last = advance (last, -3);
  	result = advance (result, -3);
  	movsd (last, nMiddleBlocks, result);
  	nBytes %= 4;
      }
      movsb (last, nBytes, result);
      movsb_dir_up();
  }
  #endif // __i386__
  
  //----------------------------------------------------------------------
  // Fill functions
  //----------------------------------------------------------------------
  
  #if CPU_HAS_MMX
  template <typename T> static inline void build_block (T) {}
  template <> inline void build_block (uint8_t v)
  {
      asm volatile (
  	"movd %0, %%mm0\n\tpunpcklbw %%mm0, %%mm0\n\tpshufw $0, %%mm0, %%mm0"
  	: : "g"(uint32_t(v)) : "mm0");
  }
  template <> inline void build_block (uint16_t v)
  {
      asm volatile (
  	"movd %0, %%mm0\n\tpshufw $0, %%mm0, %%mm0"
  	: : "g"(uint32_t(v)) : "mm0");
  }
  template <> inline void build_block (uint32_t v)
  {
      asm volatile (
  	"movd %0, %%mm0\n\tpunpckldq %%mm0, %%mm0"
  	: : "g"(uint32_t(v)) : "mm0");
  }
  
  static inline void simd_block_fill_loop (uint8_t*& dest, size_t count)
  {
      prefetch (advance (dest, 512), 1, 0);
      for (const uint8_t* destEnd = dest + count * MMX_BS; dest < destEnd; dest += MMX_BS)
  	simd_block_store (dest);
      simd_block_cleanup();
      simd::reset_mmx();
  }
  
  template <typename T>
  static inline void fill_n_fast (T* dest, size_t count, T v)
  {
      size_t nHead = Align(uintptr_t(dest), MMX_ALIGN) - uintptr_t(dest) / sizeof(T);
      nHead = min (nHead, count);
      stosv (dest, nHead, v);
      count -= nHead;
      build_block (v);
      uint8_t* bdest = (uint8_t*) dest;
      simd_block_fill_loop (bdest, count * sizeof(T) / MMX_BS);
      count %= MMX_BS;
      dest = (T*) bdest;
      stosv (dest, count, v);
  }
  
  void fill_n8_fast (uint8_t* dest, size_t count, uint8_t v) throw()
      { fill_n_fast (dest, count, v); }
  void fill_n16_fast (uint16_t* dest, size_t count, uint16_t v) throw()
      { fill_n_fast (dest, count, v); }
  void fill_n32_fast (uint32_t* dest, size_t count, uint32_t v) throw()
      { fill_n_fast (dest, count, v); }
  #else
  void fill_n8_fast (uint8_t* dest, size_t count, uint8_t v) throw() { memset (dest, v, count); }
  void fill_n16_fast (uint16_t* dest, size_t count, uint16_t v) throw() { stosv (dest, count, v); }
  void fill_n32_fast (uint32_t* dest, size_t count, uint32_t v) throw() { stosv (dest, count, v); }
  #endif // CPU_HAS_MMX
  
  /// Exchanges ranges [first, middle) and [middle, last)
  void rotate_fast (void* first, void* middle, void* last) throw()
  {
  #if HAVE_ALLOCA_H
      const size_t half1 (distance (first, middle)), half2 (distance (middle, last));
      const size_t hmin (min (half1, half2));
      if (!hmin)
  	return;
      void* buf = alloca (hmin);
      if (buf) {
  	if (half2 < half1) {
  	    copy_n_fast (middle, half2, buf);
  	    copy_backward_fast (first, middle, last);
  	    copy_n_fast (buf, half2, first);
  	} else {
  	    copy_n_fast (first, half1, buf);
  	    copy_n_fast (middle, half2, first);
  	    copy_n_fast (buf, half1, advance (first, half2));
  	}
      } else
  #else
      if (first == middle || middle == last)
  	return;
  #endif
      {
  	char* f = (char*) first;
  	char* m = (char*) middle;
  	char* l = (char*) last;
  	reverse (f, m);
  	reverse (m, l);
  	while (f != m && m != l)
  	    iter_swap (f++, --l);
  	reverse (f, (f == m ? l : m));
      }
  }
  
  #if __GNUC__ < 4
  size_t popcount (uint32_t v)
  {
      const uint32_t w = v - ((v >> 1) & 0x55555555); // Algorithm from AMD optimization guide
      const uint32_t x = (w & 0x33333333) + ((w >> 2) & 0x33333333);
      return (((x + (x >> 4) & 0x0F0F0F0F) * 0x01010101) >> 24);
  }
  
  #if HAVE_INT64_T
  /// \brief Returns the number of 1s in \p v in binary.
  size_t popcount (uint64_t v)
  {
      v -= (v >> 1) & UINT64_C(0x5555555555555555);		// Algorithm from Wikipedia
      v = (v & UINT64_C(0x3333333333333333)) + ((v >> 2) & UINT64_C(0x3333333333333333));
      v = (v + (v >> 4)) & UINT64_C(0x0F0F0F0F0F0F0F0F);
      return ((v * UINT64_C(0x0101010101010101)) >> 56);
  }
  #endif	// HAVE_INT64_T
  #endif	// !__GNUC__
  
  //----------------------------------------------------------------------
  // Miscellaneous instantiated stuff from headers which don't have enough
  // to warrant creation of a separate file.cc
  //----------------------------------------------------------------------
  
  // Used in uspecial to print printable characters
  const char _FmtPrtChr[2][8]={"'%c'","%d"};
  
  } // namespace ustl
  