// %flair:license{
// This file is part of the Flair framework distributed under the
// CECILL-C License, Version 1.0.
// %flair:license}

//  created:    2013/08/23
//  filename:   HokuyoUTM30Lx.cpp
//
//  author:     Cesar Richard
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    classe intégrant le telemetre laser Hokuyo UTM 30lx
//
//
/*********************************************************************/

#include "HokuyoUTM30Lx.h"
#include <SerialPort.h>
#include <FrameworkManager.h>
#include <RangeFinderPlot.h>
#include <cvmatrix.h>
#include <Tab.h>
#include <sstream>
#include <iomanip>
#include <string.h>
#include <iostream>

using namespace std;
using namespace flair::core;
using namespace flair::gui;

namespace flair
{
namespace sensor
{

HokuyoUTM30Lx::HokuyoUTM30Lx(const FrameworkManager* parent,string name,SerialPort *serialport,uint8_t priority) : LaserRangeFinder(parent,name), Thread(parent,name,priority)
{
    main_tab=new Tab(parent->GetTabWidget(),name);
	cvmatrix_descriptor* desc=new cvmatrix_descriptor(1081,1);
    output=new cvmatrix((IODevice*)this,desc,SignedIntegerType(16));

    bufRetMut = new Mutex(reinterpret_cast<Object*>(this),(string)name+"BufRetMut");
    sendingCmdMut = new Mutex(reinterpret_cast<Object*>(this),(string)name+"SendingCmdMut");

    this->serialport=serialport;
    serialport->SetRxTimeout(100000000);
}

HokuyoUTM30Lx::~HokuyoUTM30Lx()
{
    delete main_tab;
    SafeStop();
    Join();
}

void HokuyoUTM30Lx::Run(void)
{
	/** Debut init **/
    char c,lastC;
    int startStep;
    bool mustDecode=false;
    stringstream ss;
    vector<string> ret;
    lastC=c=0;

	/** Fin init **/
	resetConfig();
	startLaser();
	Printf("Laser started\r\n");

    /** Debut running loop **/
    WarnUponSwitches(true);

    while(!ToBeStopped())
    {
        ss.clear();
        ss.str("");
        do{
            lastC=c;
            serialport->Read(&c,1);
            ss << c;
        }while(!(c=='\n'&&lastC=='\n'));
        ret=explode(ss.str(),'\n');
        if(!checkSum(ret.at(1)))
            perror("Bad Checksum !");
        if(ret.at(0).substr(0,2)=="MD")
            mustDecode=true;

        startStep=atoi(ret.at(0).substr(2,4).c_str());
        ret.erase(ret.begin());
        bufRetMut->GetMutex();
        bufRet.push(ret.at(0).substr(0,2));
        bufRetMut->ReleaseMutex();
        if(mustDecode){
            mustDecode=false;
            ss.clear();
            ss.str("");
            do{
                lastC=c;
                serialport->Read(&c,1);
                ss << c;
            }while(!(c=='\n'&&lastC=='\n'));
            ret=explode(ss.str(),'\n');
            ret.erase(ret.begin());
            ret.erase(ret.begin());
            if(ret.at(0).size()==5)
                if(!checkSum(ret.at(0)))
                    perror("TimeStamp checksum error!");
            ret.erase(ret.begin());
            Printf("!post\r\n");
            ret.pop_back();
            decodeDatas(ret, startStep);
        }
    }
    /** fin running loop **/
    stopLaser();
    WarnUponSwitches(false);
}


void HokuyoUTM30Lx::decodeDatas(vector<string> datas, int startStep){
            stringstream ss;
            for (unsigned i=0; i<datas.size(); i++){
                if(datas.at(i).size()!=0){
                    if(!checkSum(datas.at(i)))
                        perror("Datas checksum error !");
                    datas.at(i).erase(datas.at(i).end()-1);
                    ss << datas.at(i);
                }
            }
            output->GetMutex();
            for(unsigned i=0;i<1080;i++){
            //TODO: remettre -1 pour les points non lus
                output->SetValueNoMutex(i,0,0);
            }
            for(unsigned i=0;i<ss.str().size();i+=3){
                output->SetValueNoMutex(startStep+(i/3),0,decode3car(ss.str().substr(i,3).c_str()));
            }
            UpdateFrom(output);
            ProcessUpdate(output);
            output->ReleaseMutex();

}

bool HokuyoUTM30Lx::checkSum(string data){
    return (char)encodeSum(data.substr(0,data.size()-1).c_str(),data.size()-1)==data.at(data.size()-1);
}

void HokuyoUTM30Lx::startLaser(){
    string ret = sendCommand("BM");
    #ifdef VERBOSE
        if(ret == "00"){
            cout << "BM: Alright !" << endl;
        }else if(ret == "01"){
            cout << "BM: Laser malfunction !" << endl;
        }else if(ret == "02"){
            cout << "BM: Laser already started !" << endl;
        }
    #endif
}

void HokuyoUTM30Lx::stopLaser(){
    sendCommand("QT");
}

void HokuyoUTM30Lx::resetConfig(){
    sendCommand("RS");
}

vector<string> HokuyoUTM30Lx::explode(const string str, char delimiter){
    istringstream split(str);
    vector<string> tokens;
    for (string each; getline(split, each, delimiter); tokens.push_back(each));
    return tokens;
}

int HokuyoUTM30Lx::encodeSum(const char* code, int byte) {
    unsigned char value = 0;
    int i;
    for (i = 0; i < byte; ++i) {
        value+=code[i];
    }
    value &= 0x3f;
    value += 0x30;
    return value;
}

float HokuyoUTM30Lx::decode2car(const char* data){
    return ((data[0]-0x30)<<6)|((data[1]-0x30));
}
float HokuyoUTM30Lx::decode3car(const char* data){
    return ((data[0]-0x30)<<12)|((data[1]-0x30)<<6)|((data[2]-0x30));
}
float HokuyoUTM30Lx::decode4car(const char* data){
    return ((data[0]-0x30)<<18)|((data[1]-0x30)<<12)|((data[2]-0x30)<<6)|((data[3]-0x30));
}

string HokuyoUTM30Lx::sendCommand(string command){
    char c;
    string ret="00";
    c='\n';
    sendingCmdMut->GetMutex();
    serialport->Write(command.c_str(),command.size());
    serialport->Write(&c,1);
    sendingCmdMut->ReleaseMutex();
    return ret;
}

void HokuyoUTM30Lx::getMesure(int startStep, int endStep,int clusterCount, int interval, int scanNumber){
    stringstream ss;
    string ret;
    ss << "MD" << std::setfill('0') << std::setw(4) << startStep << std::setw(4) << endStep << std::setw(2) << clusterCount << std::setw(1) << interval << std::setw(2) << scanNumber;
    ret = sendCommand(ss.str());
    #ifdef VERBOSE
        if(ret == "00"){
            cout << "MD: Alright !" << endl;
        }else if(ret == "01"){
            cout << "MD: Laser malfunction !" << endl;
        }else if(ret == "02"){
            cout << "MD: End Step has non-numeric value !" << endl;
        }else if(ret == "03"){
            cout << "MD: Cluster Count has non-numeric value !" << endl;
        }else if(ret == "04"){
            cout << "MD: End Step is out of range !" << endl;
        }else if(ret == "05"){
            cout << "MD: End Step is smaller than Starting Step !" << endl;
        }else if(ret == "06"){
            cout << "MD: Scan Interval has non-numeric value !" << endl;
        }else if(ret == "07"){
            cout << "MD: Number of Scan has non-numeric value !" << endl;
        }else if(ret == "98"){
            cout << "MD: Resumption of process after confirming normal HokuyoUTM30Lx operation." << endl;
        }else{
            /* Concerne :
            21~49 --- Processing stopped to verify the error.
            50~97 --- Hardware trouble (such as laser, motor malfunctions etc.)*/
            cout << "MD: Malfunction !" << endl;
        }
    #endif
    }
cvmatrix* HokuyoUTM30Lx::getDatas(){
    return output;
}

void HokuyoUTM30Lx::UseDefaultPlot(void)
{
    plot=new RangeFinderPlot(main_tab->NewRow(),"plot","x",-100,100,"y",-0,100,output,-45,225,output->Rows());
}

} // end namespace sensor
} // end namespace framewor