/*****************************************************************************
 * $CAMITK_LICENCE_BEGIN$
 *
 * CamiTK - Computer Assisted Medical Intervention ToolKit
 * (c) 2001-2014 UJF-Grenoble 1, CNRS, TIMC-IMAG UMR 5525 (GMCAO)
 *
 * Visit http://camitk.imag.fr for more information
 *
 * This file is part of CamiTK.
 *
 * CamiTK is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License version 3
 * only, as published by the Free Software Foundation.
 *
 * CamiTK is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License version 3 for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * version 3 along with CamiTK.  If not, see <http://www.gnu.org/licenses/>.
 *
 * $CAMITK_LICENCE_END$
 ****************************************************************************/

#include <QApplication>
#include <QCursor>
#include <QTimer>

#include <vtkTransform.h>

#include "PMManagerDC.h"

#include <pml/PhysicalModel.h>
#include <pml/Atom.h>
#include <pml/StructuralComponent.h>


#include <InteractiveViewer.h>
using namespace camitk;

#include "LoadsSimulationDriver.h"
#include "LoadsManager.h"

#include "AtomDC.h"
#include "StructuralComponentDC.h"

#include <lml/Loads.h>


#include <limits>

//--------------- Constructor ---------------------------------
LoadsSimulationDriver::LoadsSimulationDriver ( LoadsManager *lm, Loads * initialLoads ) {
    myLM = lm;
    timer = NULL; // not started yet

    lastUpdateTime = -1.0;
    lastRefreshTime = -1000.0;
    refreshDt = dt = 0.1;
    tMin = tMax = 0.0;

    // set the initial loads as current loads (needed by init)
    setLoads ( initialLoads );

    // init the display
    init();
}


//--------------- Destructor ---------------------------------
LoadsSimulationDriver::~LoadsSimulationDriver() {
    stopTimer();
    delete timer;
    timer = NULL;
}


//--------------- init ---------------------------------
void LoadsSimulationDriver::init() {
    // store the initial position
    // and make sure that all Atom DC's have a 3D representation
    PMManagerDC *pmMgrDC = myLM->getPMManagerDC();
    PhysicalModel * pm = pmMgrDC->getPhysicalModel();
    AtomDC * adc;
    Atom *a;

    
    /*
    if (you want to show the absolute displacement of only the
      implied atom, just use this code) {
// => the best in this case is to constrained the scale yourself (user-constrained)
 // which are the implied atoms
        // get the initial position
        for (unsigned int i = 0; i < currentLoads->numberOfLoads();i++) {
              Load *l = currentLoads->getLoad(i);

          for (unsigned int j = 0; j < l->numberOfTargets(); j++) {
            // get the target
            a = pm->getAtom(l->getTarget(j));
            // activate the target's 3D representation
            adc = pmMgrDC->getDC(a);

            if (adc != NULL) {
              double pos[3];
              a->getPosition(pos);
              Position p;
              p.x = pos[0];
              p.y = pos[1];
              p.z = pos[2];
              initialPositionMap.insert(l->getTarget(j), p);
            }
          }
        }
      }
    */
    StructuralComponent * theAtoms = pm->getAtoms();

    for ( unsigned int i = 0; i < theAtoms->getNumberOfStructures();i++ ) {
        // get the target index
        a = ( Atom * ) theAtoms->getStructure ( i );
        // activate the target's 3D representation
        adc = pmMgrDC->getDC ( a );

        if ( adc != NULL ) {
            double pos[3];
            a->getPosition ( pos );
            Position p;
            p.x = pos[0];
            p.y = pos[1];
            p.z = pos[2];
            initialPositionMap.insert ( a->getIndex(), p );
        }
    }

    if ( currentLoads ) {
        // look for default value in the loads
        resetTMinToDefault();

        resetTMaxToDefault();
    } else {
        // set the default values
        tMin = 0.0;
        tMax = 1.0;
    }

    // start from tMin
    rewind();

    // start the timer
    timer = new QTimer ( this );

    // connect the timer tick signal to play()
    connect ( timer, SIGNAL ( timeout() ), this, SLOT ( play() ) );

    // max interval is 1000 times per sec
    maxSpeed();
}

//--------------- setLoads ---------------
void LoadsSimulationDriver::setLoads ( Loads * l ) {
    currentLoads = l;
}


//--------------- rewind -----------------------
void LoadsSimulationDriver::rewind() {
    if ( myLM->getLoads() ) {
        // set time just before the first active load
        t = myLM->getLoads()->getFirstEventDate() - dt;
    } else
        t = tMin - dt;

    resetPositions();

    // see the time passing by
    while ( t < tMin - dt ) {
        t += dt;
        updatePositions ( false );
    }

    t = tMin - dt;
}


//--------------- resetPositions -----------------------
void LoadsSimulationDriver::resetPositions() {
    AtomDC *adc;
    PMManagerDC *pmMgrDC = myLM->getPMManagerDC();
    PhysicalModel * pm = pmMgrDC->getPhysicalModel();
    Atom *a;

    // reset all the stored initial position
    QMapIterator<unsigned int, Position> it ( initialPositionMap );
    while ( it.hasNext() ) {
        it.next();
        // get the target
        a = pm->getAtom ( it.key() );
        adc = pmMgrDC->getDC ( a );

        if ( adc != NULL ) {
            adc->resetAlreadyMovedFlag();
            adc->setPosition ( it.value().x, it.value().y, it.value().z );
        }

    }

    lastRefreshTime = -1000.0;
    lastUpdateTime = -1.0;
}

//---------------  updateAtomData -----------------------
void LoadsSimulationDriver::updateAtomData() {
    if ( myLM->getAtomDataDisplay() != LoadsManager::NONE ) {
        //-- get the double value of the atom data
        std::AtomDataVector atomDataValues;

        //-- initialize the referencePositionMap
        if ( ( myLM->getAtomDataDisplay() == LoadsManager::DISTANCES ||
               myLM->getAtomDataDisplay() == LoadsManager::RELATIVE_ENERGY_NORM_ERROR ) &&
             ( referencePositionMap.size() == 0 ) ) {

            StructuralComponent * theAtoms = myLM->getReferencePM()->getAtoms();

            for ( unsigned int i = 0; i < theAtoms->getNumberOfStructures();i++ ) {
                // get the target index
                Atom *a = ( Atom * ) theAtoms->getStructure ( i );
                double pos[3];
                a->getPosition ( pos );
                Position p;
                p.x = pos[0];
                p.y = pos[1];
                p.z = pos[2];
                referencePositionMap.insert ( a->getIndex(), p );
            }
        }

        switch ( myLM->getAtomDataDisplay() ) {

        case LoadsManager::ADD_ON:
            // simply get the add-on atom data range
            atomDataValues = myLM->getAtomData();
            break;

        case LoadsManager::DISPLACEMENTS: {
            // compute the total displacements
            PhysicalModel * pm = myLM->getPMManagerDC()->getPhysicalModel();
            double pos[3], distance;
            Atom *a;

            // compute distance between initial position and now, and min/max
            QMapIterator<unsigned int, Position> it ( initialPositionMap );
            while ( it.hasNext() ) {
                it.next();
                // get the atom position
                a = pm->getAtom ( it.key() );
                a->getPosition ( pos );
                distance = sqrt ( ( pos[0] - it.value().x ) * ( pos[0] - it.value().x )
                                  + ( pos[1] - it.value().y ) * ( pos[1] - it.value().y )
                                  + ( pos[2] - it.value().z ) * ( pos[2] - it.value().z ) );
                atomDataValues.push_back ( std::AtomDataPair ( a, distance ) );
            }

            break;
        }

        case LoadsManager::DISTANCES: {
            //-- compute the distance
            PhysicalModel * pm = myLM->getPMManagerDC()->getPhysicalModel();
            double pos[3], distance;
            Atom *a;

            // compute distance between initial position and now, and min/max
            QMapIterator<unsigned int, Position> it ( referencePositionMap );
            while ( it.hasNext() ) {
                it.next();
                // get the atom position
                a = pm->getAtom ( it.key() );
                a->getPosition ( pos );
                distance = sqrt ( ( pos[0] - it.value().x ) * ( pos[0] - it.value().x )
                                  + ( pos[1] - it.value().y ) * ( pos[1] - it.value().y )
                                  + ( pos[2] - it.value().z ) * ( pos[2] - it.value().z ) );

                atomDataValues.push_back ( std::AtomDataPair ( a, distance ) );
            }

            break;
        }

        case LoadsManager::RELATIVE_ENERGY_NORM_ERROR: {
            //-- compute the error percentage
            PhysicalModel * pm = myLM->getPMManagerDC()->getPhysicalModel();
            double pos[3], percentage;
            double simulatedToReal;
            double totalDisplacement;
            Atom *a;

            // compute distance between initial position and now, and min/max
            QMapIterator<unsigned int, Position> it ( referencePositionMap );
            while ( it.hasNext() ) {
                it.next();
                // get the atom position
                a = pm->getAtom ( it.key() );
                a->getPosition ( pos );
                simulatedToReal = sqrt ( ( pos[0] - it.value().x ) * ( pos[0] - it.value().x )
                                         + ( pos[1] - it.value().y ) * ( pos[1] - it.value().y )
                                         + ( pos[2] - it.value().z ) * ( pos[2] - it.value().z ) );
                // get the init position
                getInitialPosition ( it.key(), pos );
                totalDisplacement = sqrt ( ( pos[0] - it.value().x ) * ( pos[0] - it.value().x )
                                           + ( pos[1] - it.value().y ) * ( pos[1] - it.value().y )
                                           + ( pos[2] - it.value().z ) * ( pos[2] - it.value().z ) );

                if ( totalDisplacement < 1e-10 )
                    percentage = 0.0;
                else
                    percentage = simulatedToReal / totalDisplacement;

                //percentage *= 100.0;

                atomDataValues.push_back ( std::AtomDataPair ( a, percentage ) );
            }

            break;
        }

        default: // NONE
            break;
        }

        //-- check the range if needed
        double min, max;
        std::AtomDataVector::iterator it;
        std::vector<AtomDC *> atomDCs; // for efficiency, to store the DCs
        Atom *a;
        AtomDC *adc;
        PMManagerDC *pmMgrDC = myLM->getPMManagerDC();

        // destroy existing atom data (in case not all atom is given a point data)
        myLM->getPMManagerDC()->destroyPointData();

        if ( !myLM->getUserConstrainedAtomDataScale() ) {
            min = std::numeric_limits<double>::max();
            max = -1.0;

            if ( atomDataValues.size() == 0 ) {
                min = -1.0;
                max = 1.0;
            }

            for ( it = atomDataValues.begin(); it != atomDataValues.end(); it++ ) {
                // get the atom
                a = it->first;
                // ...and its dc
                adc = pmMgrDC->getDC ( a );

                if ( adc != NULL ) {
                    if ( ( it->second ) > max )
                        max = ( it->second );

                    if ( ( it->second ) < min )
                        min = ( it->second );

                    atomDCs.push_back ( adc );
                }
            }
        } else {
            myLM->getAtomDataScale ( &min,&max );
            for ( it = atomDataValues.begin(); it != atomDataValues.end(); it++ ) {
                // get the atom
                a = it->first;
                // ...and its dc
                adc = pmMgrDC->getDC ( a );

                if ( adc != NULL ) {
                    atomDCs.push_back ( adc );
                }
            }
        }

        //-- rescale the distance between 0 and 1 and tell the atoms
        double val;
        double range = max - min;

        for ( unsigned int i = 0; i < atomDCs.size();i++ ) {
            val = atomDataValues[i].second;

            if ( atomDataValues.size() == 0 )
                val = 0.0;
            else
                if ( fabs ( range ) < 1e-10 || val <= min ) {
                    val = 0.0;
                } else {
                    if ( val >= max ) {
                        val = 1.0;
                    } else {
                        // renormalized the size between min and max
                        val = ( ( val - min ) / range );
                    }
                }

            atomDCs[i]->updatePointData ( 1.0 - val );
        }

        // show values on the scale
        myLM->updateAtomDataScale ( min, max );

    }

}

//---------------  updatePositions -----------------------
void LoadsSimulationDriver::updatePositions ( bool force ) {
    Load * l;
    double dir[3];
    double displ[3];
    double pos[3];
    double value;
    double lastValue;
    AtomDC *adc;
    Atom *a;
    bool refresh3D;
    PMManagerDC *pmMgrDC = myLM->getPMManagerDC();
    PhysicalModel * pm = pmMgrDC->getPhysicalModel();

    refresh3D = ( getNextRefreshTime() < t ) || ( fabs ( getNextRefreshTime() - t ) < 1e-15 );

    if ( refresh3D || force ) {
        // if there are no curernt load, just display the atom
        // (some simulation motors can actually give no current loads, but also
        // directly modify the positions of the atoms, in order to increase speed)
        if ( !currentLoads ) {
            StructuralComponent * theAtoms = pm->getAtoms();

            for ( unsigned int i = 0; i < theAtoms->getNumberOfStructures();i++ ) {
                // get the target index
                a = ( Atom * ) theAtoms->getStructure ( i );
                // activate the target's 3D representation
                adc = pmMgrDC->getDC ( a );

                if ( adc != NULL ) {
                    a->getPosition ( pos );
                    adc->resetAlreadyMovedFlag();
                    adc->setPosition ( pos[0], pos[1], pos[2] );
                }
            }
        } else {
            // get the informations from the current Loads
            for ( unsigned int i = 0; i < currentLoads->numberOfLoads();i++ ) {
                l = currentLoads->getLoad ( i );
                // get the informations
                value = l->getValue ( t );  // now
                // display only active translation

                if ( l->getType() == "Translation" && value != 0.0 ) {
                    dir[0] = l->getDirection().getX();
                    dir[1] = l->getDirection().getY();
                    dir[2] = l->getDirection().getZ();
                    // is that a zero-translation constraint?

                    if ( ! ( dir[0] == 0.0 && dir[1] == 0.0 && dir[2] == 0.0 ) ) {
                        // the last time we updated the display (so it is possible to linearly interpolate)
                        lastValue = l->getValue ( lastUpdateTime );
                        // normalize
                        // AND multiply by the difference between the current value and the last time value,
                        // so that when t is equal to the date of the current value event, the total
                        // of displacement will sum up to value. (linear interpolation)
                        double n = sqrt ( dir[0] * dir[0] + dir[1] * dir[1] + dir[2] * dir[2] );

                        for ( unsigned int j = 0;j < 3;j++ )
                            dir[j] *= ( value - lastValue ) / n;

                        // apply the translation to the targets (atoms)
                        for ( unsigned int j = 0; j < l->numberOfTargets(); j++ ) {
                            // get the target
                            a = pm->getAtom ( l->getTarget ( j ) );

                            if ( a ) {
                                // compute new displacement
                                for ( unsigned int j = 0;j < 3;j++ )
                                    displ[j] = dir[j];

                                // displ is the displacement vector to transform to a ImpTransform
                                a->getPosition ( pos );

                                for ( unsigned int j = 0;j < 3;j++ )
                                    displ[j] += pos[j];

                                a->setPosition ( displ[0], displ[1], displ[2] );

                                if ( refresh3D ) {
                                    adc = pmMgrDC->getDC ( a );

                                    if ( adc != NULL ) {
                                        adc->resetAlreadyMovedFlag();
                                        adc->setPosition ( displ[0], displ[1], displ[2] );
                                    }
                                }
                            }
                        }
                    }
                }
            }
        }

        // update the atom data
        updateAtomData();

        // remember the last time we updated the positions
        lastUpdateTime = t;
    }
}

//--------------- play -----------------------
void LoadsSimulationDriver::play() {
    if ( !timer->isActive() ) {
        // timer wasn't active, action to be done = launch the timer
        startTimer();
    } else {
        // timer is active, action to be done = play one step
        emit doOneStep();
    }
}

//---------------  updateDisplay -----------------------
void LoadsSimulationDriver::updateDisplay ( bool force ) {

    // update all the positions & displacements
    updatePositions ( force );

    // refresh the view
    if ( force || getNextRefreshTime() <= t ) {
        InteractiveViewer::get3DViewer()->refreshRenderer();
        lastRefreshTime = t;
        //Application::getMainWindow()->statusMessage ( QString ( "t=%1" ).arg ( t ) );
    }

    updateAtomData();

    myLM->updateLoadsDisplay();

}

//---------------  getNextRefreshTime -----------------------
double LoadsSimulationDriver::getNextRefreshTime() {
    return ( lastRefreshTime + refreshDt );
}


//--------------- startTimer -------------------
void LoadsSimulationDriver::startTimer() {
    timer->start ( interval );  // 200 times per second
}

//--------------- stopTimer -------------------
void LoadsSimulationDriver::stopTimer() {
    timer->stop();
}

//--------------- isTimerActive -------------------
bool LoadsSimulationDriver::isTimerActive() {
    return timer->isActive();
}


//--------------- resetToDefault ---------------
void LoadsSimulationDriver::resetToDefault() {
    resetTMinToDefault();
    resetTMaxToDefault();
    dt = refreshDt = 0.1;
}

//--------------- resetTMinToDefault ---------------
void LoadsSimulationDriver::resetTMinToDefault() {
    if ( myLM != NULL && currentLoads != NULL ) {
        // search for the first event
        tMin = currentLoads->getFirstEventDate();
    }
}

//--------------- getTMin -------------------
double LoadsSimulationDriver::getTMin() const {
    return tMin;
}

//--------------- setTMin -------------------
void LoadsSimulationDriver::setTMin ( double newTMin ) {
    tMin = newTMin;
}

//--------------- resetTMaxToDefault ---------------
void LoadsSimulationDriver::resetTMaxToDefault() {
    if ( myLM != NULL && currentLoads != NULL ) {
        // search for the last event
        tMax  = currentLoads->getLastEventDate();
    }
}

//--------------- getTMax -------------------
double LoadsSimulationDriver::getTMax() const {
    return tMax;
}

//--------------- setTMax -------------------
void LoadsSimulationDriver::setTMax ( double newTMax ) {
    tMax = newTMax;
}

//--------------- getDt -------------------
double LoadsSimulationDriver::getDt() const {
    return dt;
}

//--------------- setDt ---------------
void LoadsSimulationDriver::setDt ( double dt ) {
    // default dt value
    this->dt = dt;
}

//--------------- getTime -------------------
double LoadsSimulationDriver::getTime() const {
    return t;
}

//--------------- setTime -------------------
void LoadsSimulationDriver::setTime ( double newTime ) {
    // we have to move along the time line little by little, otherwise
    // we might jump over some translations

    // in case we have to comm
    if ( newTime < t ) {
        // restart from the beginning
        //resetPositions();
        rewind();

        // see the time passing by up to newTime-dt

        for ( t = tMin - dt; ( t < ( newTime - dt ) ) && fabs ( t - ( newTime - dt ) ) > 1e-15;t += dt )
            updatePositions ( false );

    }

    t = newTime;
}

//--------------- getRefreshDt -------------------
double LoadsSimulationDriver::getRefreshDt() const {
    return refreshDt;
}

//--------------- setRefreshDt ---------------
void LoadsSimulationDriver::setRefreshDt ( double dt ) {
    refreshDt = dt;
}

//--------------- slower ---------------
void LoadsSimulationDriver::slower() {
    interval += 5;

    if ( timer->isActive() )
        timer->setInterval ( interval );
}

//--------------- quicker ---------------
void LoadsSimulationDriver::quicker() {
    interval -= 5;

    if ( interval <= 0 )
        maxSpeed();
    else {
        if ( timer->isActive() )
            timer->setInterval ( interval );
    }
}

//--------------- maxSpeed ---------------
void LoadsSimulationDriver::maxSpeed() {
    interval = 1;

    if ( timer->isActive() )
        timer->setInterval ( interval );
}