//  created:    2012/04/12
//  filename:   OpticalFlowSpeed.cpp
//
//  author:     Guillaume Sanahuja
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    calcul de la vitesse à partir du flux optique
//
//
/*********************************************************************/

#include "OpticalFlowSpeed.h"
#include "OpticalFlowData.h"
#include <cvmatrix.h>
#include <Object.h>

using std::string;
using namespace flair::core;

namespace flair
{
namespace filter
{

OpticalFlowSpeed::OpticalFlowSpeed(const IODevice* parent,string name) : IODevice(parent,name)
{
    cvmatrix_descriptor* desc=new cvmatrix_descriptor(2,1);
    desc->SetElementName(0,0,"vx");
	desc->SetElementName(1,0,"vy");
    output=new cvmatrix(this,desc,floatType,name);

	AddDataToLog(output);
}

OpticalFlowSpeed::~OpticalFlowSpeed(void)
{
}

void OpticalFlowSpeed::UpdateFrom(const io_data *data)
{
    OpticalFlowData *input=(OpticalFlowData*)data;
    float deplx,deply;
    int nb_depl=0;

    deplx=0;
    deply=0;

    input->GetMutex();
    for(int i=0;i<input->NbFeatures();i++)
    {
        if(input->FoundFeature()[i]!=0)
        {
            deplx+=input->PointsB()[i].x-input->PointsA()[i].x;
            deply+=input->PointsB()[i].y-input->PointsA()[i].y;
            nb_depl++;
        }
    }
    input->ReleaseMutex();

    if(nb_depl!=0)
    {
        output->SetValue(0,0,deplx/nb_depl);
        output->SetValue(1,0,deply/nb_depl);
    }

    output->SetDataTime(data->DataTime());
    ProcessUpdate(output);
}

float OpticalFlowSpeed::Vx(void) const
{
    return output->Value(0,0);
}

float OpticalFlowSpeed::Vy(void) const
{
    return output->Value(1,0);
}

core::cvmatrix *OpticalFlowSpeed::Output() const
{
    return output;
}
} // end namespace filter
} // end namespace flair