#include "HybridVector.h"
 #include "../math/Math.h"

 #include "../math/SIMD.h"
 

 #define SIMD_PAD(x) (gaps::simd::Index::increment() + \
     gaps::simd::Index::increment() * ((x) / gaps::simd::Index::increment())) 
 

 HybridVector::HybridVector(unsigned sz)

     :

 mIndexBitFlags(sz / 64 + 1, 0),

 mData(SIMD_PAD(sz), 0.f),

 mSize(sz)

 {
     GAPS_ASSERT(mData.size() % gaps::simd::Index::increment() == 0);
 }

 
 HybridVector::HybridVector(const std::vector<float> &v)
     :
 mIndexBitFlags(v.size() / 64 + 1, 0),

 mData(SIMD_PAD(v.size()), 0.f),

 mSize(v.size())

 {

     GAPS_ASSERT(mData.size() % gaps::simd::Index::increment() == 0);
 
     for (unsigned i = 0; i < mSize; ++i)

     {
         mData[i] = v[i];
         if (v[i] > 0.f)
         {
             mIndexBitFlags[i / 64] ^= (1ull << (i % 64));
         }
     }
 }
 
 bool HybridVector::empty() const
 {
     for (unsigned i = 0; i < mIndexBitFlags.size(); ++i)
     {
         if (mIndexBitFlags[i] != 0)
         {
             return false;
         }
     }
     return true;
 }
 
 unsigned HybridVector::size() const
 {

     return mSize;

 }
 
 bool HybridVector::add(unsigned i, float v)
 {
     if (mData[i] + v < gaps::epsilon)
     {
         mIndexBitFlags[i / 64] ^= (1ull << (i % 64));
         mData[i] = 0.f;
         return true;
     }
     else
     {
         mIndexBitFlags[i / 64] |= (1ull << (i % 64));
         mData[i] += v;
         return false;
     }
 }
 
 float HybridVector::operator[](unsigned i) const
 {

     GAPS_ASSERT(i < mSize);

     return mData[i];
 }
 
 const float* HybridVector::densePtr() const
 {
     return &(mData[0]);
 }
 

 Archive& operator<<(Archive &ar, const HybridVector &vec)

 {

     ar << vec.mSize;

     for (unsigned i = 0; i < vec.mIndexBitFlags.size(); ++i)
     {
         ar << vec.mIndexBitFlags[i];
     }
 

     for (unsigned i = 0; i < vec.mSize; ++i)

     {
         ar << vec.mData[i];
     }
     return ar;
 }
 
 Archive& operator>>(Archive &ar, HybridVector &vec)
 {
     unsigned sz = 0;
     ar >> sz;

     GAPS_ASSERT(sz == vec.size());

 
     for (unsigned i = 0; i < vec.mIndexBitFlags.size(); ++i)
     {
         ar >> vec.mIndexBitFlags[i];
     }
 

     for (unsigned i = 0; i < vec.mSize; ++i)

     {
         ar >> vec.mData[i];
     }
     return ar;
 }