#include <iostream>
#include <random>
#include <vector>

#include <boost/math/constants/constants.hpp>
using boost::math::constants::two_pi;


class Particle {
public:
    Particle() = delete;
    Particle(float q, float p)
     : q(q)
     , p(p)
     {}

    float q;
    float p;
};

int main(int argc, char** argv)
{
    auto rng = std::mt19937(std::random_device{}());
    auto normdist = std::normal_distribution<float>(0, 1);

    auto fs = 1e3f;
    auto tstep = 0.5e-6f;
    const auto angle = (fs*tstep)*two_pi<float>();

    auto tau = 0.001;

    auto tmax = 0.01f;

    uint32_t timax = std::ceil(tmax/tstep);

    auto nparts = 1000;

    std::vector<Particle> pvec;

    auto E_p2 = 0.0f;
    auto Ep = 0.0f;

    std::cout << "time" << " "
              << "periods" << " "
              << "particle" << " "
              << "position" << " "
              << "length" << std::endl;

    for (auto i=0; i<nparts; i++) {
        auto q = 2*normdist(rng);
        auto p = 1+normdist(rng);
        pvec.push_back(Particle(q,p));
        Ep += p;
        E_p2 += p*p;
    }
    E_p2 /= nparts;
    Ep /= nparts;
    std::cout << "0" << " "
              << "0" << " "
              << pvec[0].p << " "
              << Ep << " "
              << std::sqrt(E_p2-Ep*Ep) << std::endl;

    for (uint32_t ti=1; ti<=timax; ti++) {
        E_p2 = 0;
        for (auto &p : pvec) {
            auto p_tmp = p.q * std::sin(angle) + p.p * std::cos(angle);
            auto q_tmp = p.q * std::cos(angle) - p.p * std::sin(angle);
            p.p = p_tmp;
            p.q = q_tmp;
            p.p -= p.p*2*tstep/(tau)+2*std::sqrt(tstep/tau)*normdist(rng);
            Ep += p_tmp;
            E_p2 += p_tmp*p_tmp;
        }
        E_p2 /= nparts;
        Ep /= nparts;
        std::cout << ti*tstep << " "
                  << ti*tstep*fs <<  " "
                  << pvec[0].p << " "
                  << Ep << " "
                  << std::sqrt(E_p2-Ep*Ep) << std::endl;
    }

    return EXIT_SUCCESS;
}