/*
 * Interpolator.cpp
 *
 * Copyright (C) 1999 Stephen F. White
 * 
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program 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 General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program (see the file "COPYING" for details); if 
 * not, write to the Free Software Foundation, Inc., 675 Mass Ave, 
 * Cambridge, MA 02139, USA.
 */

#include "stdafx.h"

#include "Interpolator.h"
#include "FieldValue.h"
#include "MFFloat.h"
#include "Scene.h"
#include "MFieldCommand.h"
#include "FieldCommand.h"
#include "CommandList.h"
#include "InputDeviceTime.h"
#include "Field.h"

ProtoInterpolator::ProtoInterpolator(Scene *scene, const char *name, int keyType,
					 int keysType, FieldValue *defaultValue)
  : Proto(scene, name)
{
    addEventIn(SFFLOAT, "set_fraction", EIF_RECOMMENDED);
    key.set(addExposedField(MFFLOAT, "key", new MFFloat()));
    keyValue.set(addExposedField(keysType, "keyValue", defaultValue));
    addEventOut(keyType, "value_changed", EOF_RECOMMENDED);
}

Interpolator::Interpolator(Scene *scene, Proto *def)
  : Node(scene, def)
{
    _fraction = 0.0f;
    _oldFraction=1.0f;
    _veryOldFraction=1.0f;
    _oldTimestamp=0.0f;
    _keyIndex.set(((ProtoInterpolator *)def)->key);
    _keyValueIndex.set(((ProtoInterpolator *)def)->keyValue);
    _value_changedIndex.set(2);
}

int
Interpolator::getNumKeys() const
{
    MFFloat	*key = (MFFloat *) getField(_keyIndex);

    return key->getSize();
}

float
Interpolator::getKey(int index) const
{
    MFFloat	*key = (MFFloat *) getField(_keyIndex);
    if (index < key->getSize()) {
	return key->getValue(index);
    } else {
	return 0.0f;
    }
}

float
Interpolator::getKeyValue(int channel, int index) const
{
    MFFloat     *keyValue = (MFFloat *) getField(_keyValueIndex);
    int		 i = index * getNumChannels() + channel;
    if (i < keyValue->getSize()) {
	return keyValue->getValue(i);
    } else {
	return 0.0f;
    }
}

void
Interpolator::setKey(int index, float value)
{
    MFFloat	*key = (MFFloat *) getField(_keyIndex);

    key->setValue(index, value);
    _scene->OnFieldChange(this, _keyIndex, index);
}

void
Interpolator::setKeyValue(int channel, int index, float value)
{
    MFFloat	*keyValue = (MFFloat *) getField(_keyValueIndex);

    keyValue->setValue(index * getNumChannels() + channel, value);
    _scene->OnFieldChange(this, _keyValueIndex, index);
}


void
Interpolator::backupKey(int index)
{
    CommandList	    *list = new CommandList();

    list->append(new MFieldCommand(this, _keyIndex, index));
    list->append(new MFieldCommand(this, _keyValueIndex, index));
    _scene->add(list);
}

void
Interpolator::backup(CommandList *list)
{
    int	    pos = findKeyInclusive(_fraction);

    if (pos != getNumKeys()) {
	list->append(new MFieldCommand(this, _keyIndex, pos));
	list->append(new MFieldCommand(this, _keyValueIndex, pos));
    }
    list->append(new FieldCommand(this, _keyIndex));
    list->append(new FieldCommand(this, _keyValueIndex));
}

//
// findKey() - return the closest key stricty greater than the given value,
//	       or n if none found
//

int
Interpolator::findKey(float value) const
{
    MFFloat    *key = (MFFloat *) getField(_keyIndex);
    int		i, numKeys = key->getSize();

    for(i = 0; i < numKeys; i++) {
	float	    k = key->getValue(i);
	if (k > value) break;
    }
    return i;
}

//
// findKeyInclusive() - return the closest key greater than or equal to 
//			the given value, or numKeys if none found
//

int
Interpolator::findKeyInclusive(float value) const
{
    MFFloat    *key = (MFFloat *) getField(_keyIndex);
    int		i, numKeys = key->getSize();

    for(i = 0; i < numKeys; i++) {
	float	    k = key->getValue(i);
	if (k >= value) break;
    }
    return i;
}

//
// findKeyExclusive() - return the closest key greater than given value, 
//                      or numKeys+1 if none found

int
Interpolator::findKeyExclusive(float value) const
{
    MFFloat    *key = (MFFloat *) getField(_keyIndex);
    int		i, numKeys = key->getSize();

    for(i = 0; i < numKeys; i++) {
	float	    k = key->getValue(i);
	if (k > value) return(i);
    }
    return numKeys+1;
}


bool
Interpolator::getNearestKeys(float k, float *k1, float *k2, int *pos1, int *pos2)
{
    MFFloat    *key = (MFFloat *) getField(_keyIndex);
    int		numKeys = key->getSize();

    if (numKeys == 0) {
	return false;
    }

    int pos = findKey(k);

    if (pos == 0) {
        *k1 = 0.0f;
	*pos1 = 0;
    } else {
	*k1 = key->getValue(pos - 1);
	*pos1 = pos - 1;
    }

    if (pos == numKeys) {
	*k2 = 1.0f;
	*pos2 = numKeys-1;
    } else {
	*k2 = key->getValue(pos);
	*pos2 = pos;
    }
    return true;
}

void
Interpolator::oldFraction(double timestamp, float fraction)
{
    if ( (timestamp-_oldTimestamp) > (((float)INPUTDEVICE_TIME)/1000.0) ) {
       _oldFraction=_veryOldFraction;
       _veryOldFraction=fraction;
       _oldTimestamp=timestamp;
    }  
}

void
Interpolator::receiveEvent(int eventIn, double timestamp, FieldValue *value)
{
    float fraction=((SFFloat *) value)->getValue();
    oldFraction(timestamp,fraction);
    switch (eventIn) {
      case 0:
	sendInterpolatedEvent(timestamp, fraction);
        break;
      default:
	Node::receiveEvent(eventIn, timestamp, value);
	break;
    }
}

void
Interpolator::sendInterpolatedEvent(double timestamp, float k)
{
    _fraction = k;
    float      *values = new float[getNumChannels()];
    interpolate(k, values);
    sendEvent(_value_changedIndex, timestamp, createKey(values));
}

void
Interpolator::sendInterpolatedValue(double timestamp, float k)
{
    sendInterpolatedEvent(timestamp,k);          
    // send same values to all Interpolators driven by the same EventIn
    int eventInIndex=lookupEventIn("set_fraction");
    if (eventInIndex==INVALID_INDEX) {
       fprintf(stderr,"Problem: Interpolater without set_fraction ???\n");
       return;
    }
    // for all EventIn;s
    SocketList::Iterator* in;
    for (in = _inputs[eventInIndex].first(); in != NULL; in = in->next()) {
       Node *EventInSrcNode = in->item()._node;
       int EventInSrcEventOutIndex = in->item()._index;
       // for all EventOuts of Node of EventIn
       SocketList::Iterator* out;
       for (out = EventInSrcNode->getOutput(EventInSrcEventOutIndex).first(); 
            out != NULL; out = out->next()) 
          if (out->item()._node!=this)
             {
             // is node a Interpolator ?
             Interpolator* Inod=dynamic_cast_Interpolator(out->item()._node);
             if (Inod!=NULL)
                Inod->sendInterpolatedEvent(timestamp,k);
             }
    }
}


void
Interpolator::interpolate(float k, float *values)
{
    MFFloat    *keyValue = (MFFloat *) getField(_keyValueIndex);
    int		numChannels = getNumChannels();

    float k1 = 0.0;  
    float k2 = 1.0;  
    int   pos1 = 0;
    int   pos2 = 0;  

    if (keyValue->getSize() == 0) {
	for (int i = 0; i < numChannels; i++) {
	    values[i] = 0.0f;
	}
	return;
    }

    getNearestKeys(k, &k1, &k2, &pos1, &pos2);

    const float    *value = keyValue->getValues();

    float	    alpha;

    if (k1 == k2) {
	alpha = 0.0;
    } else {
	alpha = (k - k1) / (k2 - k1);
    }

    for (int i = 0; i < numChannels; i++) {
	float	    v1 = value[pos1 * numChannels + i];
	float	    v2 = value[pos2 * numChannels + i];
	values[i] = v1 + (v2 - v1) * alpha;
    }
}

void
Interpolator::insertKey(int pos, float value, const float *values)
{
    MFFloat        *key = (MFFloat *) getField(_keyIndex);
    MFFloat	   *keyValue = (MFFloat *) getField(_keyValueIndex);
    int		    numKeys = key->getSize();
    int		    numChannels = getNumChannels();
    const float    *k = key->getValues();
    const float	   *v = keyValue->getValues();

    float	   *newK = new float[numKeys + 1];
    float	   *newV = new float[(numKeys + 1) * numChannels];

    if (pos > 0) {
	memcpy(newK, k, pos * sizeof(float));
	memcpy(newV, v, pos * numChannels * sizeof(float));
    }

    if (pos < numKeys) {
	memcpy(newK + pos + 1, k + pos, (numKeys - pos) * sizeof(float));
	memcpy(newV + (pos + 1) * numChannels, v + pos * numChannels, 
	       (numKeys - pos) * numChannels * sizeof(float));
    }

    newK[pos] = value;
    for (int i = 0; i < numChannels; i++) {
        if (values) {
	    newV[pos * numChannels + i] = values[i];
	} else {
            newV[pos * numChannels + i] = 0.0;
        }
    }

    MFFloat	    *newKey = new MFFloat(newK, numKeys + 1);
    FieldValue	    *newKeyValue = createKeys(newV, numKeys + 1);
    
    if (values) {
	CommandList	    *list = new CommandList();

	list->append(new FieldCommand(this, _keyIndex, newKey));
	list->append(new FieldCommand(this, _keyValueIndex, newKeyValue));
	_scene->execute(list);
    } else {
	setField(_keyIndex, newKey);
	setField(_keyValueIndex, newKeyValue);
	_scene->OnFieldChange(this, _keyIndex);
	_scene->OnFieldChange(this, _keyValueIndex);
    }
}

void
Interpolator::deleteKeys(int start, int end)
{
    MFFloat        *key = (MFFloat *) getField(_keyIndex);
    MFFloat	   *keyValue = (MFFloat *) getField(_keyValueIndex);
    int		    numKeys = key->getSize();
    int		    numChannels = getNumChannels();
    const float    *k = key->getValues();
    const float	   *v = keyValue->getValues();

    if (start >= end) return;

    int		    newlen = numKeys - (end - start);
    float	   *newK = new float[newlen];
    float	   *newV = new float[newlen * numChannels];

    if (start > 0) {
	memcpy(newK, k, start * sizeof(float));
	memcpy(newV, v, start * numChannels * sizeof(float));
    }

    if (end < numKeys) {
	memcpy(newK + start, k + end, (numKeys - end) * sizeof(float));
	memcpy(newV + start * numChannels, v + end * numChannels, 
	       (numKeys - end) * numChannels * sizeof(float));
    }

    MFFloat	    *newKey = new MFFloat(newK, newlen);
    FieldValue	    *newKeyValue = createKeys(newV, newlen);

    CommandList	    *list = new CommandList();

    list->append(new FieldCommand(this, _keyIndex, newKey));
    list->append(new FieldCommand(this, _keyValueIndex, newKeyValue));
    _scene->execute(list);
}

void 
Interpolator::recordValue(int key, FieldValue *value)
{
    MFFloat	*keyValue = (MFFloat *) getField(_keyValueIndex);
    keyValue->setSFValue(key, value);
}

//
// recordKey() -- record a key at the current position (_fraction)
//

void
Interpolator::recordKey(FieldValue *value, bool isrunning)
{
    int		 key = findKeyInclusive(_fraction);

    if (key == getNumKeys() || !EQUALF(getKey(key), _fraction)) {
	// no exact key found, create one

        if (isrunning) {
           int oldkey=findKey(_oldFraction);
           if (key>oldkey) {
              deleteKeys(oldkey,key);
              key=key-(key-oldkey);
           }                   

        }
	insertKey(key, _fraction, NULL);
    }
    recordValue(key, value);
    _scene->OnFieldChange(this, _keyIndex);
    _scene->OnFieldChange(this, _keyValueIndex);
    if (isrunning)
       _oldFraction=_fraction;
}

//
// this is a ugly "dynamic_cast<Interpolator *>(Node* node)"
// but works with compilers without or defect rtti implementations...
//

Interpolator       *dynamic_cast_Interpolator(Node* node)
{
    if (node==NULL)
       return NULL;
    if (node->isInterpolator())
       return (Interpolator *)(void *)node;
    else
       return NULL;
}