// %flair:license{
// This file is part of the Flair framework distributed under the
// CECILL-C License, Version 1.0.
// %flair:license}
//  created:    2015/06/15
//  filename:   AfroBldc_impl.cpp
//
//  author:     Guillaume Sanahuja
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    objet integrant les moteurs i2c
//
//
/*********************************************************************/
#include "AfroBldc_impl.h"
#include "AfroBldc.h"
#include "Bldc_impl.h"
#include "I2cPort.h"
#include <Layout.h>
#include <SpinBox.h>
#include <Label.h>
#include <cvmatrix.h>
#include <string.h>

#define BASE_ADDRESS 0x29
#define MAX_VALUE 2047

using std::string;
using namespace flair::core;
using namespace flair::gui;
using namespace flair::sensor;
using namespace flair::actuator;

AfroBldc_impl::AfroBldc_impl(AfroBldc* self,Layout *layout,I2cPort* i2cport) {
    this->self=self;
    this->i2cport=i2cport;
    nb_mot=self->MotorsCount();
}

AfroBldc_impl::~AfroBldc_impl() {

}

void AfroBldc_impl::SetMotors(float* value) {
    uint16_t tosend_value[nb_mot];

    for(int i=0; i<nb_mot; i++) tosend_value[i]=(uint16_t)(MAX_VALUE*value[i]);

    i2cport->GetMutex();

    for(int i=0; i<nb_mot; i++) {
        i2cport->SetSlave(BASE_ADDRESS+i);
        WriteValue(tosend_value[i]);
    }
    i2cport->ReleaseMutex();
}

//I2cPort mutex must be taken before calling this function
void AfroBldc_impl::WriteValue(uint16_t value) {
    uint8_t tx[2];
    ssize_t written;

    tx[0]=(uint8_t)(value>>3);//msb
    tx[1]=(value&0x07);//lsb
    written =i2cport->Write(tx, 2);
    if(written<0) {
        self->Err("rt_dev_write error (%s)\n",strerror(-written));
    } else if (written != sizeof(tx)) {
        self->Err("rt_dev_write error %i/%i\n",written,sizeof(tx));
    }
}