#!/usr/bin/env @PYTHON_EXECUTABLE@
import os
import sys
import atexit

print('##################################################################')
print('############## Siconos/Mechanics mechanisms module ###############')
print('##################################################################')

siconos_pythonpath = "@CURRENT_PYTHON_PATH@"

# path to data (bodydef, CAD files ...).
example_path = '@EXAMPLE_PATH@'

sys.path.append(siconos_pythonpath)
mechanisms_path = os.path.join(siconos_pythonpath,
                               'siconos/mechanisms')

print('############## Enrironment variable                ###############')


print("sys.executable: ", sys.executable)
print("sys.path: ", sys.path)

print('############## Loading of definition files         ###############')


from siconos.mechanisms import mbtb
from siconos.mechanisms import cadmbtb

# simple hdf5 output
# a dummy SpaceFilter is needed for model encapsulation
# a (pseudo) Contactor class is needed for the object-shape association.
from siconos.mechanics.collision.native import SpaceFilter
with_io = "@HAVE_SICONOS_IO@" is "TRUE"
if with_io:
    from siconos.io.mechanics_run import MechanicsHdf5Runner
from siconos.mechanics.collision.tools import Shape, Contactor

default_options = os.path.join(mechanisms_path, 'mbtbDefaultOptions.py')
local_options = os.path.join(example_path, 'mbtbLocalOptions.py')

exec(compile(open(default_options).read(), default_options, 'exec'))

if os.path.exists(local_options):
    exec(compile(open(local_options).read(), local_options, 'exec'))
    print("siconos-mechanisms.py, info: mbtbLocalOptions.py loaded")

bodydef_file = os.path.join(example_path, 'bodydef.py')
exec(compile(open(bodydef_file).read(), bodydef_file, 'exec'))
print('siconos-mechanisms.py, info:', bodydef_file, ' loaded')

with_run_id = False

try:
    print(" run id: ", run_id)
    os.mkdir(run_id)
    os.chdir(run_id)
    os.symlink(os.path.join('..', 'CAD'), 'CAD')
    os.symlink(os.path.join('..', 'plugins'), 'plugins')
    with_run_id = True

except:
    with_run_id = False

if with3D:
    from OCC.BRepPrimAPI import *
    from OCC.gp import *
    from OCC.TopLoc import *
    from OCC.AIS import *
    from OCC.Display.SimpleGui import init_display
    display, start_display, add_menu, add_function_to_menu = init_display()
    cadmbtb.CADMBTB_setGraphicContext(display.Context)
    v = display.GetView().GetObject()
    cadmbtb.CADMBTB_setGraphicView(v)
    display.View_Iso()


mbtb.MBTB_init(NBBODIES, NBJOINTS, NBCONTACTS)

if NBCONTACTS > 0:
    mbtb.MBTB_ContactSetDParam(2, 0, 0, ContactArtefactLength)
    mbtb.MBTB_ContactSetDParam(3, 0, 0, ArtefactThreshold)
    mbtb.MBTB_ContactSetDParam(4, 0, 0, NominalForce)

for idContact in range(NBCONTACTS):
    cadmbtb.CADMBTB_setContactDParam(
        0, idContact, 0, contactTrans1[idContact])  # visu
    cadmbtb.CADMBTB_setContactDParam(
        0, idContact, 1, contactTrans2[idContact])  # visu


#build contacts (must be done before the dynamical system building

for idContact in range(NBCONTACTS):
    afileContact1[idContact] = os.path.join(example_path,
                                            afileContact1[idContact])
    afileContact2[idContact] = os.path.join(example_path,
                                            afileContact2[idContact])
    print(afileContact2[idContact])

    mbtb.MBTB_ContactLoadCADFile(idContact, afileContact1[idContact],
                                 afileContact2[idContact],
                                 contactDraw1[idContact],
                                 contactDraw2[idContact])
    mbtb.MBTB_ContactBuild(idContact, contactName[idContact],
                           contactBody1[idContact], contactBody2[idContact],
                           contactType3D[idContact], contactmu[idContact],
                           contacten[idContact], et)
    mbtb.MBTB_ContactSetDParam(
        1, idContact, 0, contactOffset[idContact])  # offset
    # application point P1 or Trans P1.
    mbtb.MBTB_ContactSetIParam(0, idContact, 0, contactOffsetP1[idContact])
    # normal computed from Face1.
    mbtb.MBTB_ContactSetIParam(
        1, idContact, 0, contactNormalFromFace1[idContact])

for idBody in range(NBBODIES):
    cadmbtb.CADMBTB_setShapeDParam(0, idBody, bodyTrans[idBody])  # visu

#build dynamical systems
if with_run_id:
    plugin = os.path.join('..', plugin)

print("User plugin : ", plugin)

for idBody in range(NBBODIES):
    afile[idBody] = os.path.join(example_path, afile[idBody])
    mbtb.MBTB_BodyLoadCADFile(idBody, afile[idBody], bodyDraw[idBody])
    mbtb.MBTB_BodyBuild(idBody, body[idBody], m[idBody],
                        initPos[idBody], initCenterMass[idBody],
                        inertialMatrix[idBody],
                        plugin, fctfext[idBody], plugin, fctmext[idBody],
                        plugin, fctfint[idBody], plugin, fctmint[idBody],
                        plugin, fctfintjacq[
                            idBody], plugin, fctmintjacq[idBody],
                        plugin, fctfintjacv[
                            idBody], plugin, fctmintjacv[idBody],
                        plugin, boundaryCondition[idBody],
                        boundaryConditionIndex[idBody])

    mbtb.MBTB_BodySetVelocity(idBody, initVel[idBody])

#build joint
for idJoint in range(NBJOINTS):
    mbtb.MBTB_JointBuild(idJoint, jointName[idJoint],
                         jointType[idJoint], jointBody1[idJoint],
                         jointBody2[idJoint], jointPos[idJoint])

try:
    for idJoint in range(NBJOINTS):
        mbtb.MBTB_setJointPoints(idJoint, jointPoints[
                                 2 * idJoint], jointPoints[2 * idJoint + 1])
except NameError:
    print("run.py, info: joint points not defined.")

for idArtefact in range(NBARTEFACTS):
    Artefactfile[idArtefact] = os.path.join(example_path,
                                            Artefactfile[idArtefact])
    cadmbtb.CADMBTB_loadArtefactCADFile(
        Artefactfile[idArtefact], ArtefactTrans[idArtefact])


mbtb.MBTB_SetDParam(0, TSTheta)
mbtb.MBTB_SetDParam(1, TSGamma)

mbtb.MBTB_SetDParam(2, TSNewtonTolerance)
mbtb.MBTB_SetDParam(3, TSNewtonMaxIteration)

mbtb.MBTB_SetDParam(4, TSdeactivateYPosThreshold)
mbtb.MBTB_SetDParam(5, TSdeactivateYVelThreshold)
mbtb.MBTB_SetDParam(6, TSactivateYPosThreshold)
mbtb.MBTB_SetDParam(7, TSactivateYVelThreshold)
mbtb.MBTB_SetDParam(8, TSProjectionMaxIteration)
mbtb.MBTB_SetDParam(9, TSConstraintTol)
mbtb.MBTB_SetDParam(10, TSConstraintTolUnilateral)
mbtb.MBTB_SetDParam(11, TSLevelOfProjection)

mbtb.MBTB_initSimu(stepSize, withProj)
mbtb.MBTB_setGraphicFreq(freqUpdate)
mbtb.MBTB_setOutputFreq(freqOutput)
mbtb.MBTB_setSolverIOption(2, withReduced)
mbtb.MBTB_setSolverIOption(0, solverIt)
mbtb.MBTB_setSolverDOption(0, solverTol)
if gotoPos:
    mbtb.MBTB_moveBodyToPosWithSpeed(0, aPos0, aVel0)
    mbtb.MBTB_moveBodyToPosWithSpeed(1, aPos1, aVel1)
    mbtb.MBTB_moveBodyToPosWithSpeed(2, aPos2, aVel2)

if with3D:
    display.View_Iso()
    display.FitAll()

    # ord('W'): self._display.SetModeWireFrame,
    # ord('S'): set_shade_mode,
    # ord('A'): self._display.EnableAntiAliasing,
    # ord('B'): self._display.DisableAntiAliasing,
    # ord('Q'): self._display.SetModeQuickHLR,
    # ord('E'): self._display.SetModeExactHLR,
    # ord('F'): self._display.FitAll(
ais_boxshp = None


def remove_run_id_dir():

    if with_run_id:
        os.rename('simu.txt', os.path.join(
            '..', 'simu-{0}.txt'.format(run_id)))
        os.rename('siconos-mechanisms.hdf5', os.path.join('..',
                                                          'siconos-mechanisms-{0}.hdf5'.format(run_id)))
        os.remove('CAD')
        os.remove('plugins')
        os.chdir('..')
        os.removedirs(run_id)


atexit.register(remove_run_id_dir)

# Hdf5 IO setup
if with_io:
    with MechanicsHdf5Runner(simulation = mbtb.MBTB_simulation(),
                 nsds=mbtb.MBTB_nsds()) as io:
        def _run(nsteps):
            fp = mbtb._MBTB_open('simu.txt', 'a')
            currentTimerCmp = mbtb.cvar.sTimerCmp
            for i in range(nsteps):
                mbtb.cvar.sTimerCmp += 1
                print('STEP Number =', mbtb.cvar.sTimerCmp,
                      '<', nsteps + currentTimerCmp)
                mbtb._MBTB_STEP()
                mbtb._MBTB_displayStep()
                io.output_dynamic_objects()
                io.output_velocities()
                io.output_contact_forces()
                io.output_solver_infos()
                if mbtb.cvar.sTimerCmp % mbtb.cvar.sFreqOutput == 0:
                    mbtb._MBTB_printStep(fp)
            mbtb._MBTB_close(fp)
            mbtb.ACE_PRINT_TIME()

        def STEP(event=None):
            _run(1)

        def RUN10(event=None):
            _run(10)

        def RUN(event=None):
            _run(stepNumber)

        def RUN100(event=None):
            _run(100)

        def RUN500(event=None):
            _run(500)

        def RUN1000(event=None):
            _run(1000)

        def RUN10000(event=None):
            _run(10000)

        def RUN50000(event=None):
            _run(50000)

        def RUN100000(event=None):
            _run(100000)

        def QUIT(event=None):
            exit()

        def GRAPHIC_ON(event=None):
            cadmbtb.CADMBTB_setGraphicContext(display.Context)
            v = display.GetView().GetObject()
            cadmbtb.CADMBTB_setGraphicView(v)

        def GRAPHIC_OFF(event=None):
            cadmbtb.CADMBTB_disableGraphic()

        def DISABLE_PROJ(event=None):
            mbtb.MBTB_doProj(0)

        def ENABLE_PROJ(event=None):
            mbtb.MBTB_doProj(1)

        def DISABLE_ONLYPROJ(event=None):
            mbtb.MBTB_doOnlyProj(0)

        def ENABLE_ONLYPROJ(event=None):
            mbtb.MBTB_doOnlyProj(1)

        def GRAPHIC_DUMP(event=None):
            cadmbtb.CADMBTB_DumpGraphic()

        def ENABLE_VERBOSE_MBTB_PRINT_DIST(event=None):
            mbtb.MBTB_print_dist(1)
            cadmbtb.CADMBTB_print_dist(1)

        def DISABLE_VERBOSE_MBTB_PRINT_DIST(event=None):
            mbtb.MBTB_print_dist(0)
            cadmbtb.CADMBTB_print_dist(0)

        def ENABLE_VERBOSE_MBTB_DISPLAY_STEP_BODIES(event=None):
            mbtb.MBTB_displayStep_bodies(1)

        def DISABLE_VERBOSE_MBTB_DISPLAY_STEP_BODIES(event=None):
            mbtb.MBTB_displayStep_bodies(0)

        def ENABLE_VERBOSE_MBTB_DISPLAY_STEP_JOINTS(event=None):
            mbtb.MBTB_displayStep_joints(1)

        def DISABLE_VERBOSE_MBTB_DISPLAY_STEP_JOINTS(event=None):
            mbtb.MBTB_displayStep_joints(0)

        def ENABLE_VERBOSE_MBTB_DISPLAY_STEP_CONTACTS(event=None):
            mbtb.MBTB_displayStep_contacts(1)

        def DISABLE_VERBOSE_MBTB_DISPLAY_STEP_CONTACTS(event=None):
            mbtb.MBTB_displayStep_contacts(0)

        def VIEW_TOP(event=None):
            display.View_Top()

        def VIEW_BOTTOM(event=None):
            display.View_Bottom()

        def VIEW_LEFT(event=None):
            display.View_Left()

        def VIEW_RIGHT(event=None):
            display.View_Right()

        def VIEW_REAR(event=None):
            display.View_Rear()

        def VIEW_FRONT(event=None):
            display.View_Front()

        def VIEW_ISO(event=None):
            display.View_Iso()

        def VIEW_FITALL(event=None):
            display.FitAll()

        def VIEW_TOP(event=None):
            display.View_Top()

        assoc_shapes = dict()
        offset = dict()
        for idBody in range(NBBODIES):
            shape_name = os.path.basename(os.path.splitext(afile[idBody])[0])

            offset[idBody] = [-v for v in initCenterMass[idBody]]

            io.add_shape_data_from_file(shape_name, afile[idBody])

            assoc_shapes[idBody] = [
                Contactor(shape_name, relative_translation=offset[idBody], collision_group=-1)]
    #        io.addObject('obj-{0}'.format(shape_name),
    #                     [Shape(shape_name, relative_translation=offset)],
    #                     mass=1,
    #                     translation=[0, 0, 0])

        for idContact in range(NBCONTACTS):
            shape_name1 = os.path.basename(
                os.path.splitext(afileContact1[idContact])[0])
            shape_name2 = os.path.basename(
                os.path.splitext(afileContact2[idContact])[0])

            io.add_shape_data_from_file(shape_name1, afileContact1[idContact])
            io.add_shape_data_from_file(shape_name2, afileContact2[idContact])

            print(contactBody1[idContact], contactBody2[idContact])

            assoc_shapes[contactBody1[idContact]] += [
                Contactor(
                    shape_name1,
                    relative_translation=offset[contactBody1[idContact]])]

            if contactBody2[idContact] >= 0:
                assoc_shapes[contactBody2[idContact]] += [
                Contactor(
                    shape_name2,
                    relative_translation=offset[contactBody2[idContact]])]

        for idArtefact in range(NBARTEFACTS):

            shape_name = os.path.basename(
                os.path.splitext(Artefactfile[idArtefact])[0])

            io.add_shape_data_from_file(shape_name, Artefactfile[idArtefact])

        for idBody in range(NBBODIES):
            shape_name = os.path.basename(os.path.splitext(afile[idBody])[0])

            io.add_object('obj-{0}'.format(shape_name),
                         assoc_shapes[idBody],
                         mass=1,
                         translation=[0, 0, 0])

        for idArtefact in range(NBARTEFACTS):

            shape_name = os.path.basename(
                os.path.splitext(Artefactfile[idArtefact])[0])

            io.add_object('artefact-{0}'.format(shape_name),
                         [Contactor(shape_name,collision_group=-1)],
                         mass=None,
                         translation=[0, 0, 0])

        io.output_static_objects()

        if with3D and __name__ == '__main__':
            cadmbtb.CADMBTB_setIParam(0, dumpGraphic)
            mbtb.MBTB_ContactSetIParam(2, 0, 0, drawMode)
            add_menu('Siconos Mechanisms')
            add_menu('Options')
            add_menu('Verbose')
            add_menu('Display')
            add_menu('View')
            add_function_to_menu('Siconos Mechanisms', STEP)

            add_function_to_menu('Siconos Mechanisms', RUN10)
            add_function_to_menu('Siconos Mechanisms', RUN100)
            add_function_to_menu('Siconos Mechanisms', RUN500)
            add_function_to_menu('Siconos Mechanisms', RUN1000)
            add_function_to_menu('Siconos Mechanisms', RUN10000)
            add_function_to_menu('Siconos Mechanisms', RUN50000)
            add_function_to_menu('Siconos Mechanisms', RUN100000)

            add_function_to_menu('Siconos Mechanisms', RUN)
            add_function_to_menu('Siconos Mechanisms', QUIT)

            add_function_to_menu('Display', GRAPHIC_OFF)
            add_function_to_menu('Display', GRAPHIC_ON)
            add_function_to_menu('Display', GRAPHIC_DUMP)
            add_function_to_menu('Verbose', ENABLE_VERBOSE_MBTB_PRINT_DIST)
            add_function_to_menu('Verbose', DISABLE_VERBOSE_MBTB_PRINT_DIST)
            add_function_to_menu(
                'Verbose', ENABLE_VERBOSE_MBTB_DISPLAY_STEP_BODIES)
            add_function_to_menu(
                'Verbose', DISABLE_VERBOSE_MBTB_DISPLAY_STEP_BODIES)
            add_function_to_menu(
                'Verbose', ENABLE_VERBOSE_MBTB_DISPLAY_STEP_JOINTS)
            add_function_to_menu(
                'Verbose', DISABLE_VERBOSE_MBTB_DISPLAY_STEP_JOINTS)
            add_function_to_menu(
                'Verbose', ENABLE_VERBOSE_MBTB_DISPLAY_STEP_CONTACTS)
            add_function_to_menu(
                'Verbose', DISABLE_VERBOSE_MBTB_DISPLAY_STEP_CONTACTS)
            add_function_to_menu('Options', DISABLE_PROJ)
            add_function_to_menu('Options', ENABLE_PROJ)
            add_function_to_menu('Options', DISABLE_ONLYPROJ)
            add_function_to_menu('Options', ENABLE_ONLYPROJ)
            add_function_to_menu('View', VIEW_TOP)
            add_function_to_menu('View', VIEW_BOTTOM)
            add_function_to_menu('View', VIEW_RIGHT)
            add_function_to_menu('View', VIEW_LEFT)
            add_function_to_menu('View', VIEW_FRONT)
            add_function_to_menu('View', VIEW_REAR)
            add_function_to_menu('View', VIEW_ISO)
            add_function_to_menu('View', VIEW_FITALL)
            start_display()
        else:
            _run(stepNumber)
