

C3DFluidVectorfieldRegularizer::C3DFluidVectorfieldRegularizer(float mu, float lambda, 
                                                               size_t maxiter, float omega, float epsilon):
        m_mu(mu), 
        m_lambda(lambda),  
        m_omega(omega), 
        m_epsilon(epsilon),
        m_maxiter(maxiter), 
        m_dx(0), 
        m_dxy(0)
{
        float a = mu;
	float b = lambda + mu;
	m_c = 1 / (6.0f * a + 2.0f * b);
	m_b_4 = 0.25f * b * m_c;
	m_a_b = ( a + b ) * m_c;
	m_a = a * m_c;
}

double C3DFluidVectorfieldRegularizer::do_run(C3DFVectorfield& output, C3DFVectorfield& input) const
{
        // reserve space for temporaries
        T3DDatafield<float> residua(get_size());
        T3DDatafield<unsigned char> update_flags1(get_size());
        T3DDatafield<unsigned char> update_flags2(get_size());
        
        // these two will ping-pong in each update 
        T3DDatafield<unsigned char>& update_flags = update_flags1;
        T3DDatafield<unsigned char>& dset_flags = update_flags2;
        
        float start_residuum = 0.0;
        float residuum;
        float thresh = 0.0;
        
        size_t iter = 0;

        // first iteration runs over the whole field 
        fill(update_flags.begin(), update_flags.end(), 1);

        m_fluid_row_kernel->set_data_fields(output, input, residua);
        
        do {
                iter++; 
                residuum == 0.0f; 


                m_fluid_row_kernel->set_update_fields(update_flags, dset_flags); 
                
                for (unsigned z = 1; z < work_size.z; ++z) {
                        for (unsigned y = 1; y < work_size.y; ++y) {
                                residuum += m_fluid_row_kernel->evaluate_row(y, z);
                        }
                }
                        
                
        } while (iter < m_maxiter && residuum > m_epsilon); 
        
        

        
}


// this moves to the specific solver         

float C3DFluidVectorfieldRegularizer::solve_at(C3DFVector_sse *v, const C3DFVector_sse& b) const 
{
        C3DFVector *vm = v - m_dxy; 

        __m128 	Vp0m1m1 = _mm_loadu_ps(&vm[ -m_dx]);
        __m128 	Vm1p0m1 = _mm_loadu_ps(&vm[ -1   ]);
	__m128 	Vp0p0m1 = _mm_loadu_ps(&vm[  0   ]);
	__m128 	Vp1p0m1 = _mm_loadu_ps(&vm[  1   ]);
	__m128 	Vp0p1m1 = _mm_loadu_ps(&vm[  m_dx]);

        vm = v - m_dx; 

        __m128 	Vm1m1p0 = _mm_loadu_ps(&vm[ -1 ]);
	__m128 	Vp0m1p0 = _mm_loadu_ps(&vm[  0 ]);
	__m128 	Vp1m1p0 = _mm_loadu_ps(&vm[  1 ]);

        __m128 Vp0p0m1 = _mm_loadu_ps(&v[-1]); 
        __m128 Vp0p0p1 = _mm_loadu_ps(&v[1]); 

        vm = v + m_dx; 

        __m128 	Vm1p1p0 = _mm_loadu_ps(&vm[ -1 ]);
	__m128 	Vp0p1p0 = _mm_loadu_ps(&vm[  0 ]);
	__m128 	Vp1p1p0 = _mm_loadu_ps(&vm[  1 ]);
        
        vm = v + m_dxy; 

        __m128 	Vp0m1p1 = _mm_loadu_ps(&vm[ -m_dx]);
        __m128 	Vm1p0p1 = _mm_loadu_ps(&vm[ -1   ]);
	__m128 	Vp0p0p1 = _mm_loadu_ps(&vm[  0   ]);
	__m128 	Vp1p0p1 = _mm_loadu_ps(&vm[  1   ]);
	__m128 	Vp0p1p1 = _mm_loadu_ps(&vm[  m_dx]);
        
        //
        //             V(-1,-1,0).xy - V(1,-1,0).xy + V(1,1,0).xy - V(-1,1,0).xy
        // 
        __m128 vdxy = Vm1m1p0 - Vp1m1p0 + Vp1p1p0 - Vm1p1p0; // only 1 and 2 of interest

        //
        //             V(-1,0,-1).xz - V(1,0,-1).xz + V(1,0,1).xz - V(-1,0,1).xz
        //             
	__m128 vdxz = Vm1p0m1 - Vp1p0m1 + Vp1p0p1 - Vm1p0p1; // only 1 and 3 of interest

        //
        //             V(0,-1,-1).yz - V(0,1,-1).yz + V(0,1,1).yz - V(0,-1,1).yz
        //             
        __m128 vdyz = Vp0m1m1 - Vp0p1m1 + Vp0p1p1  - Vp0m1p1; // only 2 and 3 of interest 
        
        // 
        //            V(1,0,0) + V(-1,0,0)
        // 
        __m128 sxx = Vp1p0p0 + Vm1p0p0; 

        // 
        //            V(0,1,0) + V(0,-1,0)
        // 
        __m128 syy = Vp0p1p0 + Vp0m1p0;
        
        // 
        //            V(0,0,1) + V(0,0,-1)
        // 
	__m128 szz = Vp0p0p1 + Vp0p0m1;

        __m128 vdxx = Vp1p0m0 + Vm1p0p0;
        __m128 vdyy = Vp0p1p0 + Vp0m1p0;
	__m128 vdzz = Vp0p0p1 + Vp0p0m1;

        // a_b * sxx.x + a * (syy.x + szz.x) 
        // a_b * syy.y + a * (sxx.y + szz.y) 
        // a_b * szz.z + a * (sxx.z + syy.z)

        __m128 p1 = sxx; 
        _mm_shuffle_ps(p1, szz, _MM_SHUFFLE(3,2,3,0)); // [0, szz.z, 0, sxx.x] 
        
        __m128 p2 = syy; 
        _mm_shuffle_ps(p2, p2, _MM_SHUFFLE(3,3,1,3)); // [0, 0, syy.y, 0] 
        _mm_add_ps(p1, p2);                            // [0, szz.z, syy.y, sxx.x] 

        __m128 p2 = sxx;
        _mm_shuffle_ps(szz, sxx, _MM_SHUFFLE(3,2,1,0)); // [0, sxx.z, szz.y, szz.x] 
        
        _mm_shuffle_ps(p2, syy, _MM_SHUFFLE(2,0,2,1));  // [syy.z, syy.x, sxx.z, sxx.y]
        _mm_shuffle_ps(p2, p2, _MM_SHUFFLE(0,1,0,2));         // [dc  , syy.z, sxx.y, syy.x]


        auto p = p1 * m_a_b_sse +  m_a_sse * (szz + p2); 
        
        // now q
        //   vdxy[ 0, 0, y, x ],  vdxz[ 0, z, 0, x], vdyz [0, z, 0, x] 
        // 
        // dxy.y + dxz.z   //  
        // dyz.z + dxy.x   
        // dxz.x + dyz.y


        _mm_shuffle_ps(vdxy, vdyz, _MM_SHUFFLE(0, 1, 0, 1)); // [* , vdyz.y, vdxy.x, vdxy.y]
        _mm_shuffle_ps(vdxz, vdyz, _MM_SHUFFLE(2, 0, 2, 1)); // [vdxz.z, vdxz.x, vdyz.z, vdyz.y]
        _mm_shuffle_ps(vdxz, vdxz  _MM_SHUFFLE(0, 2, 1, 3));// [*,vdxz.x, vdyz.z, vdxz.z]
        auto q = vdxz + vdxy; 
        
        auto b_sse = _mm_set_pd(b.x, b.y. b.z, 0.0f); 
        
        auto s = m_sor_sse * (b_sse + p + m_b_4_sse * q);
        auto delta = s - *v; 
        
        *v = s; 
        
        
        __m128 p1 = _mm_shuffle_ps(vdxx, vdyy, _MM_SHUFFLE(0, 1, 1, 0));
        __m128 p2 = _mm_shuffle_ps(vdyy, vdzz, _MM_SHUFFLE(0, 1, 1, 0));
                                   
        p1 = _mm_shuffle_ps(p1, vdzz, _MM_SHUFFLE(0, 2, 2, 0));   // p1 should now contain (xx.x, yy.y, zz.z)
                            
        __m128 p2 = _mm_shuffle_ps(vdxx, vdzz, _MM_SHUFFLE(0, 2, 1, 3));
        

        return sse_norm2(delta);

}

void C3DFluidVectorfieldRegularizer::on_size_changed()
{
        m_dx = get_size().x; 
        m_dxy = m_dx * get_size().y; 
        cvinfo() << "Temporary memory requirement will be " << 
                (4 + 2 + 32) * get_size().product() / (1024 * 1024 * 1024) << " GiB\n"; 
}



