struct particle{
    floattype y;
    domain_t x;
    domain_t v;
    domain_t local_best_x;
    floattype local_best_y; 
};

struct particle_swarm
{
    static const int NParticles = 8192;
    typedef std::vector<particle> particles_cont_t;
    particles_cont_t particles;
    domain_t gbest_x;
    typedef floattype (* pfitness_func_t)(const domain_t&);

    pfitness_func_t pfitness_function;

    void init(const domain_t& iv, float r ,pfitness_func_t pf){
        particles.resize(NParticles);
        for (unsigned int i=0;i<particles.size();++i){
            particle & p = particles[i];
            p.local_best_x.resize(XN);
            p.x.resize(XN);
            p.v.resize(XN);
            std::fill(p.v.begin(),p.v.end(),(floattype)0.0);
            floattype rnd = rand()*r/(floattype)RAND_MAX;
            for(int j=0;j<XN;++j){p.x[j] = iv[j]+(rnd-r/2);}
            //for(int j=0;j<XN;++j){p.x[j] = iv[j]+i*0.1;}
            p.y = pf(p.x);
            p.local_best_x = p.x;
            p.local_best_y = p.y;
            gbest_x.resize(XN);
        }
        pfitness_function = pf;
    //    srand((unsigned int)time(0));
    }

    int optimize()
    {
        static const floattype c1 = 1.1f;
        static const floattype c2 = 2.03f;
        static const  floattype w = (floattype)0.67;
        floattype min_y = (floattype)1e20;

        for (unsigned int i=0;i<particles.size();++i){
            floattype y = pfitness_function(particles[i].x);
            if(y<min_y){
                min_y =  y;
                gbest_x = particles[i].x;
            }
        }
        int cnt = 0;
        omp_lock_t my_lock;
        omp_init_lock(&my_lock);


        int best_i = -1;
#ifdef OMP_ENABLED
#pragma omp parallel num_threads(NTHREADS) shared(min_y) shared(best_i) private(cnt)
        {
#endif        
        cnt = 0;
        while (cnt++<max_iter)
        {
            best_i = -1;
            particle* ppart = &particles[0];
            int nthr = omp_get_thread_num();
            for (unsigned int k=0;k<NParticles/NTHREADS;++k)
            {
                const int i= nthr*(NParticles/NTHREADS)+k;
                particle& part = particles[i];
                for(unsigned int j=0;j<XN;++j)
                {
                    const floattype r1 = (floattype)(rand()+1)/(RAND_MAX+1);
                    const floattype r2 = (floattype)(rand()+1)/(RAND_MAX+1);
                    part.v[j] = w*part.v[j]+
                        c1*r1*(part.local_best_x[j]  - part.x[j])+
                        c2*r2*(gbest_x[j] - part.x[j]);
                    part.x[j] += part.v[j];
                }
                part.y = pfitness_function(part.x);

                if(part.y < part.local_best_y){
                    part.local_best_y = part.y ;
                    part.local_best_x = part.x ;
                }    

                omp_set_lock(&my_lock);
                if(part.y<min_y){
                    
                    min_y = part.y;
                    //gbest_x = part.x;
                    best_i = i;
                }

                omp_unset_lock(&my_lock);
            }

            if(nthr == 0)
            {
                if(best_i!=-1){
                    gbest_x = particles[best_i].x;
                }
            }
#pragma omp barrier
        }
#ifdef OMP_ENABLED
        }
#endif
        return cnt;
    }