update_state<<<numBlocks,blockSize>>>(positions, momenta, particle_number, dt, t);

__global__
void update_state(double *pos, double *mom, unsigned int part_num, double dt, double t)
{
    double dx = 0.001, dp = 0.001; //finite differences
    double H_pplus, H_pminus, H_xplus, H_xminus;
    double p_grad, x_grad;

    int index = threadIdx.x;
    int stride = blockDim.x;

    __syncthreads();
    for (int i = index; i<part_num; i += stride)
    {
        //Calculate phase space gradient
        pos[i] += dx;
        H_xplus = Hamiltonian(pos, mom, part_num, t);
        pos[i] -= 2*dx;
        H_xminus = Hamiltonian(pos, mom, part_num, t);
        pos[i] += dx;
        x_grad = (H_xplus-H_xminus)/(2*dx);

        //Update momentum
        mom[i] -= dt*x_grad;

        mom[i] += dp;
        H_pplus = Hamiltonian(pos, mom, part_num, t);
        mom[i] -= 2*dp;
        H_pminus = Hamiltonian(pos, mom, part_num, t);
        mom[i] += dp;
        p_grad = (H_pplus-H_pminus)/(2*dp);

        //Update position
        pos[i] += dt*p_grad;
    }
}

void write_state(FILE *fp, double t, double *pos, double *mom, unsigned int part_num)
{
    cout << "check 1n";

    //Write time
    fprintf(fp,"%ft",t);

    cout << "check 2n";

    //Write positions
    for (unsigned int i = 0; i<part_num; i++)
    {
        cout << "stuff: "<< pos[i] << endl;
        fprintf(fp ,"%f %f %f",pos[i],0.,0.);
    }

    cout << "check 3n";

    //Write momenta
    for (unsigned int i = 0; i<part_num; i++)
    {
        fprintf(fp,"%f",mom[i]);
    }
fprintf(fp,"n");
}