#!/bin/sh

exec 2>&1
set -e
SRCDIR=$PWD

test_bullet_bouncing_box() {
    cd "$AUTOPKGTEST_TMP"
    cat >BulletBouncingBox.py <<EOF
import os, sys
from siconos.kernel import \
    NonSmoothDynamicalSystem, MoreauJeanOSI, TimeDiscretisation, \
    FrictionContact, NewtonImpactFrictionNSL, TimeStepping
import siconos.kernel as sk
from siconos.mechanics.collision.bullet import \
     SiconosBulletCollisionManager
from siconos.mechanics.collision import \
    SiconosBox, SiconosPlane, RigidBodyDS, SiconosContactor, SiconosContactorSet
from numpy import zeros
from numpy.linalg import norm
t0 = 0       # start time
T = 20       # end time
h = 0.005    # time step
g = 9.81     # gravity
theta = 0.5  # theta scheme
position_init = 10
velocity_init = 0
box1 = SiconosBox(1.0, 1.0, 1.0)
body = RigidBodyDS([0, 0, position_init, 1., 0, 0, 0],
              [0, 0, velocity_init, 0., 0., 0.],
              1.0)
body.contactors().push_back(SiconosContactor(box1))
weight = [0, 0, -body.scalarMass() * g]
body.setFExtPtr(weight)
bouncingBox = NonSmoothDynamicalSystem(t0, T)
bouncingBox.insertDynamicalSystem(body)
osi = MoreauJeanOSI(theta)
ground = SiconosPlane()
groundOffset = [0,0,-0.5,1,0,0,0]
timedisc = TimeDiscretisation(t0, h)
osnspb = FrictionContact(3)
osnspb.numericsSolverOptions().iparam[0] = 1000
osnspb.numericsSolverOptions().dparam[0] = 1e-5
osnspb.setMaxSize(16384)
osnspb.setMStorageType(1)
osnspb.setNumericsVerboseMode(False)
osnspb.setKeepLambdaAndYState(True)
nslaw = NewtonImpactFrictionNSL(0.8, 0., 0., 3)
broadphase = SiconosBulletCollisionManager()
broadphase.insertNonSmoothLaw(nslaw, 0, 0)
scs = SiconosContactorSet()
scs.append(SiconosContactor(ground))
broadphase.insertStaticContactorSet(scs, groundOffset)
simulation = TimeStepping(bouncingBox, timedisc)
simulation.insertInteractionManager(broadphase)
simulation.insertIntegrator(osi)
simulation.insertNonSmoothProblem(osnspb)
N = int((T - t0) / h)
k = 1
while(simulation.hasNextEvent()):
    simulation.computeOneStep()
    k += 1
    simulation.nextStep()
print('Done',k,'iterations.')
EOF
    python3 BulletBouncingBox.py
    assertEquals "Done 4001 iterations." "$(python3 BulletBouncingBox.py | tail -n1)"
}

. shunit2
