// Filename: movingPartMatrix.cxx
// Created by:  drose (23Feb99)
//
////////////////////////////////////////////////////////////////////
//
// PANDA 3D SOFTWARE
// Copyright (c) Carnegie Mellon University.  All rights reserved.
//
// All use of this software is subject to the terms of the revised BSD
// license.  You should have received a copy of this license along
// with this source code in a file named "LICENSE."
//
////////////////////////////////////////////////////////////////////


#include "movingPartMatrix.h"
#include "animChannelMatrixDynamic.h"
#include "animChannelMatrixFixed.h"
#include "compose_matrix.h"
#include "datagram.h"
#include "datagramIterator.h"
#include "bamReader.h"
#include "bamWriter.h"

// Tell GCC that we'll take care of the instantiation explicitly here.
#ifdef __GNUC__
#pragma implementation
#endif

TypeHandle MovingPartMatrix::_type_handle;

////////////////////////////////////////////////////////////////////
//     Function: MovingPartMatrix::Destructor
//       Access: Public, Virtual
//  Description: 
////////////////////////////////////////////////////////////////////
MovingPartMatrix::
~MovingPartMatrix() {
}


////////////////////////////////////////////////////////////////////
//     Function: MovingPartMatrix::make_default_channel
//       Access: Public, Virtual
//  Description: Creates and returns a new AnimChannel that is not
//               part of any hierarchy, but that returns the default
//               value associated with this part.
////////////////////////////////////////////////////////////////////
AnimChannelBase *MovingPartMatrix::
make_default_channel() const {
  LVecBase3 pos, hpr, scale, shear;
  decompose_matrix(_default_value, pos, hpr, scale, shear);
  return new AnimChannelMatrixFixed(get_name(), pos, hpr, scale);
}

////////////////////////////////////////////////////////////////////
//     Function: MovingPartMatrix::get_blend_value
//       Access: Public
//  Description: Attempts to blend the various matrix values
//               indicated, and sets the _value member to the
//               resulting matrix.
////////////////////////////////////////////////////////////////////
void MovingPartMatrix::
get_blend_value(const PartBundle *root) {
  // If a forced channel is set on this particular joint, we always
  // return that value instead of performing the blend.  Furthermore,
  // the frame number is always 0 for the forced channel.
  if (_forced_channel != (AnimChannelBase *)NULL) {
    ChannelType *channel = DCAST(ChannelType, _forced_channel);
    channel->get_value(0, _value);
    return;
  }

  PartBundle::CDReader cdata(root->_cycler);

  if (cdata->_blend.empty()) {
    // No channel is bound; supply the default value.
    if (restore_initial_pose) {
      _value = _default_value;
    }

  } else if (_effective_control != (AnimControl *)NULL && 
             !cdata->_frame_blend_flag) {
    // A single value, the normal case.
    ChannelType *channel = DCAST(ChannelType, _effective_channel);
    channel->get_value(_effective_control->get_frame(), _value);

  } else {
    // A blend of two or more values, either between multiple
    // different animations, or between consecutive frames of the same
    // animation (or both).
    switch (cdata->_blend_type) {
    case PartBundle::BT_linear:
      {
        // An ordinary, linear blend.
        LMatrix4 net_value = LMatrix4::zeros_mat();
        PN_stdfloat net_effect = 0.0f;
        
        PartBundle::ChannelBlend::const_iterator cbi;
        for (cbi = cdata->_blend.begin(); cbi != cdata->_blend.end(); ++cbi) {
          AnimControl *control = (*cbi).first;
          PN_stdfloat effect = (*cbi).second;
          nassertv(effect != 0.0f);
          
          int channel_index = control->get_channel_index();
          nassertv(channel_index >= 0 && channel_index < (int)_channels.size());
          ChannelType *channel = DCAST(ChannelType, _channels[channel_index]);
          if (channel != (ChannelType *)NULL) {
            ValueType v;
            channel->get_value(control->get_frame(), v);

            if (!cdata->_frame_blend_flag) {
              // Hold the current frame until the next one is ready.
              net_value += v * effect;
            } else {
              // Blend between successive frames.
              PN_stdfloat frac = (PN_stdfloat)control->get_frac();
              net_value += v * (effect * (1.0f - frac));

              channel->get_value(control->get_next_frame(), v);
              net_value += v * (effect * frac);
            }
            net_effect += effect;
          }
        }
        
        if (net_effect == 0.0f) {
          if (restore_initial_pose) {
            _value = _default_value;
          }
        } else {
          _value = net_value / net_effect;
        }
      }
      break;

    case PartBundle::BT_normalized_linear:
      {
        // A normalized linear blend.  This means we do a linear blend
        // without scales or shears, normalize the scale and shear
        // components of the resulting matrix to eliminate
        // artificially-introduced scales, and then reapply the
        // scales and shears.
        
        LMatrix4 net_value = LMatrix4::zeros_mat();
        LVecBase3 scale(0.0f, 0.0f, 0.0f);
        LVecBase3 shear(0.0f, 0.0f, 0.0f);
        PN_stdfloat net_effect = 0.0f;
        
        PartBundle::ChannelBlend::const_iterator cbi;
        for (cbi = cdata->_blend.begin(); cbi != cdata->_blend.end(); ++cbi) {
          AnimControl *control = (*cbi).first;
          PN_stdfloat effect = (*cbi).second;
          nassertv(effect != 0.0f);
          
          ChannelType *channel = NULL;
          int channel_index = control->get_channel_index();
          if (channel_index >= 0 && channel_index < (int)_channels.size()) {
            channel = DCAST(ChannelType, _channels[channel_index]);
          }
          if (channel != (ChannelType *)NULL) {
            int frame = control->get_frame();
            ValueType v;
            LVecBase3 iscale, ishear;
            channel->get_value_no_scale_shear(frame, v);
            channel->get_scale(frame, iscale);
            channel->get_shear(frame, ishear);
            
            if (!cdata->_frame_blend_flag) {
              // Hold the current frame until the next one is ready.
              net_value += v * effect;
              scale += iscale * effect;
              shear += ishear * effect;
            } else {
              // Blend between successive frames.
              PN_stdfloat frac = (PN_stdfloat)control->get_frac();
              PN_stdfloat e0 = effect * (1.0f - frac);
              net_value += v * e0;
              scale += iscale * e0;
              shear += ishear * e0;

              int next_frame = control->get_next_frame();
              channel->get_value_no_scale_shear(next_frame, v);
              channel->get_scale(next_frame, iscale);
              channel->get_shear(next_frame, ishear);
              PN_stdfloat e1 = effect * frac;
              net_value += v * e1;
              scale += iscale * e1;
              shear += ishear * e1;
            }
            net_effect += effect;
          }
        }
        
        if (net_effect == 0.0f) {
          if (restore_initial_pose) {
            _value = _default_value;
          }

        } else {
          net_value /= net_effect;
          scale /= net_effect;
          shear /= net_effect;
          
          // Now rebuild the matrix with the correct scale values.
          
          LVector3 false_scale, false_shear, hpr, translate;
          decompose_matrix(net_value, false_scale, false_shear, hpr, translate);
          compose_matrix(_value, scale, shear, hpr, translate);
        }
      }
      break;

    case PartBundle::BT_componentwise:
      {
        // Componentwise linear, including componentwise H, P, and R.
        LVecBase3 scale(0.0f, 0.0f, 0.0f);
        LVecBase3 hpr(0.0f, 0.0f, 0.0f);
        LVecBase3 pos(0.0f, 0.0f, 0.0f);
        LVecBase3 shear(0.0f, 0.0f, 0.0f);
        PN_stdfloat net_effect = 0.0f;
        
        PartBundle::ChannelBlend::const_iterator cbi;
        for (cbi = cdata->_blend.begin(); cbi != cdata->_blend.end(); ++cbi) {
          AnimControl *control = (*cbi).first;
          PN_stdfloat effect = (*cbi).second;
          nassertv(effect != 0.0f);
          
          ChannelType *channel = NULL;
          int channel_index = control->get_channel_index();
          if (channel_index >= 0 && channel_index < (int)_channels.size()) {
            channel = DCAST(ChannelType, _channels[channel_index]);
          }
          if (channel != (ChannelType *)NULL) {
            int frame = control->get_frame();
            LVecBase3 iscale, ihpr, ipos, ishear;
            channel->get_scale(frame, iscale);
            channel->get_hpr(frame, ihpr);
            channel->get_pos(frame, ipos);
            channel->get_shear(frame, ishear);
            
            if (!cdata->_frame_blend_flag) {
              // Hold the current frame until the next one is ready.
              scale += iscale * effect;
              hpr += ihpr * effect;
              pos += ipos * effect;
              shear += ishear * effect;
            } else {
              // Blend between successive frames.
              PN_stdfloat frac = (PN_stdfloat)control->get_frac();
              PN_stdfloat e0 = effect * (1.0f - frac);

              scale += iscale * e0;
              hpr += ihpr * e0;
              pos += ipos * e0;
              shear += ishear * e0;

              int next_frame = control->get_next_frame();
              channel->get_scale(next_frame, iscale);
              channel->get_hpr(next_frame, ihpr);
              channel->get_pos(next_frame, ipos);
              channel->get_shear(next_frame, ishear);
              PN_stdfloat e1 = effect * frac;

              scale += iscale * e1;
              hpr += ihpr * e1;
              pos += ipos * e1;
              shear += ishear * e1;
            }
            net_effect += effect;
          }
        }
        
        if (net_effect == 0.0f) {
          if (restore_initial_pose) {
            _value = _default_value;
          }

        } else {
          scale /= net_effect;
          hpr /= net_effect;
          pos /= net_effect;
          shear /= net_effect;
          
          compose_matrix(_value, scale, shear, hpr, pos);
        }
      }
      break;

    case PartBundle::BT_componentwise_quat:
      {
        // Componentwise linear, except for rotation, which is a
        // quaternion.
        LVecBase3 scale(0.0f, 0.0f, 0.0f);
        LQuaternion quat(0.0f, 0.0f, 0.0f, 0.0f);
        LVecBase3 pos(0.0f, 0.0f, 0.0f);
        LVecBase3 shear(0.0f, 0.0f, 0.0f);
        PN_stdfloat net_effect = 0.0f;
        
        PartBundle::ChannelBlend::const_iterator cbi;
        for (cbi = cdata->_blend.begin(); cbi != cdata->_blend.end(); ++cbi) {
          AnimControl *control = (*cbi).first;
          PN_stdfloat effect = (*cbi).second;
          nassertv(effect != 0.0f);
          
          ChannelType *channel = NULL;
          int channel_index = control->get_channel_index();
          if (channel_index >= 0 && channel_index < (int)_channels.size()) {
            channel = DCAST(ChannelType, _channels[channel_index]);
          }
          if (channel != (ChannelType *)NULL) {
            int frame = control->get_frame();
            LVecBase3 iscale, ipos, ishear;
            LQuaternion iquat;
            channel->get_scale(frame, iscale);
            channel->get_quat(frame, iquat);
            channel->get_pos(frame, ipos);
            channel->get_shear(frame, ishear);
            
            if (!cdata->_frame_blend_flag) {
              // Hold the current frame until the next one is ready.
              scale += iscale * effect;
              quat += iquat * effect;
              pos += ipos * effect;
              shear += ishear * effect;

            } else {
              // Blend between successive frames.
              PN_stdfloat frac = (PN_stdfloat)control->get_frac();
              PN_stdfloat e0 = effect * (1.0f - frac);

              scale += iscale * e0;
              quat += iquat * e0;
              pos += ipos * e0;
              shear += ishear * e0;

              int next_frame = control->get_next_frame();
              channel->get_scale(next_frame, iscale);
              channel->get_quat(next_frame, iquat);
              channel->get_pos(next_frame, ipos);
              channel->get_shear(next_frame, ishear);
              PN_stdfloat e1 = effect * frac;

              scale += iscale * e1;
              quat += iquat * e1;
              pos += ipos * e1;
              shear += ishear * e1;
            }
            net_effect += effect;
          }
        }
        
        if (net_effect == 0.0f) {
          if (restore_initial_pose) {
            _value = _default_value;
          }

        } else {
          scale /= net_effect;
          quat /= net_effect;
          pos /= net_effect;
          shear /= net_effect;
          
          // There should be no need to normalize the quaternion,
          // assuming all of the input quaternions were already
          // normalized.
          
          _value = LMatrix4::scale_shear_mat(scale, shear) * quat;
          _value.set_row(3, pos);
        }
      }
      break;
    }
  }
}

////////////////////////////////////////////////////////////////////
//     Function: MovingPartMatrix::apply_freeze_matrix
//       Access: Public, Virtual
//  Description: Freezes this particular joint so that it will always
//               hold the specified transform.  Returns true if this
//               is a joint that can be so frozen, false otherwise.
//               This is called internally by
//               PartBundle::freeze_joint().
////////////////////////////////////////////////////////////////////
bool MovingPartMatrix::
apply_freeze_matrix(const LVecBase3 &pos, const LVecBase3 &hpr, const LVecBase3 &scale) {
  _forced_channel = new AnimChannelMatrixFixed(get_name(), pos, hpr, scale);
  return true;
}

////////////////////////////////////////////////////////////////////
//     Function: MovingPartMatrix::apply_control
//       Access: Public, Virtual
//  Description: Specifies a node to influence this particular joint
//               so that it will always hold the node's transform.
//               Returns true if this is a joint that can be so
//               controlled, false otherwise.  This is called
//               internally by PartBundle::control_joint().
////////////////////////////////////////////////////////////////////
bool MovingPartMatrix::
apply_control(PandaNode *node) {
  AnimChannelMatrixDynamic *chan = new AnimChannelMatrixDynamic(get_name());
  chan->set_value_node(node);
  _forced_channel = chan;
  return true;
}

////////////////////////////////////////////////////////////////////
//     Function: MovingPartMatrix::make_MovingPartMatrix
//       Access: Protected
//  Description: Factory method to generate a MovingPartMatrix object
////////////////////////////////////////////////////////////////////
TypedWritable* MovingPartMatrix::
make_MovingPartMatrix(const FactoryParams &params) {
  MovingPartMatrix *me = new MovingPartMatrix;
  DatagramIterator scan;
  BamReader *manager;

  parse_params(params, scan, manager);
  me->fillin(scan, manager);
  return me;
}

////////////////////////////////////////////////////////////////////
//     Function: MovingPartMatrix::register_with_factory
//       Access: Public, Static
//  Description: Factory method to generate a MovingPartMatrix object
////////////////////////////////////////////////////////////////////
void MovingPartMatrix::
register_with_read_factory() {
  BamReader::get_factory()->register_factory(get_class_type(), make_MovingPartMatrix);
}
