// %flair:license{
// This file is part of the Flair framework distributed under the
// CECILL-C License, Version 1.0.
// %flair:license}
//  created:    2011/09/13
//  filename:   BlCtrlV2_impl.cpp
//
//  author:     Guillaume Sanahuja
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    objet integrant les moteurs i2c
//
//
/*********************************************************************/
#include "BlCtrlV2_impl.h"
#include "BlCtrlV2.h"
#include "Bldc_impl.h"
#include "BatteryMonitor.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;

BlCtrlV2_impl::BlCtrlV2_impl(BlCtrlV2* self,Layout *layout,I2cPort* i2cport) {
    this->self=self;
	this->i2cport=i2cport;
	last_voltage_time=0;

    DetectMotors();
    //if(nb_mot!=self->MotorsCount()) self->Err("motors count different from multiplex count\n");

    //BatteryMonitor
    battery=new BatteryMonitor(layout->NewRow(),"battery");

    //user interface
    GroupBox *setupgroupbox=new GroupBox(layout->NewRow(),"Motors");
    poles=new SpinBox(setupgroupbox->NewRow(),"nb poles:",0,255,1,14);
}

BlCtrlV2_impl::~BlCtrlV2_impl() {

}

void BlCtrlV2_impl::SetMotors(float* value) {
    uint16_t tosend_value[nb_mot];
    //stocke dans une variable pour garder le moins longtemps l'i2c (pour ne pas bloquer sur le mutex de l'output)
    float speeds[nb_mot];
    float currents[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]);
    }

    for(int i=0;i<nb_mot;i++) {
        i2cport->SetSlave(BASE_ADDRESS+i);

        if(i==0 && GetTime()>(last_voltage_time+5*(Time)1000000000)) {
            //toute les 5 secondes sur moteur 0
            float voltage;
            GetCurrentSpeedAndVoltage(currents[i],speeds[i],voltage);
            battery->SetBatteryValue(voltage);
            last_voltage_time=GetTime();
        } else {
            GetCurrentAndSpeed(currents[i],speeds[i]);
        }
    }
//printf("%f %f %f %f\n",speeds[0],speeds[1],speeds[2],speeds[3]);
/*
    if(GetTime()>(last_voltage_time+5*(Time)1000000000))//toute les 5 secondes
    {
        i2cport->SetSlave(BASE_ADDRESS);
        battery->SetBatteryValue(ReadVoltage());
        last_voltage_time=GetTime();
    }
*/
    i2cport->ReleaseMutex();

    //on prend une fois pour toute le mutex et on fait des accès directs
    cvmatrix* output=self->output;
    output->GetMutex();
    for(int i=0;i<nb_mot;i++) output->SetValueNoMutex(i,0,speeds[i]);
    for(int i=0;i<nb_mot;i++) output->SetValueNoMutex(i,1,currents[i]);
    output->ReleaseMutex();
}

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

    tx[0]=(uint8_t)(value>>3);//msb
    tx[1]=(value&0x07);;//+16+8; //pour recuperer la vitesse en premier
    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));
    }
}

//I2cPort mutex must be taken before calling this function
void BlCtrlV2_impl::GetCurrentAndSpeed(float &current,float &speed) {
    ssize_t read;
    uint8_t value[4];
    read = i2cport->Read(value, sizeof(value));

    if(read<0) {
        self->Err("rt_dev_read error (%s)\n",strerror(-read));
        speed=-1;
        current=-1;
    } else if (read != sizeof(value)) {
        self->Err("rt_dev_read error %i/%i\n",read,sizeof(value));
        speed=-1;
        current=-1;
    } else {
        current=value[0]/10.;
        speed= value[3]*780./poles->Value();
    }
}

//I2cPort mutex must be taken before calling this function
void BlCtrlV2_impl::GetCurrentSpeedAndVoltage(float &current,float &speed,float &voltage) {
    ssize_t read;
    uint8_t value[6];
    read = i2cport->Read(value, sizeof(value));

    if(read<0) {
        self->Err("rt_dev_read error (%s)\n",strerror(-read));
        speed=-1;
        voltage=-1;
        current=-1;
    } else if (read != sizeof(value)) {
        self->Err("rt_dev_read error %i/%i\n",read,sizeof(value));
        speed=-1;
        voltage=-1;
        current=-1;
    } else {
        current=value[0]/10.;
        voltage=value[5]/10.;
        speed= value[3]*780./poles->Value();
    }
}

void BlCtrlV2_impl::DetectMotors(void) {
    int nb=0;
    ssize_t read,nb_write;
    uint8_t value[3];
    uint8_t tx[2];

    i2cport->GetMutex();

    for(int i=0;i<MAX_MOTORS;i++) {
        //printf("test %i\n",i);
        i2cport->SetSlave(BASE_ADDRESS+i);
        tx[0]=0;
        tx[1]=16+8;//16+8 pour recuperer la vitesse

        nb_write = i2cport->Write(tx, 2);

        if (nb_write != sizeof(tx)) {
            continue;
        }
        nb++;
    }

    for(int i=0;i<MAX_MOTORS;i++) {
        i2cport->SetSlave(BASE_ADDRESS+i);
        read = i2cport->Read(value, 3);

        if (read != sizeof(value)) {
            continue;
        }
    }

    i2cport->ReleaseMutex();

    Printf("BlCtrlV2: Dectected motors: %i\n",nb);
    if(nb==4) {
        Printf("BlCtrlV2: Configuration X4\n");
    } else if(nb==8) {
        Printf("BlCtrlV2: Configuration X8\n");
    } else {
        //self->Err("Error, configuration not supported (%i/%i)\n",nb,MAX_MOTORS);
    }

    nb_mot=nb;
}