bool checkDistance(Particle& p1, Particle& p2){ //checks whether the distance between p1 and p2 is less than 1
double dist = 0, relDist;
for ( int i = 0; i < dim; i++){ //loop over all dimensions
    relDist = p1.pos[i] - p2.pos[i];
    if (fabs(relDist) > halfSize){      //max separation in one dimension is half of system size with PBC
        if(relDist < 0){ //shift the relative distance to the shortest possible one (minimum image convention)
            relDist += systemSize;
        }
        else {
            relDist -= systemSize;
        }
    }
    dist += relDist * relDist;
 }
 return (dist < 1);
}

vector< vector< double> > System::calcForces(vector<vector<double> >& noise){ //calculate the force working on all particles in my system, adds a noise to this force (which is made in other function)

    vector<vector<double> > forces (numberOfParticles, vector<double>(dim,0.0)); //contains the force working on every particle

    /* this looks up neighbors with linked cell method*/

    int numberOfCells = systemSize/interactionRange; //number of cells in one dimension. This way, all particles within interactionRange from a certain particle are in the same cell and in the neighboring cells.
    vector<vector<int> > header(numberOfCells, vector<int> (numberOfCells, -1)); //contains highest particle index for each cell, -1 if no particle
    vector<int> link(numberOfParticles, -1); //gives index of particle in same cell; -1 if no particle
    vector<int> centralCell, neighboringCell; //these contain the indices of particle in central cell and four of its neighbouring cells (in 2D). Don't need to loop over all neighbours for all cells.
    centralCell.reserve(numberOfParticles/(systemSize*systemSize) *2); //this is the amount of ints every cell will on average contain, probably unnecessary to reserve
    neighboringCell.reserve(numberOfParticles/(systemSize*systemSize) *2* 4);

    vector<vector<double> > unitVectors (numberOfParticles, vector<double>(dim)); //create unit vector of velocities for all particles, needed for later force calculation

    double norm = 0.0;
    double dotProduct = 0.0; //needed for force calculation

    for(int i = 0; i < numberOfParticles; i++){
        for(int j = 0; j < dim; j++){
            norm += particleList[i].vel[j] * particleList[i].vel[j];
        }
        norm = sqrt(norm);
        for (int j = 0; j < dim; j++){
            unitVectors[i][j] = particleList[i].vel[j] / norm;
        }
        norm = 0.0;
    }

    vector<vector<double> > averages(numberOfParticles, vector<double>(dim,0.0)); //contains average velocities of neighboring particles
    vector<int> neighborCount(numberOfParticles,0); //number of neighbours for every particle
    for (int i = 0; i < numberOfParticles; i++){  //fill header and link
        int xIndex = numberOfCells * particleList[i].pos[0] / systemSize;
        int yIndex = numberOfCells * particleList[i].pos[1] / systemSize;
        link[i] = header[xIndex][yIndex];
        header[xIndex][yIndex] = i;
    }


    for (int i = 0; i < numberOfCells; i++){ //only need to check with 4 cells. Uses PBC
        for (int j = 0; j < numberOfCells; j++){
            int tempIndex = header[i][j];
            while (tempIndex > -1){ //fill vector with particles in central cell
                centralCell.emplace_back(tempIndex);
                tempIndex = link[tempIndex];
            }

            //fill vector with particles in 4 of the neighboring cells. The '%'s are used for periodic boundary conditions.
            tempIndex = header[(i+1) % numberOfCells][j];
            while (tempIndex > -1){
                neighboringCell.emplace_back(tempIndex);
                tempIndex = link[tempIndex];
            }

            tempIndex = header[(i+1) % numberOfCells][(j+1) % numberOfCells];
            while (tempIndex > -1){
                neighboringCell.emplace_back(tempIndex);
                tempIndex = link[tempIndex];
            }

            tempIndex = header[i][(j+1) % numberOfCells];
            while (tempIndex > -1){
                neighboringCell.emplace_back(tempIndex);
                tempIndex = link[tempIndex];
            }

            tempIndex = header[(i+1) % numberOfCells][((j-1) % numberOfCells + numberOfCells) % numberOfCells];
            while (tempIndex > -1){
                neighboringCell.emplace_back(tempIndex);
                tempIndex = link[tempIndex];
            }

            //calculate distance between particles in central cell + particles in neighboring cell. if smaller than interaction range, add to neighbors
            for (unsigned int k = 0; k < centralCell.size(); k++){
                neighborCount[centralCell[k]] +=1;
                for (int d = 0; d < dim; d++){
                    averages[centralCell[k]][d] += unitVectors[centralCell[k]][d];
                }
                for(unsigned int l = 0; l < k; l++){
                    if(checkDistance(particleList[centralCell[k]], particleList[centralCell[l]])){
                        neighborCount[centralCell[k]] +=1;
                        neighborCount[centralCell[l]] +=1;
                        for (int d = 0; d < dim; d++){
                            averages[centralCell[k]][d] += unitVectors[centralCell[l]][d];
                            averages[centralCell[l]][d] += unitVectors[centralCell[k]][d];
                        }
                    }
                }
            }

            for(unsigned int k = 0; k < centralCell.size(); k++){
                for(unsigned int l = 0; l < neighboringCell.size(); l++){
                    if(checkDistance(particleList[centralCell[k]], particleList[neighboringCell[l]])){
                        neighborCount[centralCell[k]] +=1;
                        neighborCount[neighboringCell[l]] +=1;
                        for (int d = 0; d < dim; d++){
                            averages[centralCell[k]][d] += unitVectors[neighboringCell[l]][d];
                            averages[neighboringCell[l]][d] += unitVectors[centralCell[k]][d];
                        }
                    }
                }
            }
            centralCell.clear();
            neighboringCell.clear();
        }
    }

    int numberOfNeighbors;
    for(int i = 0; i < numberOfParticles; i++){ //calculate social force + friction  + noise
        for (int k = 0; k < dim; k++){
            forces[i][k] = noise[i][k] - particleList[i].vel[k]; //stochastic + friction
        }
        if(neighborCount[i] != 0){
            numberOfNeighbors = neighborCount[i];
            for(int k = 0; k < dim; k++){
                dotProduct += averages[i][k]/numberOfNeighbors * unitVectors[i][k];
            }

            for (int k = 0; k < dim; k++){
                forces[i][k] += couplingFactor * (averages[i][k]/numberOfNeighbors - unitVectors[i][k] * dotProduct);
            }
            dotProduct = 0.0;
        }
    }
    return forces;
  }

class Vector2D {
    // ...methods, etc.

    float x;
    float y;
};

class Vector3D {
    // ...methods, etc.

    float x;
    float y;
    float z;
};

for (int i = 0; i < numberOfParticles; i++) {
    unitVectors[i] = particleList[i].normalize();
}

//calculate distance between particles in central cell + particles in neighboring cell. if smaller than interaction range, add to neighbors
        for (unsigned int k = 0; k < centralCell.size(); k++){
            neighborCount[centralCell[k]] +=1;
            for (int d = 0; d < dim; d++){
                averages[centralCell[k]][d] += unitVectors[centralCell[k]][d];
            }
            for(unsigned int l = 0; l < k; l++){
                if(checkDistance(particleList[centralCell[k]], particleList[centralCell[l]])){
                    neighborCount[centralCell[k]] +=1;
                    neighborCount[centralCell[l]] +=1;
                    for (int d = 0; d < dim; d++){
                        averages[centralCell[k]][d] += unitVectors[centralCell[l]][d];
                        averages[centralCell[l]][d] += unitVectors[centralCell[k]][d];
                    }
                }
            }
        }

for (unsigned int k = 0; k < centralCell.size(); k++)
{
    int index = centralCell[k];
    neighborCount[index] += 1;
    //... etc., replacing centralCell[k] with index everywhere
}

int neighborIndexes[4] = {
    header[i][j],
    header[(i+1) % numberOfCells][j],
    header[i][(j+1) % numberOfCells],
    header[(i+1) % numberOfCells][((j-1) % numberOfCells + numberOfCells) % numberOfCells]
};

for (int k = 0; k < 4; k++)
{
    int tempIndex = neighborIndexes[k];
    while(tempIndex > -1) {
        neighboringCell.emplace_back(tempIndex);
        tempIndex = link[tempIndex];
    }
}

vector<VectorBase> forces(numberOfParticles);

vector<T> forces(numberOfParticles);

class Vector2D{
public:
   double _x;
   double _y;
   Vector2D(){ _x = 0; _y = 0; }
   Vector2D(double x, double y){ _x = x; _y = y; }
   double norm() const
   {
       return sqrt(_x*_x + _y*_y);
   }

   Vector2D& operator+=(Vector2D v){
       _x += v._x;
       _y += v._y;
    return *this;
   }

   Vector2D& operator-=(Vector2D v){
       _x -= v._x;
       _y -= v._y;
       return *this;
   }

   Vector2D& operator*=(double s) {
       _x *= s;
       _y *= s;
       return *this;
   }

   Vector2D& operator/=(double s) {
       _x /= s;
       _y /= s;
       return *this;
   }
};

inline Vector2D operator+(Vector2D a, Vector2D b) { return a += b; }
inline Vector2D operator-(Vector2D a, Vector2D b) { return a -= b; }
inline Vector2D operator*(Vector2D a, double s) { return a *= s; }
inline Vector2D operator*(Vector2D a, Vector2D b){ return a._x * b._x + a._y * b._y; } //inner product of two vectors
inline Vector2D operator*(double s, Vector2D b) { return b *= s; }
inline Vector2D operator/(Vector2D a, double s) { return a /= s; }
inline Vector2D normalize(Vector2D a){ return a / a.norm(); }

class Particle {
public:
    Vector2D pos;
    Vector2D vel;
};

bool checkDistance(Particle& p1, Particle& p2){
   double dist = 0, relDist;
   relDist = p1.pos._x - p2.pos._x;
   if (fabs(relDist) > systemSize / 2){     //max separation in one dimension is half of system size
       if (relDist < 0){
            relDist += systemSize;
       }
       else {
           relDist -= systemSize;
       }
   }
   dist += relDist * relDist;

   relDist = p1.pos._y - p2.pos._y;
   if (fabs(relDist) > systemSize / 2){     //max separation in one dimension is half of system size
       if (relDist < 0){
           relDist += systemSize;
       }
       else {
           relDist -= systemSize;
       }
   }
   dist += relDist * relDist;

   return (dist < 1);
}

vector<Vector2D> System::calcForces(vector<Vector2D>& noise){

   vector<Vector2D> forces (numberOfParticles); //contains the force working on every particle
   vector<Vector2D> averages(numberOfParticles); //contains average velocities of neighboring particles

/* creates neighborlist with linked cell method*/

   int numberOfCells = systemSize; //number of cells in one dimension. This way, all particles within interactionRange from a certain particle are in the same cell and in the neighboring cells.
   vector<vector<int> > header(numberOfCells, vector<int> (numberOfCells, -1)); //contains highest particle index for each cell, -1 if no particle -> need better option for this
   vector<int> link(numberOfParticles, -1); //gives index of particle in same cell; -1 if no particle
   vector<int> centralCell, neighboringCell;
   centralCell.reserve(numberOfParticles/(systemSize*systemSize) *2);
   neighboringCell.reserve(numberOfParticles/(systemSize*systemSize) *2* 4);

   vector<Vector2D> unitVectors (numberOfParticles); //create unit vector of velocities for all particles
   for(int i = 0; i < numberOfParticles; i++){
       unitVectors[i] = normalize(particleList[i].vel);
   }

   vector<int> neighborCount(numberOfParticles,0);
   for (int i = 0; i < numberOfParticles; i++){  //fill header and link
       int xIndex = particleList[i].pos._x;
       int yIndex = particleList[i].pos._y;
       link[i] = header[xIndex][yIndex];
       header[xIndex][yIndex] = i;
   }


   for (int i = 0; i < numberOfCells; i++){ //only need to check with 4 cells. Uses PBC
       for (int j = 0; j < numberOfCells; j++){

           int tempIndex = header[i][j];
           while (tempIndex > -1){ //fill vector with particles in central cell
               centralCell.emplace_back(tempIndex);
               tempIndex = link[tempIndex];
           }

           int neighborIndexes[4] = {
               header[(i + 1) % numberOfCells][j],
               header[i][(j + 1) % numberOfCells],
               header[(i + 1) % numberOfCells][(j + 1) % numberOfCells],
               header[(i + 1) % numberOfCells][((j - 1) % numberOfCells + numberOfCells) % numberOfCells]
           };

           for (int k = 0; k < 4; k++)
           {
               int tempIndex = neighborIndexes[k];
               while (tempIndex > -1) {
                   neighboringCell.emplace_back(tempIndex);
                   tempIndex = link[tempIndex];
               }
           }

           int index1, index2;
        //calculate distance between particles in central cell + particles in neighboring cell. if smaller than interaction range, add to neighbors
           for (unsigned int k = 0; k < centralCell.size(); k++){
               index1 = centralCell[k];
               neighborCount[index1] +=1;
               averages[index1] += unitVectors[index1];
               for(unsigned int l = 0; l < k; l++){
                   index2 = centralCell[l];
                   if(checkDistance(particleList[index1], particleList[index2])){
                       neighborCount[index1] +=1;
                       neighborCount[index2] +=1;
                       averages[index1] += unitVectors[index2];
                       averages[index2] += unitVectors[index1];
                   }
               }
           }

           for(unsigned int k = 0; k < centralCell.size(); k++){
               index1 = centralCell[k];
               for(unsigned int l = 0; l < neighboringCell.size(); l++){
                   index2 = neighboringCell[l];
                   if(checkDistance(particleList[index1], particleList[index2])){
                       neighborCount[index1] +=1;
                       neighborCount[index2] +=1;
                       averages[index1] += unitVectors[index2];
                       averages[index1] += unitVectors[index2];
                   }
               }
           }
           centralCell.clear();
           neighboringCell.clear();
       }
   }

   double dotProduct = 0.0;

   for(int i = 0; i < numberOfParticles; i++){ //calculate social force + friction  + noise
       forces[i] = noise[i]- particleList[i].vel; //stochastic + friction
       averages[i] /= neighborCount[i] ;
       dotProduct = averages[i] * unitVectors[i];
       forces[i] += couplingFactor * (averages[i]- unitVectors[i] * dotProduct);
       }
   }
   return forces;
  }