Beginnings of OpenCL implementation of CustomIntegrator

platforms/opencl/src/OpenCLKernelFactory.cpp

@@ -108,6 +108,8 @@ KernelImpl* OpenCLKernelFactory::createKernelImpl(std::string name, const Platfo
         return new OpenCLIntegrateVariableVerletStepKernel(name, platform, cl);
     if (name == IntegrateVariableLangevinStepKernel::Name())
         return new OpenCLIntegrateVariableLangevinStepKernel(name, platform, cl);
+    if (name == IntegrateCustomStepKernel::Name())
+        return new OpenCLIntegrateCustomStepKernel(name, platform, cl);
     if (name == ApplyAndersenThermostatKernel::Name())
         return new OpenCLApplyAndersenThermostatKernel(name, platform, cl);
     if (name == ApplyMonteCarloBarostatKernel::Name())

platforms/opencl/src/OpenCLKernels.cpp

@@ -43,6 +43,7 @@
 #include "lepton/Parser.h"
 #include "lepton/ParsedExpression.h"
 #include "../src/SimTKUtilities/SimTKOpenMMRealType.h"
+#include "../src/SimTKUtilities/SimTKOpenMMUtilities.h"
 #include <cmath>
 #include <set>
 
@@ -71,6 +72,20 @@ static bool isZeroExpression(const Lepton::ParsedExpression& expression) {
     return (dynamic_cast<const Lepton::Operation::Constant&>(op).getValue() == 0.0);
 }
 
+static bool usesVariable(const Lepton::ExpressionTreeNode& node, const string& variable) {
+    const Lepton::Operation& op = node.getOperation();
+    if (op.getId() == Lepton::Operation::VARIABLE && op.getName() == variable)
+        return true;
+    for (int i = 0; i < (int) node.getChildren().size(); i++)
+        if (usesVariable(node.getChildren()[i], variable))
+            return true;
+    return false;
+}
+
+static bool usesVariable(const Lepton::ParsedExpression& expression, const string& variable) {
+    return usesVariable(expression.getRootNode(), variable);
+}
+
 static pair<ExpressionTreeNode, string> makeVariable(const string& name, const string& value) {
     return make_pair(ExpressionTreeNode(new Operation::Variable(name)), value);
 }
@@ -3486,6 +3501,271 @@ void OpenCLIntegrateVariableLangevinStepKernel::execute(ContextImpl& context, co
     cl.setStepCount(cl.getStepCount()+1);
 }
 
+OpenCLIntegrateCustomStepKernel::~OpenCLIntegrateCustomStepKernel() {
+    if (globalValues != NULL)
+        delete globalValues;
+    if (perDofValues != NULL)
+        delete perDofValues;
+}
+
+void OpenCLIntegrateCustomStepKernel::initialize(const System& system, const CustomIntegrator& integrator) {
+    cl.getPlatformData().initializeContexts(system);
+    cl.getIntegrationUtilities().initRandomNumberGenerator(integrator.getRandomNumberSeed());
+    numGlobalVariables = integrator.getNumGlobalVariables();
+    globalValues = new OpenCLArray<cl_float>(cl, max(1, numGlobalVariables), "globalVariables", true);
+    perDofValues = new OpenCLParameterSet(cl, integrator.getNumPerDofVariables(), 3*system.getNumParticles(), "perDofVariables");
+    prevStepSize = -1.0;
+    SimTKOpenMMUtilities::setRandomNumberSeed(integrator.getRandomNumberSeed());
+}
+
+string OpenCLIntegrateCustomStepKernel::createGlobalComputation(const string& variable, const Lepton::ParsedExpression& expr, CustomIntegrator& integrator) {
+    map<string, Lepton::ParsedExpression> expressions;
+    if (variable == "dt")
+        expressions["dt[0].y = "] = expr;
+    else {
+        for (int i = 0; i < integrator.getNumGlobalVariables(); i++)
+            if (variable == integrator.getGlobalVariableName(i))
+                expressions["globals["+intToString(i)+"] = "] = expr;
+    }
+    map<string, string> variables;
+    variables["dt"] = "dt[0].y";
+    variables["uniform"] = "uniform";
+    variables["gaussian"] = "gaussian";
+    variables["energy"] = "energy";
+    for (int i = 0; i < integrator.getNumGlobalVariables(); i++)
+        variables[integrator.getGlobalVariableName(i)] = "globals["+intToString(i)+"]";
+    vector<pair<string, string> > functions;
+    return OpenCLExpressionUtilities::createExpressions(expressions, variables, functions, "temp", "");
+}
+
+string OpenCLIntegrateCustomStepKernel::createPerDofComputation(const string& variable, const Lepton::ParsedExpression& expr, int component, CustomIntegrator& integrator) {
+    const string suffixes[] = {".x", ".y", ".z"};
+    string suffix = suffixes[component];
+    map<string, Lepton::ParsedExpression> expressions;
+    if (variable == "x")
+        expressions["position"+suffix+" = "] = expr;
+    else if (variable == "v")
+        expressions["velocity"+suffix+" = "] = expr;
+    else {
+        for (int i = 0; i < integrator.getNumPerDofVariables(); i++)
+            if (variable == integrator.getPerDofVariableName(i))
+                expressions["perDof"+suffix.substr(1)+perDofValues->getParameterSuffix(i)+" = "] = expr;
+    }
+    map<string, string> variables;
+    variables["x"] = "position"+suffix;
+    variables["v"] = "velocity"+suffix;
+    variables["f"] = "f"+suffix;
+    variables["gaussian"] = "gaussian"+suffix;
+    variables["m"] = "mass";
+    variables["dt"] = "stepSize";
+    variables["energy"] = "energy";
+    for (int i = 0; i < integrator.getNumGlobalVariables(); i++)
+        variables[integrator.getGlobalVariableName(i)] = "globals["+intToString(i)+"]";
+    for (int i = 0; i < integrator.getNumPerDofVariables(); i++)
+        variables[integrator.getPerDofVariableName(i)] = "perDof"+suffix.substr(1)+perDofValues->getParameterSuffix(i);
+    vector<pair<string, string> > functions;
+    return OpenCLExpressionUtilities::createExpressions(expressions, variables, functions, "temp"+intToString(component)+"_", "");
+}
+
+void OpenCLIntegrateCustomStepKernel::execute(ContextImpl& context, CustomIntegrator& integrator, bool& forcesAreValid) {
+    OpenCLIntegrationUtilities& integration = cl.getIntegrationUtilities();
+    int numAtoms = cl.getNumAtoms();
+    int numSteps = integrator.getNumComputations();
+    if (!hasInitializedKernels) {
+        hasInitializedKernels = true;
+        kernels.resize(integrator.getNumComputations());
+        requiredRandoms.resize(integrator.getNumComputations());
+        needsForces.resize(numSteps, false);
+        needsEnergy.resize(numSteps, false);
+        invalidatesForces.resize(numSteps, false);
+        
+        // Build a list of all variables that affect the forces, so we can tell which
+        // steps invalidate them.
+        
+        set<string> affectsForce;
+        affectsForce.insert("x");
+        for (vector<ForceImpl*>::const_iterator iter = context.getForceImpls().begin(); iter != context.getForceImpls().end(); ++iter) {
+            const map<string, double> params = (*iter)->getDefaultParameters();
+            for (map<string, double>::const_iterator param = params.begin(); param != params.end(); ++param)
+                affectsForce.insert(param->first);
+        }
+        map<string, string> defines;
+        defines["NUM_ATOMS"] = intToString(cl.getNumAtoms());
+        
+        // Loop over all steps and create the kernels for them.
+        
+        for (int step = 0; step < numSteps; step++) {
+            CustomIntegrator::ComputationType type;
+            string variable, expression;
+            integrator.getComputationStep(step, type, variable, expression);
+            stepType.push_back(type);
+            invalidatesForces[step] = (type == CustomIntegrator::ConstrainPositions || affectsForce.find(variable) != affectsForce.end());
+            if (type == CustomIntegrator::ComputePerDof) {
+                // Compute a per-DOF value.
+                
+                stringstream compute;
+                for (int i = 0; i < (int) perDofValues->getBuffers().size(); i++) {
+                    const OpenCLNonbondedUtilities::ParameterInfo& buffer = perDofValues->getBuffers()[i];
+                    compute << buffer.getType()<<" perDofx"<<intToString(i+1)<<" = perDofValues"<<intToString(i+1)<<"[3*index];\n";
+                    compute << buffer.getType()<<" perDofy"<<intToString(i+1)<<" = perDofValues"<<intToString(i+1)<<"[3*index+1];\n";
+                    compute << buffer.getType()<<" perDofz"<<intToString(i+1)<<" = perDofValues"<<intToString(i+1)<<"[3*index+2];\n";
+                }
+                Lepton::ParsedExpression expr = Lepton::Parser::parse(expression).optimize();
+                needsForces[step] = usesVariable(expr, "f");
+                needsEnergy[step] = usesVariable(expr, "energy");
+                for (int i = 0; i < 3; i++)
+                    compute << createPerDofComputation(variable, expr, i, integrator);
+                if (variable == "x")
+                    compute << "posq[index] = position;\n";
+                else if (variable == "v")
+                    compute << "velm[index] = velocity;\n";
+                for (int i = 0; i < (int) perDofValues->getBuffers().size(); i++) {
+                    const OpenCLNonbondedUtilities::ParameterInfo& buffer = perDofValues->getBuffers()[i];
+                    compute << "perDofValues"<<intToString(i+1)<<"[3*index] = perDofx"<<intToString(i+1)<<";\n";
+                    compute << "perDofValues"<<intToString(i+1)<<"[3*index+1] = perDofy"<<intToString(i+1)<<";\n";
+                    compute << "perDofValues"<<intToString(i+1)<<"[3*index+2] = perDofz"<<intToString(i+1)<<";\n";
+                }
+                map<string, string> replacements;
+                replacements["COMPUTE_STEP"] = compute.str();
+                stringstream args;
+                for (int i = 0; i < (int) perDofValues->getBuffers().size(); i++) {
+                    const OpenCLNonbondedUtilities::ParameterInfo& buffer = perDofValues->getBuffers()[i];
+                    string valueName = "perDofValues"+intToString(i+1);
+                    args << ", __global " << buffer.getType() << "* restrict " << valueName;
+                }
+                replacements["PARAMETER_ARGUMENTS"] = args.str();
+                cl::Program program = cl.createProgram(cl.replaceStrings(OpenCLKernelSources::customIntegratorPerDof, replacements), defines);
+                cl::Kernel kernel = cl::Kernel(program, "computePerDof");
+                kernels[step].push_back(kernel);
+                if (usesVariable(expr, "uniform"))
+                    throw OpenMMException("OpenCL platform does not currently support per-DOF uniform random numbers");
+                requiredRandoms[step].push_back(numAtoms*usesVariable(expr, "gaussian"));
+                int index = 0;
+                kernel.setArg<cl::Buffer>(index++, cl.getPosq().getDeviceBuffer());
+                kernel.setArg<cl::Buffer>(index++, integration.getPosDelta().getDeviceBuffer());
+                kernel.setArg<cl::Buffer>(index++, cl.getVelm().getDeviceBuffer());
+                kernel.setArg<cl::Buffer>(index++, cl.getForce().getDeviceBuffer());
+                kernel.setArg<cl::Buffer>(index++, integration.getStepSize().getDeviceBuffer());
+                kernel.setArg<cl::Buffer>(index++, globalValues->getDeviceBuffer());
+                kernel.setArg<cl::Buffer>(index++, integration.getRandom().getDeviceBuffer());
+                index += 2;
+                for (int i = 0; i < (int) perDofValues->getBuffers().size(); i++)
+                    kernel.setArg<cl::Memory>(index++, perDofValues->getBuffers()[i].getMemory());
+            }
+            else if (type == CustomIntegrator::ComputeGlobal) {
+                // Compute a global value.
+
+                stringstream compute;
+                Lepton::ParsedExpression expr = Lepton::Parser::parse(expression).optimize();
+                needsEnergy[step] = usesVariable(expr, "energy");
+                compute << createGlobalComputation(variable, expr, integrator);
+                map<string, string> replacements;
+                replacements["COMPUTE_STEP"] = compute.str();
+                stringstream args;
+                cl::Program program = cl.createProgram(cl.replaceStrings(OpenCLKernelSources::customIntegratorGlobal, replacements), defines);
+                cl::Kernel kernel = cl::Kernel(program, "computeGlobal");
+                kernels[step].push_back(kernel);
+                int index = 0;
+                kernel.setArg<cl::Buffer>(index++, integration.getStepSize().getDeviceBuffer());
+                kernel.setArg<cl::Buffer>(index++, globalValues->getDeviceBuffer());
+            }
+        }
+    }
+    if (!deviceValuesAreCurrent) {
+        perDofValues->setParameterValues(localPerDofValues);
+        deviceValuesAreCurrent = true;
+    }
+    localValuesAreCurrent = false;
+    double stepSize = integrator.getStepSize();
+    if (stepSize != prevStepSize) {
+        integration.getStepSize()[0].y = (cl_float) stepSize;
+        integration.getStepSize().upload();
+        prevStepSize = stepSize;
+    }
+
+    // Loop over computation steps in the integrator and execute them.
+
+    for (int i = 0; i < numSteps; i++) {
+        if ((needsForces[i] || needsEnergy[i]) && !forcesAreValid) {
+            // Recompute forces and/or energy.  Figure out what is actually needed
+            // between now and the next time they get invalidated again.
+            
+            bool computeForce = false, computeEnergy = false;
+            for (int j = i; ; j++) {
+                if (needsForces[j])
+                    computeForce = true;
+                if (needsEnergy[j])
+                    computeEnergy = true;
+                if (invalidatesForces[j])
+                    break;
+                if (j == numSteps-1)
+                    j = -1;
+                if (j == i-1)
+                    break;
+            }
+            RealOpenMM e = context.calcForcesAndEnergy(computeForce, computeEnergy);
+            if (computeEnergy)
+                energy = e;
+            forcesAreValid = true;
+        }
+        if (stepType[i] == CustomIntegrator::ComputePerDof) {
+            kernels[i][0].setArg<cl_uint>(7, integration.prepareRandomNumbers(requiredRandoms[i][0]));
+            kernels[i][0].setArg<cl_float>(8, energy);
+            cl.executeKernel(kernels[i][0], numAtoms);
+        }
+        else if (stepType[i] == CustomIntegrator::ComputeGlobal) {
+            kernels[i][0].setArg<cl_float>(2, SimTKOpenMMUtilities::getUniformlyDistributedRandomNumber());
+            kernels[i][0].setArg<cl_float>(3, SimTKOpenMMUtilities::getNormallyDistributedRandomNumber());
+            kernels[i][0].setArg<cl_float>(4, energy);
+            cl.executeKernel(kernels[i][0], 1);
+        }
+        else if (stepType[i] == CustomIntegrator::UpdateContextState)
+            context.updateContextState();
+        if (invalidatesForces[i])
+            forcesAreValid = false;
+    }
+
+    // Update the time and step count.
+
+    cl.setTime(cl.getTime()+stepSize);
+    cl.setStepCount(cl.getStepCount()+1);
+}
+
+void OpenCLIntegrateCustomStepKernel::getGlobalVariables(ContextImpl& context, vector<double>& values) const {
+    globalValues->download();
+    values.resize(numGlobalVariables);
+    for (int i = 0; i < numGlobalVariables; i++)
+        values[i] = globalValues->get(i);
+}
+
+void OpenCLIntegrateCustomStepKernel::setGlobalVariables(ContextImpl& context, const vector<double>& values) {
+    for (int i = 0; i < numGlobalVariables; i++)
+        globalValues->set(i, (float) values[i]);
+    globalValues->upload();
+}
+
+void OpenCLIntegrateCustomStepKernel::getPerDofVariable(ContextImpl& context, int variable, vector<Vec3>& values) const {
+    if (!localValuesAreCurrent) {
+        perDofValues->getParameterValues(localPerDofValues);
+        localValuesAreCurrent = true;
+    }
+    values.resize(perDofValues->getNumObjects()/3);
+    for (int i = 0; i < (int) values.size(); i++)
+        for (int j = 0; j < 3; j++)
+            values[i][j] = localPerDofValues[variable][3*i+j];
+}
+
+void OpenCLIntegrateCustomStepKernel::setPerDofVariable(ContextImpl& context, int variable, const vector<Vec3>& values) {
+    if (!localValuesAreCurrent) {
+        perDofValues->getParameterValues(localPerDofValues);
+        localValuesAreCurrent = true;
+    }
+    for (int i = 0; i < (int) values.size(); i++)
+        for (int j = 0; j < 3; j++)
+            localPerDofValues[variable][3*i+j] = (float) values[i][j];
+    deviceValuesAreCurrent = false;
+}
+
 OpenCLApplyAndersenThermostatKernel::~OpenCLApplyAndersenThermostatKernel() {
     if (atomGroups != NULL)
         delete atomGroups;

platforms/opencl/src/OpenCLKernels.h

@@ -877,6 +877,82 @@ private:
     double prevTemp, prevFriction, prevErrorTol;
 };
 
+/**
+ * This kernel is invoked by CustomIntegrator to take one time step.
+ */
+class OpenCLIntegrateCustomStepKernel : public IntegrateCustomStepKernel {
+public:
+    OpenCLIntegrateCustomStepKernel(std::string name, const Platform& platform, OpenCLContext& cl) : IntegrateCustomStepKernel(name, platform), cl(cl),
+            hasInitializedKernels(false), localValuesAreCurrent(false), globalValues(NULL), perDofValues(NULL) {
+    }
+    ~OpenCLIntegrateCustomStepKernel();
+    /**
+     * Initialize the kernel.
+     * 
+     * @param system     the System this kernel will be applied to
+     * @param integrator the CustomIntegrator this kernel will be used for
+     */
+    void initialize(const System& system, const CustomIntegrator& integrator);
+    /**
+     * Execute the kernel.
+     * 
+     * @param context    the context in which to execute this kernel
+     * @param integrator the CustomIntegrator this kernel is being used for
+     * @param forcesAreValid if the context has been modified since the last time step, this will be
+     *                       false to show that cached forces are invalid and must be recalculated.
+     *                       On exit, this should specify whether the cached forces are valid at the
+     *                       end of the step.
+     */
+    void execute(ContextImpl& context, CustomIntegrator& integrator, bool& forcesAreValid);
+    /**
+     * Get the values of all global variables.
+     *
+     * @param context   the context in which to execute this kernel
+     * @param values    on exit, this contains the values
+     */
+    void getGlobalVariables(ContextImpl& context, std::vector<double>& values) const;
+    /**
+     * Set the values of all global variables.
+     *
+     * @param context   the context in which to execute this kernel
+     * @param values    a vector containing the values
+     */
+    void setGlobalVariables(ContextImpl& context, const std::vector<double>& values);
+    /**
+     * Get the values of a per-DOF variable.
+     *
+     * @param context   the context in which to execute this kernel
+     * @param variable  the index of the variable to get
+     * @param values    on exit, this contains the values
+     */
+    void getPerDofVariable(ContextImpl& context, int variable, std::vector<Vec3>& values) const;
+    /**
+     * Set the values of a per-DOF variable.
+     *
+     * @param context   the context in which to execute this kernel
+     * @param variable  the index of the variable to get
+     * @param values    a vector containing the values
+     */
+    void setPerDofVariable(ContextImpl& context, int variable, const std::vector<Vec3>& values);
+private:
+    std::string createGlobalComputation(const std::string& variable, const Lepton::ParsedExpression& expr, CustomIntegrator& integrator);
+    std::string createPerDofComputation(const std::string& variable, const Lepton::ParsedExpression& expr, int component, CustomIntegrator& integrator);
+    OpenCLContext& cl;
+    double prevStepSize, energy;
+    int numGlobalVariables;
+    bool hasInitializedKernels, deviceValuesAreCurrent;
+    mutable bool localValuesAreCurrent;
+    OpenCLArray<cl_float>* globalValues;
+    OpenCLParameterSet* perDofValues;
+    mutable std::vector<std::vector<cl_float> > localPerDofValues;
+    std::vector<std::vector<cl::Kernel> > kernels;
+    std::vector<CustomIntegrator::ComputationType> stepType;
+    std::vector<bool> needsForces;
+    std::vector<bool> needsEnergy;
+    std::vector<bool> invalidatesForces;
+    std::vector<std::vector<int> > requiredRandoms;
+};
+
 /**
  * This kernel is invoked by AndersenThermostat at the start of each time step to adjust the particle velocities.
  */

platforms/opencl/src/OpenCLPlatform.cpp

@@ -67,6 +67,7 @@ OpenCLPlatform::OpenCLPlatform() {
     registerKernelFactory(IntegrateBrownianStepKernel::Name(), factory);
     registerKernelFactory(IntegrateVariableVerletStepKernel::Name(), factory);
     registerKernelFactory(IntegrateVariableLangevinStepKernel::Name(), factory);
+    registerKernelFactory(IntegrateCustomStepKernel::Name(), factory);
     registerKernelFactory(ApplyAndersenThermostatKernel::Name(), factory);
     registerKernelFactory(ApplyMonteCarloBarostatKernel::Name(), factory);
     registerKernelFactory(CalcKineticEnergyKernel::Name(), factory);

platforms/opencl/src/kernels/customIntegratorGlobal.cl

+__kernel void computeGlobal(__global float2* restrict dt, __global float* restrict globals, float uniform, float gaussian, float energy) {
+    COMPUTE_STEP
+}

platforms/opencl/src/kernels/customIntegratorPerDof.cl

+__kernel void computePerDof(__global float4* restrict posq, __global float4* restrict posDelta, __global float4* restrict velm,
+        __global const float4* restrict force, __global const float2* restrict dt, __global const float* restrict globals,
+        __global const float4* restrict random, unsigned int randomIndex, float energy
+        PARAMETER_ARGUMENTS) {
+    float stepSize = dt[0].y;
+    int index = get_global_id(0);
+    randomIndex += index;
+    while (index < NUM_ATOMS) {
+        float4 position = posq[index];
+        float4 velocity = velm[index];
+        float4 f = force[index];
+        float4 gaussian = random[randomIndex];
+        float mass = 1.0f/velocity.w;
+        COMPUTE_STEP
+        randomIndex += get_global_size(0);
+        index += get_global_size(0);
+    }
+}

platforms/opencl/tests/TestOpenCLCustomIntegrator.cpp

+/* -------------------------------------------------------------------------- *
+ *                                   OpenMM                                   *
+ * -------------------------------------------------------------------------- *
+ * This is part of the OpenMM molecular simulation toolkit originating from   *
+ * Simbios, the NIH National Center for Physics-Based Simulation of           *
+ * Biological Structures at Stanford, funded under the NIH Roadmap for        *
+ * Medical Research, grant U54 GM072970. See https://simtk.org.               *
+ *                                                                            *
+ * Portions copyright (c) 2008-2011 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.                                     *
+ * -------------------------------------------------------------------------- */
+
+/**
+ * This tests the OpenCL implementation of CustomIntegrator.
+ */
+
+#include "../../../tests/AssertionUtilities.h"
+#include "openmm/Context.h"
+#include "OpenCLPlatform.h"
+#include "openmm/AndersenThermostat.h"
+#include "openmm/HarmonicBondForce.h"
+#include "openmm/NonbondedForce.h"
+#include "openmm/System.h"
+#include "openmm/CustomIntegrator.h"
+#include "../src/SimTKUtilities/SimTKOpenMMRealType.h"
+#include "sfmt/SFMT.h"
+#include <iostream>
+#include <vector>
+
+using namespace OpenMM;
+using namespace std;
+
+const double TOL = 1e-5;
+
+/**
+ * Test a simple leapfrog integrator on a single bond.
+ */
+void testSingleBond() {
+    OpenCLPlatform platform;
+    System system;
+    system.addParticle(2.0);
+    system.addParticle(2.0);
+    CustomIntegrator integrator(0.01);
+    integrator.addComputePerDof("v", "v+dt*f/m");
+    integrator.addComputePerDof("x", "x+dt*v");
+    HarmonicBondForce* forceField = new HarmonicBondForce();
+    forceField->addBond(0, 1, 1.5, 1);
+    system.addForce(forceField);
+    Context context(system, integrator, platform);
+    vector<Vec3> positions(2);
+    positions[0] = Vec3(-1, 0, 0);
+    positions[1] = Vec3(1, 0, 0);
+    context.setPositions(positions);
+    
+    // This is simply a harmonic oscillator, so compare it to the analytical solution.
+    
+    const double freq = 1.0;;
+    State state = context.getState(State::Energy);
+    const double initialEnergy = state.getKineticEnergy()+state.getPotentialEnergy();
+    for (int i = 0; i < 1000; ++i) {
+        state = context.getState(State::Positions | State::Velocities | State::Energy);
+        double time = state.getTime();
+        double expectedDist = 1.5+0.5*std::cos(freq*time);
+        ASSERT_EQUAL_VEC(Vec3(-0.5*expectedDist, 0, 0), state.getPositions()[0], 0.02);
+        ASSERT_EQUAL_VEC(Vec3(0.5*expectedDist, 0, 0), state.getPositions()[1], 0.02);
+        double expectedSpeed = -0.5*freq*std::sin(freq*time);
+        ASSERT_EQUAL_VEC(Vec3(-0.5*expectedSpeed, 0, 0), state.getVelocities()[0], 0.02);
+        ASSERT_EQUAL_VEC(Vec3(0.5*expectedSpeed, 0, 0), state.getVelocities()[1], 0.02);
+        double energy = state.getKineticEnergy()+state.getPotentialEnergy();
+        ASSERT_EQUAL_TOL(initialEnergy, energy, 0.01);
+        integrator.step(1);
+    }
+}
+
+/**
+ * Test an integrator that enforces constraints.
+ */
+void testConstraints() {
+    const int numParticles = 8;
+    const double temp = 500.0;
+    OpenCLPlatform platform;
+    System system;
+    CustomIntegrator integrator(0.002);
+    integrator.addPerDofVariable("oldx", 0);
+    integrator.addComputePerDof("v", "v+dt*f/m");
+    integrator.addComputePerDof("oldx", "x");
+    integrator.addComputePerDof("x", "x+dt*v");
+    integrator.addConstrainPositions();
+    integrator.addComputePerDof("v", "(x-oldx)/dt");
+    integrator.setConstraintTolerance(1e-5);
+    NonbondedForce* forceField = new NonbondedForce();
+    for (int i = 0; i < numParticles; ++i) {
+        system.addParticle(i%2 == 0 ? 5.0 : 10.0);
+        forceField->addParticle((i%2 == 0 ? 0.2 : -0.2), 0.5, 5.0);
+    }
+    for (int i = 0; i < numParticles-1; ++i)
+        system.addConstraint(i, i+1, 1.0);
+    system.addForce(forceField);
+    Context context(system, integrator, platform);
+    vector<Vec3> positions(numParticles);
+    vector<Vec3> velocities(numParticles);
+    OpenMM_SFMT::SFMT sfmt;
+    init_gen_rand(0, sfmt);
+
+    for (int i = 0; i < numParticles; ++i) {
+        positions[i] = Vec3(i/2, (i+1)/2, 0);
+        velocities[i] = Vec3(genrand_real2(sfmt)-0.5, genrand_real2(sfmt)-0.5, genrand_real2(sfmt)-0.5);
+    }
+    context.setPositions(positions);
+    context.setVelocities(velocities);
+    
+    // Simulate it and see whether the constraints remain satisfied.
+    
+    double initialEnergy = 0.0;
+    for (int i = 0; i < 1000; ++i) {
+        State state = context.getState(State::Positions | State::Energy);
+        for (int j = 0; j < system.getNumConstraints(); ++j) {
+            int particle1, particle2;
+            double distance;
+            system.getConstraintParameters(j, particle1, particle2, distance);
+            Vec3 p1 = state.getPositions()[particle1];
+            Vec3 p2 = state.getPositions()[particle2];
+            double dist = std::sqrt((p1[0]-p2[0])*(p1[0]-p2[0])+(p1[1]-p2[1])*(p1[1]-p2[1])+(p1[2]-p2[2])*(p1[2]-p2[2]));
+            ASSERT_EQUAL_TOL(distance, dist, 2e-5);
+        }
+        double energy = state.getKineticEnergy()+state.getPotentialEnergy();
+        if (i == 1)
+            initialEnergy = energy;
+        else if (i > 1)
+            ASSERT_EQUAL_TOL(initialEnergy, energy, 0.05);
+        integrator.step(1);
+    }
+}
+
+/**
+ * Test an integrator that applies constraints directly to velocities.
+ */
+void testVelocityConstraints() {
+    const int numParticles = 8;
+    const double temp = 500.0;
+    OpenCLPlatform platform;
+    System system;
+    CustomIntegrator integrator(0.002);
+    integrator.addComputePerDof("v", "v+0.5*dt*f/m");
+    integrator.addComputePerDof("x", "x+dt*v");
+    integrator.addConstrainPositions();
+    integrator.addComputePerDof("v", "v+0.5*dt*f/m");
+    integrator.addConstrainVelocities();
+    integrator.setConstraintTolerance(1e-5);
+    NonbondedForce* forceField = new NonbondedForce();
+    for (int i = 0; i < numParticles; ++i) {
+        system.addParticle(i%2 == 0 ? 5.0 : 10.0);
+        forceField->addParticle((i%2 == 0 ? 0.2 : -0.2), 0.5, 5.0);
+    }
+    for (int i = 0; i < numParticles-1; ++i)
+        system.addConstraint(i, i+1, 1.0);
+    system.addForce(forceField);
+    Context context(system, integrator, platform);
+    vector<Vec3> positions(numParticles);
+    vector<Vec3> velocities(numParticles);
+    OpenMM_SFMT::SFMT sfmt;
+    init_gen_rand(0, sfmt);
+
+    for (int i = 0; i < numParticles; ++i) {
+        positions[i] = Vec3(i/2, (i+1)/2, 0);
+        velocities[i] = Vec3(genrand_real2(sfmt)-0.5, genrand_real2(sfmt)-0.5, genrand_real2(sfmt)-0.5);
+    }
+    context.setPositions(positions);
+    context.setVelocities(velocities);
+    
+    // Simulate it and see whether the constraints remain satisfied.
+    
+    double initialEnergy = 0.0;
+    for (int i = 0; i < 1000; ++i) {
+        State state = context.getState(State::Positions | State::Energy);
+        for (int j = 0; j < system.getNumConstraints(); ++j) {
+            int particle1, particle2;
+            double distance;
+            system.getConstraintParameters(j, particle1, particle2, distance);
+            Vec3 p1 = state.getPositions()[particle1];
+            Vec3 p2 = state.getPositions()[particle2];
+            double dist = std::sqrt((p1[0]-p2[0])*(p1[0]-p2[0])+(p1[1]-p2[1])*(p1[1]-p2[1])+(p1[2]-p2[2])*(p1[2]-p2[2]));
+            ASSERT_EQUAL_TOL(distance, dist, 2e-5);
+        }
+        double energy = state.getKineticEnergy()+state.getPotentialEnergy();
+        if (i == 1)
+            initialEnergy = energy;
+        else if (i > 1)
+            ASSERT_EQUAL_TOL(initialEnergy, energy, 0.05);
+        integrator.step(2);
+    }
+}
+
+/**
+ * Test an integrator with an AndersenThermostat to see if updateContextState()
+ * is being handled correctly.
+ */
+void testWithThermostat() {
+    const int numParticles = 8;
+    const double temp = 100.0;
+    const double collisionFreq = 10.0;
+    const int numSteps = 10000;
+    OpenCLPlatform platform;
+    System system;
+    CustomIntegrator integrator(0.005);
+    integrator.addUpdateContextState();
+    integrator.addComputePerDof("v", "v+dt*f/m");
+    integrator.addComputePerDof("x", "x+dt*v");
+    NonbondedForce* forceField = new NonbondedForce();
+    for (int i = 0; i < numParticles; ++i) {
+        system.addParticle(2.0);
+        forceField->addParticle((i%2 == 0 ? 1.0 : -1.0), 1.0, 5.0);
+    }
+    system.addForce(forceField);
+    AndersenThermostat* thermostat = new AndersenThermostat(temp, collisionFreq);
+    system.addForce(thermostat);
+    Context context(system, integrator, platform);
+    vector<Vec3> positions(numParticles);
+    for (int i = 0; i < numParticles; ++i)
+        positions[i] = Vec3((i%2 == 0 ? 2 : -2), (i%4 < 2 ? 2 : -2), (i < 4 ? 2 : -2));
+    context.setPositions(positions);
+    
+    // Let it equilibrate.
+    
+    integrator.step(10000);
+    
+    // Now run it for a while and see if the temperature is correct.
+    
+    double ke = 0.0;
+    for (int i = 0; i < numSteps; ++i) {
+        State state = context.getState(State::Energy);
+        ke += state.getKineticEnergy();
+        integrator.step(1);
+    }
+    ke /= numSteps;
+    double expected = 0.5*numParticles*3*BOLTZ*temp;
+    ASSERT_USUALLY_EQUAL_TOL(expected, ke, 6/std::sqrt((double) numSteps));
+}
+
+/**
+ * Test a Monte Carlo integrator that uses global variables and depends on energy.
+ */
+void testMonteCarlo() {
+    OpenCLPlatform platform;
+    System system;
+    system.addParticle(1.0);
+    system.addParticle(1.0);
+    CustomIntegrator integrator(0.1);
+    const double kT = BOLTZ*300.0;
+    integrator.addGlobalVariable("kT", kT);
+    integrator.addGlobalVariable("oldE", 0);
+    integrator.addGlobalVariable("accept", 0);
+    integrator.addPerDofVariable("oldx", 0);
+    integrator.addComputeGlobal("oldE", "energy");
+    integrator.addComputePerDof("oldx", "x");
+    integrator.addComputePerDof("x", "x+dt*gaussian");
+    integrator.addComputeGlobal("accept", "step(exp((oldE-energy)/kT)-uniform)");
+    integrator.addComputePerDof("x", "accept*x + (1-accept)*oldx");
+    HarmonicBondForce* forceField = new HarmonicBondForce();
+    forceField->addBond(0, 1, 2.0, 10.0);
+    system.addForce(forceField);
+    Context context(system, integrator, platform);
+    vector<Vec3> positions(2);
+    positions[0] = Vec3(-1, 0, 0);
+    positions[1] = Vec3(1, 0, 0);
+    context.setPositions(positions);
+    
+    // Compute the histogram of distances and see if it satisfies a Boltzmann distribution.
+    
+    const int numBins = 100;
+    const double maxDist = 4.0;
+    const int numIterations = 5000;
+    vector<int> counts(numBins, 0);
+    for (int i = 0; i < numIterations; ++i) {
+        integrator.step(10);
+        State state = context.getState(State::Positions);
+        Vec3 delta = state.getPositions()[0]-state.getPositions()[1];
+        double dist = sqrt(delta.dot(delta));
+        if (dist < maxDist)
+            counts[(int) (numBins*dist/maxDist)]++;
+    }
+    vector<double> expected(numBins, 0);
+    double sum = 0;
+    for (int i = 0; i < numBins; i++) {
+        double dist = (i+0.5)*maxDist/numBins;
+        expected[i] = dist*dist*exp(-5.0*(dist-2)*(dist-2)/kT);
+        sum += expected[i];
+    }
+    for (int i = 0; i < numBins; i++)
+        ASSERT_USUALLY_EQUAL_TOL((double) counts[i]/numIterations, expected[i]/sum, 0.01);
+}
+
+int main() {
+    try {
+        testSingleBond();
+//        testConstraints();
+//        testVelocityConstraints();
+        testWithThermostat();
+        testMonteCarlo();
+    }
+    catch(const exception& e) {
+        cout << "exception: " << e.what() << endl;
+        return 1;
+    }
+    cout << "Done" << endl;
+    return 0;
+}