// %flair:license{
// This file is part of the Flair framework distributed under the
// CECILL-C License, Version 1.0.
// %flair:license}
//  created:    2020/&2/09
//  filename:   SumoUgvControls.cpp
//
//  author:     Guillaume Sanahuja
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    Class for a parrot sumo ugv controls
//
//
/*********************************************************************/
#ifdef CORE2_64

#include "SumoUgvControls.h"
#include <Matrix.h>
#include <control.h>
#include <image.h>

using std::string;
using namespace sumo;

class ImageProcessing : public sumo::Image {
public:
	void handleImage(const struct sumo::image *, const uint8_t *buffer, size_t size) {
		//nothing to do in this demo
	}
};

namespace flair {
namespace actuator {

SumoUgvControls::SumoUgvControls(string name)
    : UgvControls(name) {
  
  sumoControl= new Control (new ImageProcessing);
  if (!sumoControl->open()) {
    Err("error opening sumo control\n");
  } else {
    SetIsReady(true);
  }
}

SumoUgvControls::~SumoUgvControls() {
  sumoControl->close();
  delete sumoControl;
}


void SumoUgvControls::SetControls(float speed,float turn) {
  sumoControl->move(speed*127, turn*127);

  // on prend une fois pour toute le mutex et on fait des accès directs
  output->GetMutex();
  output->SetValueNoMutex(0, 0, speed);
  output->SetValueNoMutex(1, 0, turn);
  output->ReleaseMutex();
}


} // end namespace sensor
} // end namespace flair

#endif