// %flair:license{
// This file is part of the Flair framework distributed under the
// CECILL-C License, Version 1.0.
// %flair:license}
//  created:    2013/12/19
//  filename:   ParrotNavBoard.cpp
//
//  author:     Guillaume Sanahuja
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    objet integrant les capteurs ardrone2
//
//
/*********************************************************************/

#include "ParrotNavBoard.h"
#include <FrameworkManager.h>
#include <cvmatrix.h>
#include <Imu.h>
#include <ImuData.h>
#include <UsRangeFinder.h>
#include <SerialPort.h>

#define G 9.81

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

namespace {

//from /data/config.ini
const float gyros_gains[3] = { 1.0679195e-03 ,-1.0669589e-03 ,-1.0683396e-03 };
const float acc_gains[3] = { 2.0327346e+00 ,-1.9626701e+00 ,-1.9494846e+00 };

class ParrotImu : public Imu {
public:
    ParrotImu(const FrameworkManager* parent,string name) :Imu(parent,name) {
        //AddDataToLog(imudata);
    }

    ~ParrotImu() {}

    void SetDatas(const Vector3D &rawAcc,const Vector3D &rawMag,const Vector3D &rawGyr,Time time) {
        ImuData* imuData;
        GetDatas(&imuData);
        imuData->SetRawAccMagAndGyr(rawAcc,rawMag,rawGyr);
        imuData->SetDataTime(time);
        UpdateImu();
        ProcessUpdate(imuData);
    }

private:
    void UpdateFrom(const io_data *data){};
};

class ParrotUs : public UsRangeFinder {
public:
    ParrotUs(const FrameworkManager* parent,string name) :UsRangeFinder(parent,name) {}

    ~ParrotUs() {}

    void SetData(float value,Time time) {
        output->SetValue(0,0,value);
        output->SetDataTime(time);
        ProcessUpdate(output);
    }

private:
    void UpdateFrom(const io_data *data){};
};

}

namespace flair { namespace sensor {

ParrotNavBoard::ParrotNavBoard(const FrameworkManager* parent,string name,SerialPort* serialport,uint8_t priority) : Thread(parent,name,priority) {
    this->serialport=serialport;

    us=new ParrotUs(parent,"parrot-us");
    imu=new ParrotImu(parent,"parrot-imu");

    serialport->SetBaudrate(460800);
}

ParrotNavBoard::~ParrotNavBoard() {
    SafeStop();
    Join();

    delete imu;
    delete us;
}

Imu* ParrotNavBoard::GetImu(void) const {
    return imu;
}

UsRangeFinder* ParrotNavBoard::GetUsRangeFinder(void) const {
    return us;
}

void ParrotNavBoard::UseDefaultPlot(void) {
    us->UseDefaultPlot();
    imu->UseDefaultPlot();
}

void ParrotNavBoard::Run(void) {
    uint8_t data[60];
    nav_struct* nav=(nav_struct*)data;
    ssize_t lu=0;

    // stop acquisition
    uint8_t cmd=0x02;
    serialport->Write(&cmd, 1);

    SleepMS(20);
    serialport->FlushInput();

    // start acquisition
    cmd=0x01;
    serialport->Write(&cmd, 1);

    while(!ToBeStopped()) {
        lu+=serialport->Read(data+lu,sizeof(data)-lu);
        if(lu!=sizeof(data)) continue;

        if(data[0]==0x3a) {
            Time time=GetTime();

            if(nav->checksum == Checksum(data)) {
                computeDatas(nav,time);
                lu=0;
            } else {
                Warn("Wrong checksum: got %x, expected %x, time %lld\n",nav->checksum,Checksum(data),time);
                for(int i=0;i<lu;i++) Printf("%x ",data[i]);
                Printf("\n");
                int i;
                for(i=1;i<lu;i++) {//skeep first
                    if(data[i]==0x3a) break;
                }
                printf("ok en %i\n",i);
                int k=0;
                for(int j=i;j<lu;j++) {
                        data[k]=data[j];
                        k++;
                }
                lu=k;
                for(int i=0;i<lu;i++) Printf("%x ",data[i]);
                Printf("\n");
            }
        } else {
            Warn("Wrong header: got %x, expected 0x3a, time %lld\n",data[0],GetTime());
            for(int i=0;i<lu;i++) Printf("%x ",data[i]);
            Printf("\n");
            int i;
            for(i=1;i<lu;i++) {//skeep first
                if(data[i]==0x3a) break;
            }
            printf("ok en %i\n",i);
            int k=0;
            for(int j=i;j<lu;j++) {
                    data[k]=data[j];
                    k++;
            }
            lu=k;
            for(int i=0;i<lu;i++) Printf("%x ",data[i]);
            Printf("\n");
        }
    }
}

void ParrotNavBoard::computeDatas(nav_struct* nav,Time time) {
    static float accs_offset[3]={0,0,0};
    static float gyrs_offset[3]={0,0,0};
    static int cpt=0;

    float ax,ay,az;
    float gx,gy,gz;
    float mx,my,mz;

    if(cpt<2000) { //init offsets
        for(int i=0;i<3;i++) accs_offset[i]+=nav->acc[i];
        for(int i=0;i<3;i++) gyrs_offset[i]+=nav->gyro[i];
        cpt++;
        ax=0;
        ay=0;
        az=-G;
        gx=0;
        gy=0;
        gz=0;
        mx=0;
        my=0;
        mz=0;
    } else {
        //ax=-G*(nav->acc[0]-accs_offset[0]/cpt)/512.;
        //ay=G*(nav->acc[1]-accs_offset[1]/cpt)/512.;
        //az=G*(-(nav->acc[2]-accs_offset[2]/cpt)/512.-1);
        ax=acc_gains[0]*(nav->acc[0]-accs_offset[0]/cpt)/100.;//gains from parrot are in cm.s-2
        ay=acc_gains[1]*(nav->acc[1]-accs_offset[1]/cpt)/100.;
        az=acc_gains[2]*((nav->acc[2]-accs_offset[2]/cpt)/100.)-G;

        gx=(nav->gyro[0]-gyrs_offset[0]/cpt)*gyros_gains[0];
        gy=(nav->gyro[1]-gyrs_offset[1]/cpt)*gyros_gains[1];
        gz=(nav->gyro[2]-gyrs_offset[2]/cpt)*gyros_gains[2];

        mx=nav->mag[0];
        my=nav->mag[1];
        mz=nav->mag[2];
    }

    ((ParrotImu*)imu)->SetDatas(Vector3D(ax,ay,az),
                                Vector3D(mx,my,mz),
                                Vector3D(gx,gy,gz),
                                time
                                );


     if(((nav->us_echo)&0x8000)==0x8000) {
        float h  = (float)(((nav->us_echo)&0x7fff)) * 0.0340/100.;
        ((ParrotUs*)us)->SetData(h,time);
    }
}
uint16_t ParrotNavBoard::Checksum(const uint8_t *data) const{
    uint16_t checksum = 0;
    for(int i = 2; i < 60-2; i += 2) {
        checksum += data[i] + (data[i+1] << 8);
    }
    return checksum;
}

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