//  created:    2012/04/12
//  filename:   OpticalFlowData.cpp
//
//  author:     Guillaume Sanahuja
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    classe pour les données du flux optique
//
/*********************************************************************/

#include "OpticalFlowData.h"
#include "VisionFilter.h"
#include <string.h>

using std::string;

class OpticalFlowData_impl {
public:
    OpticalFlowData_impl(flair::filter::OpticalFlowData* self,uint32_t max_features) {
        this->self=self;
        this->max_features=max_features;
        nb_features=0;
        pointsA=(IntPoint*)malloc(max_features*sizeof(IntPoint));
        pointsB=(FloatPoint*)malloc(max_features*sizeof(FloatPoint));

        found_features=(char*)malloc(max_features*sizeof(char));
        features_error=(uint32_t*)malloc(max_features*sizeof(uint32_t));
    }

    ~OpticalFlowData_impl() {
        free((char*)pointsA);
        free((char*)pointsB);

        free((char*)found_features);
        free((char*)features_error);
    }
    
    void Resize(uint32_t value) {
        IntPoint *new_pointsA;
        FloatPoint *new_pointsB;
        char *new_found_features;
        uint32_t *new_features_error;

        new_pointsA=(IntPoint*)malloc(value*sizeof(IntPoint));
        new_pointsB=(FloatPoint*)malloc(value*sizeof(FloatPoint));
        new_found_features=(char*)malloc(value*sizeof(char));
        new_features_error=(uint32_t*)malloc(value*sizeof(uint32_t));

        self->GetMutex();
        if(value>max_features) {
            memcpy(new_pointsA,pointsA,max_features*sizeof(IntPoint));
            memcpy(new_pointsB,pointsB,max_features*sizeof(FloatPoint));
            memcpy(new_found_features,found_features,max_features*sizeof(char));
            memcpy(new_features_error,features_error,max_features*sizeof(uint32_t));
        } else {
            memcpy(new_pointsA,pointsA,value*sizeof(IntPoint));
            memcpy(new_pointsB,pointsB,value*sizeof(FloatPoint));
            memcpy(new_found_features,found_features,value*sizeof(char));
            memcpy(new_features_error,features_error,value*sizeof(uint32_t));
            if(nb_features>value) nb_features=value; //si nb_features est le nombre de point qui ont effectivement une correspondande, ça me parait louche. Sauf si les points sont classés et que ceux qui ont une correpondance sont toujours au début
        }
        max_features=value;
        self->ReleaseMutex();

        free((char*)pointsA);
        free((char*)pointsB);
        free((char*)found_features);
        free((char*)features_error);

        pointsA=new_pointsA;
        pointsB=new_pointsB;
        found_features=new_found_features;
        features_error=new_features_error;
    }
        
    uint32_t max_features;
    uint32_t nb_features;
    IntPoint* pointsA;
    FloatPoint* pointsB;
    char *found_features;
    uint32_t *features_error;

private:
    flair::filter::OpticalFlowData *self;
};

namespace flair {
namespace filter {

OpticalFlowData::OpticalFlowData(const Object* parent,uint32_t max_features,string name,uint32_t n): io_data(parent,name,n) {
    pimpl_=new OpticalFlowData_impl(this,max_features);
}

OpticalFlowData::~OpticalFlowData() {
    delete pimpl_;
}

IntPoint* OpticalFlowData::PointsA(void) const {
    return pimpl_->pointsA;
}

FloatPoint* OpticalFlowData::PointsB(void) const {
    return pimpl_->pointsB;
}

char *OpticalFlowData::FoundFeature(void) const {
    return pimpl_->found_features;
}

uint32_t *OpticalFlowData::FeatureError(void) const {
    return pimpl_->features_error;
}

// value is new max_features value
void OpticalFlowData::Resize(uint32_t value) {
    pimpl_->Resize(value);
}

void OpticalFlowData::RawRead(char* dst) const {
    Warn("non implementé\n");
}

void OpticalFlowData::SetPointsA(const IntPoint* points) {
    GetMutex();
    memcpy(pimpl_->pointsA,points,pimpl_->max_features*sizeof(IntPoint));
    ReleaseMutex();
}

void OpticalFlowData::SetPointsB(const FloatPoint* points) {
    GetMutex();
    memcpy(pimpl_->pointsB,points,pimpl_->max_features*sizeof(FloatPoint));
    ReleaseMutex();
}

void OpticalFlowData::SetFoundFeature(const char *found_features) {
    GetMutex();
    memcpy(pimpl_->found_features,found_features,pimpl_->max_features*sizeof(char));
    ReleaseMutex();
}

void OpticalFlowData::SetFeatureError(const uint32_t *features_error) {
    GetMutex();
    memcpy(pimpl_->features_error,features_error,pimpl_->max_features*sizeof(uint32_t));
    ReleaseMutex();
}

uint32_t OpticalFlowData::MaxFeatures(void) const {
    return pimpl_->max_features;
}

uint32_t OpticalFlowData::NbFeatures(void) const {
    return pimpl_->nb_features;
}

void OpticalFlowData::SetNbFeatures(uint32_t value) {
    GetMutex();
    pimpl_->nb_features=value;
    ReleaseMutex();
}

} // end namespace filter
} // end namespace flair