/* -------------------------------------------------------------------------- *
 *                                   OpenMM                                   *
 * -------------------------------------------------------------------------- *
 * This is part of the OpenMM molecular simulation toolkit.                   *
 * See https://openmm.org/development.                                        *
 *                                                                            *
 * Portions copyright (c) 2010-2023 Stanford University and the Authors.      *
 * Authors: Peter Eastman                                                     *
 * Contributors:                                                              *
 *                                                                            *
 * Permission is hereby granted, free of charge, to any person obtaining a    *
 * copy of this software and associated documentation files (the "Software"), *
 * to deal in the Software without restriction, including without limitation  *
 * the rights to use, copy, modify, merge, publish, distribute, sublicense,   *
 * and/or sell copies of the Software, and to permit persons to whom the      *
 * Software is furnished to do so, subject to the following conditions:       *
 *                                                                            *
 * The above copyright notice and this permission notice shall be included in *
 * all copies or substantial portions of the Software.                        *
 *                                                                            *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,   *
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL    *
 * THE AUTHORS, CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,    *
 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR      *
 * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE  *
 * USE OR OTHER DEALINGS IN THE SOFTWARE.                                     *
 * -------------------------------------------------------------------------- */

#include "openmm/internal/AssertionUtilities.h"
#include "openmm/Context.h"
#include "openmm/CustomExternalForce.h"
#include "openmm/HarmonicBondForce.h"
#include "openmm/LocalEnergyMinimizer.h"
#include "openmm/NonbondedForce.h"
#include "openmm/VerletIntegrator.h"
#include "openmm/VirtualSite.h"
#include "sfmt/SFMT.h"
#include <algorithm>
#include <iostream>
#include <vector>

using namespace OpenMM;
using namespace std;

void testHarmonicBonds() {
    const int numParticles = 10;
    System system;
    HarmonicBondForce* bonds = new HarmonicBondForce();
    system.addForce(bonds);

    // Create a chain of particles connected by harmonic bonds.

    vector<Vec3> positions(numParticles);
    for (int i = 0; i < numParticles; i++) {
        system.addParticle(1.0);
        positions[i] = Vec3(i, 0, 0);
        if (i > 0)
            bonds->addBond(i-1, i, 1+0.1*i, 1);
    }

    // Minimize it and check that all bonds are at their equilibrium distances.

    VerletIntegrator integrator(0.01);
    Context context(system, integrator, platform);
    context.setPositions(positions);
    LocalEnergyMinimizer::minimize(context, 1e-5);
    State state = context.getState(State::Positions);
    for (int i = 1; i < numParticles; i++) {
        Vec3 delta = state.getPositions()[i]-state.getPositions()[i-1];
        ASSERT_EQUAL_TOL(1+0.1*i, sqrt(delta.dot(delta)), 1e-4);
    }
}

void testLargeSystem() {
    const int numMolecules = 25;
    const int numParticles = numMolecules*2;
    const double cutoff = 2.0;
    const double boxSize = 4.0;
    const double tolerance = 15;
    System system;
    system.setDefaultPeriodicBoxVectors(Vec3(boxSize, 0, 0), Vec3(0, boxSize, 0), Vec3(0, 0, boxSize));
    NonbondedForce* nonbonded = new NonbondedForce();
    nonbonded->setCutoffDistance(cutoff);
    nonbonded->setNonbondedMethod(NonbondedForce::CutoffPeriodic);
    system.addForce(nonbonded);

    // Create a cloud of molecules.

    OpenMM_SFMT::SFMT sfmt;
    init_gen_rand(0, sfmt);
    vector<Vec3> positions(numParticles);
    for (int i = 0; i < numMolecules; i++) {
        system.addParticle(1.0);
        system.addParticle(1.0);
        nonbonded->addParticle(-1.0, 0.2, 0.2);
        nonbonded->addParticle(1.0, 0.2, 0.2);
        positions[2*i] = Vec3(boxSize*genrand_real2(sfmt), boxSize*genrand_real2(sfmt), boxSize*genrand_real2(sfmt));
        positions[2*i+1] = Vec3(positions[2*i][0]+1.0, positions[2*i][1], positions[2*i][2]);
        system.addConstraint(2*i, 2*i+1, 1.0);
    }

    // Minimize it and verify that the energy has decreased.
    
    VerletIntegrator integrator(0.01);
    Context context(system, integrator, platform);
    context.setPositions(positions);
    State initialState = context.getState(State::Forces | State::Energy);
    LocalEnergyMinimizer::minimize(context, tolerance);
    State finalState = context.getState(State::Forces | State::Energy | State::Positions);
    ASSERT(finalState.getPotentialEnergy() < initialState.getPotentialEnergy());

    // Compute the force magnitude, subtracting off any component parallel to a constraint, and
    // check that it satisfies the requested tolerance.

    double forceNorm = 0.0;
    for (int i = 0; i < numParticles; i += 2) {
        Vec3 dir = finalState.getPositions()[i+1]-finalState.getPositions()[i];
        double distance = sqrt(dir.dot(dir));
        dir *= 1.0/distance;
        Vec3 f = finalState.getForces()[i];
        f -= dir*dir.dot(f);
        forceNorm += f.dot(f);
        f = finalState.getForces()[i+1];
        f -= dir*dir.dot(f);
        forceNorm += f.dot(f);
    }
    forceNorm = sqrt(forceNorm/(5*numMolecules));
    ASSERT(forceNorm < 2*tolerance);
}

void testVirtualSites() {
    const int numMolecules = 25;
    const int numParticles = numMolecules*3;
    const double cutoff = 2.0;
    const double boxSize = 4.0;
    const double tolerance = 10;
    System system;
    system.setDefaultPeriodicBoxVectors(Vec3(boxSize, 0, 0), Vec3(0, boxSize, 0), Vec3(0, 0, boxSize));
    NonbondedForce* nonbonded = new NonbondedForce();
    nonbonded->setCutoffDistance(cutoff);
    nonbonded->setNonbondedMethod(NonbondedForce::CutoffPeriodic);
    system.addForce(nonbonded);

    // Create a cloud of molecules.

    OpenMM_SFMT::SFMT sfmt;
    init_gen_rand(0, sfmt);
    vector<Vec3> positions(numParticles);
    for (int i = 0; i < numMolecules; i++) {
        system.addParticle(1.0);
        system.addParticle(1.0);
        system.addParticle(0.0);
        nonbonded->addParticle(-1.0, 0.2, 0.2);
        nonbonded->addParticle(0.5, 0.2, 0.2);
        nonbonded->addParticle(0.5, 0.2, 0.2);
        positions[3*i] = Vec3(boxSize*genrand_real2(sfmt), boxSize*genrand_real2(sfmt), boxSize*genrand_real2(sfmt));
        positions[3*i+1] = Vec3(positions[3*i][0]+1.0, positions[3*i][1], positions[3*i][2]);
        positions[3*i+2] = Vec3();
        system.addConstraint(3*i, 3*i+1, 1.0);
        system.setVirtualSite(3*i+2, new TwoParticleAverageSite(3*i, 3*i+1, 0.5, 0.5));
    }

    // Minimize it and verify that the energy has decreased.
    
    VerletIntegrator integrator(0.01);
    Context context(system, integrator, platform);
    context.setPositions(positions);
    context.applyConstraints(1e-5);
    State initialState = context.getState(State::Forces | State::Energy);
    LocalEnergyMinimizer::minimize(context, tolerance);
    State finalState = context.getState(State::Forces | State::Energy | State::Positions);
    ASSERT(finalState.getPotentialEnergy() < initialState.getPotentialEnergy());

    // Compute the force magnitude, subtracting off any component parallel to a constraint, and
    // check that it satisfies the requested tolerance.

    double forceNorm = 0.0;
    for (int i = 0; i < numParticles; i += 3) {
        Vec3 dir = finalState.getPositions()[i+1]-finalState.getPositions()[i];
        double distance = sqrt(dir.dot(dir));
        dir *= 1.0/distance;
        Vec3 f = finalState.getForces()[i];
        f -= dir*dir.dot(f);
        forceNorm += f.dot(f);
        f = finalState.getForces()[i+1];
        f -= dir*dir.dot(f);
        forceNorm += f.dot(f);
        
        // Check the virtual site location.
        
        ASSERT_EQUAL_VEC((finalState.getPositions()[i+1]+finalState.getPositions()[i])*0.5, finalState.getPositions()[i+2], 1e-5);
    }
    forceNorm = sqrt(forceNorm/(5*numMolecules));
    ASSERT(forceNorm < 2*tolerance);
}

void testLargeForces() {
    // Create a set of particles that are almost on top of each other so the initial
    // forces are huge.
    
    const int numParticles = 10;
    System system;
    NonbondedForce* nonbonded = new NonbondedForce();
    system.addForce(nonbonded);
    for (int i = 0; i < numParticles; i++) {
        system.addParticle(1.0);
        nonbonded->addParticle(1.0, 0.2, 1.0);
    }
    vector<Vec3> positions(numParticles);
    OpenMM_SFMT::SFMT sfmt;
    init_gen_rand(0, sfmt);
    for (int i = 0; i < numParticles; i++)
        positions[i] = Vec3(genrand_real2(sfmt), genrand_real2(sfmt), genrand_real2(sfmt))*1e-10;

    // Minimize it and verify that it didn't blow up.                                                                               

    VerletIntegrator integrator(0.01);
    Context context(system, integrator, platform);
    context.setPositions(positions);
    LocalEnergyMinimizer::minimize(context, 1.0);
    State state = context.getState(State::Positions);
    double maxdist = 0.0;
    for (int i = 0; i < numParticles; i++) {
        Vec3 r = state.getPositions()[i];
        maxdist = max(maxdist, sqrt(r.dot(r)));
    }
    ASSERT(maxdist > 0.1);
    ASSERT(maxdist < 10.0);
}

void testForceGroups() {
    // Create a system with two forces, only one of which is in the standard
    // integration force groups.

    System system;
    system.addParticle(1.0);
    system.addParticle(1.0);
    HarmonicBondForce* bonds1 = new HarmonicBondForce();
    HarmonicBondForce* bonds2 = new HarmonicBondForce();
    system.addForce(bonds1);
    system.addForce(bonds2);
    bonds1->addBond(0, 1, 2.0, 1);
    bonds2->addBond(0, 1, 4.0, 1);
    bonds1->setForceGroup(1);
    bonds2->setForceGroup(2);

    // Minimize it and check that the bond has the correct length.

    VerletIntegrator integrator(0.01);
    integrator.setIntegrationForceGroups(1<<1);
    Context context(system, integrator, platform);
    context.setPositions({Vec3(0, 0, 0), Vec3(5, 0, 0)});
    LocalEnergyMinimizer::minimize(context, 1e-5);
    State state = context.getState(State::Positions);
    Vec3 delta = state.getPositions()[0]-state.getPositions()[1];
    ASSERT_EQUAL_TOL(2.0, sqrt(delta.dot(delta)), 1e-4);
}

void testMasslessParticles() {
    // Create a system with massless particles, some of which are involved in constraints.
    
    const int numParticles = 10;
    System system;
    HarmonicBondForce* force = new HarmonicBondForce();
    system.addForce(force);
    vector<Vec3> positions(numParticles);
    OpenMM_SFMT::SFMT sfmt;
    init_gen_rand(0, sfmt);
    for (int i = 0; i < numParticles; i++) {
        system.addParticle(i < 3 || i == 5 ? 0.0 : 1.0);
        positions[i] = Vec3(i, 0.1*genrand_real2(sfmt), 0.1*genrand_real2(sfmt));
    }
    for (int i = 0; i < numParticles-1; i++) {
        if (i < 2 || i == 6)
            system.addConstraint(i, i+1, 1.05);
        else
            force->addBond(i, i+1, 1.05, 100.0);
    }
    
    // Minimize it and check that massless particles have not moved, while other
    // constraints are satisfied.

    VerletIntegrator integrator(0.01);
    Context context(system, integrator, platform);
    context.setPositions(positions);
    LocalEnergyMinimizer::minimize(context, 1e-5);
    State state = context.getState(State::Positions);
    for (int i = 0; i < numParticles; i++)
        if (system.getParticleMass(i) == 0)
            ASSERT_EQUAL_VEC(positions[i], state.getPositions()[i], 1e-6);
    Vec3 delta = state.getPositions()[6]-state.getPositions()[7];
    ASSERT_EQUAL_TOL(1.05, sqrt(delta.dot(delta)), 1e-4);
}

void testReporter() {
    const int numParticles = 30;
    System system;
    CustomExternalForce* force = new CustomExternalForce("sin(5*x)+cos(2*y)*(sin(3*z)+1.5)");
    system.addForce(force);
    vector<Vec3> positions;
    for (int i = 0; i < numParticles; i++) {
        system.addParticle(1.0);
        force->addParticle(i);
        positions.push_back(Vec3(0.5*i, 0.3*sin(i), 0.2*cos(i)));
        if (i > 0)
            system.addConstraint(i-1, i, 1.0);
    }
    VerletIntegrator integrator(0.01);
    Context context(system, integrator, platform);
    context.setPositions(positions);
    context.applyConstraints(1e-5);

    class Reporter : public MinimizationReporter {
    public:
        int lastIter = 0;
        double lastK = 0, lastEnergy = 0;
        bool canceled = false, success = true;
        bool report(int iteration, const vector<double>& x, const vector<double>& grad, map<string, double>& args) {
            double k = args["restraint strength"];
            if (iteration > 0)
                success &= (iteration == lastIter+1 && k == lastK) | (iteration == 0 && k > lastK);
            if (canceled)
                success &= (iteration == 0 && k > lastK);
            lastEnergy = args["system energy"];
            if (iteration > 300 && args["max constraint error"] > 1e-4) {
                canceled = true;
                return true;
            }
            canceled = false;
            lastIter = iteration;
            lastK = k;
            return false;
        }
    };

    Reporter reporter;
    LocalEnergyMinimizer::minimize(context, 1.0, 0, &reporter);
    ASSERT(reporter.success);
    State state = context.getState(State::Energy);
    ASSERT_EQUAL_TOL(state.getPotentialEnergy(), reporter.lastEnergy, 1e-5);
}

void runPlatformTests();

int main(int argc, char* argv[]) {
    try {
        initializeTests(argc, argv);
        testHarmonicBonds();
        testLargeSystem();
        testVirtualSites();
        testLargeForces();
        testForceGroups();
        testMasslessParticles();
        testReporter();
        runPlatformTests();
    }
    catch(const exception& e) {
        cout << "exception: " << e.what() << endl;
        return 1;
    }
    cout << "Done" << endl;
    return 0;
}