// %flair:license{
// This file is part of the Flair framework distributed under the
// CECILL-C License, Version 1.0.
// %flair:license}
//  created:    2015/03/30
//  filename:   DualShock3.h
//
//  author:     Gildas Bayard
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    Base class for host side remote controls that talks to target side through ethernet connection
//
//
/*********************************************************************/
#include "DualShock3.h"
#include <Controller.h>
#include <cvmatrix.h>
#include <Tab.h>
#include <TabWidget.h>
#include <CheckBox.h>
#include <Label.h>
#include <DataPlot1D.h>
#include <SpinBox.h>
#include <GroupBox.h>
#include <FrameworkManager.h>
#include <Socket.h>
#include <sstream>

#include <stdlib.h>
#include <unistd.h>
#include <errno.h>
#include <fcntl.h>
#include <sstream>
#include <linux/input.h>
#include <linux/hidraw.h>
#include <bluetooth/hci.h>
#include <bluetooth/hci_lib.h>


#define USB_DIR_IN 0x80
#define USB_DIR_OUT 0
#define USB_GET_REPORT 0x01
#define USB_SET_REPORT 0x09
#define VENDOR_SONY 0x054c
#define PRODUCT_SIXAXIS_DS3 0x0268

#define L2CAP_PSM_HIDP_CTRL 0x11
#define L2CAP_PSM_HIDP_INTR 0x13

#define HIDP_TRANS_GET_REPORT    0x40
#define HIDP_TRANS_SET_REPORT    0x50
#define HIDP_DATA_RTYPE_OUTPUT   0x02
#define HIDP_DATA_RTYPE_FEATURE  0x03

#define SIXAXIS 1
#define DUALSHOCK3 2

#define X_AXIS_RANGE 127
#define Y_AXIS_RANGE 127

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

typedef struct motion_dev {
    int index;
    bdaddr_t addr;
    char type;
    int csk;
    int isk;
    struct motion_dev *next;
} motion_dev_t;

bdaddr_t bdaddr_any={{0,0,0,0,0,0}};

namespace flair
{
namespace sensor
{

DualShock3::DualShock3(const FrameworkManager* parent,string name,string receiverAddress,int receiverPort,ConnectionType_t _connectionType,uint32_t period,uint32_t bitsPerAxis,uint8_t priority)
    :HostEthController(parent,name,receiverAddress,receiverPort,period,bitsPerAxis,priority),connectionType(_connectionType) {
    controllerName="DualShock3";
    last_voltage_time=0;

    // axis stuff
    axisNumber=4;
    nativeBitsPerAxis=8;
    cvmatrix_descriptor *axisDescriptor=new cvmatrix_descriptor(axisNumber,1);
    for (unsigned int i=0;i<axisNumber;i++) {
        axisDescriptor->SetElementName(i,0,GetAxisDescription(i));
    }
    axis=new cvmatrix((IODevice*)this,axisDescriptor,Int16Type);
    AddDataToLog(axis);

    // buttons stuff
    buttonNumber=16;
    cvmatrix_descriptor *buttonDescriptor=new cvmatrix_descriptor(buttonNumber,1);
    for (unsigned int i=0;i<buttonNumber;i++) {
        buttonDescriptor->SetElementName(i,0,GetButtonDescription(i));
    }
    button=new cvmatrix((IODevice*)this,buttonDescriptor,Int8Type);
    AddDataToLog(button);

    Tab *settingsTab=new Tab(tabWidget,"Settings");
    dataSize=6;
    datas=new int8_t[dataSize];

    GroupBox* settingsGroupBox=new GroupBox(settingsTab->NewRow(),controllerName);
    deadZone=new SpinBox(settingsGroupBox->NewRow(),"dead zone:",-130,130,1);
    batteryChargeLevel=new Label(settingsGroupBox->LastRowLastCol(),"battery charge level");
    enabled=new CheckBox(settingsGroupBox->LastRowLastCol(),"enabled");

    if(connectionType==Bluetooth) {
        //init DS3
        usb_scan();

        int csk = l2cap_listen(&bdaddr_any, L2CAP_PSM_HIDP_CTRL);
        isk = l2cap_listen(&bdaddr_any, L2CAP_PSM_HIDP_INTR);

        if ( csk>=0 && isk>=0 )
            Printf("Waiting for Bluetooth connections.\n");
        else
            Thread::Err("Unable to listen on HID PSMs.\n");


        fd_set fds;
        FD_ZERO(&fds);

        if ( csk >= 0 ) FD_SET(csk, &fds);
        if ( select(csk+1,&fds,NULL,NULL,NULL) < 0 ) Thread::Err("select\n");
        // Incoming connection ?

        if ( csk>=0 && FD_ISSET(csk,&fds) )
        {
            //printf("accept\n");
            dev = accept_device(csk, isk);
            setup_device(dev);
        }
    } else if(connectionType==Usb) {
        int nr,i;
        unsigned char buf[128];

        for(i=0; i<255; i++) {
            ostringstream dev_name;
            dev_name << "/dev/hidraw" << i;
            if ((usb_fd = open(dev_name.str().c_str(), O_RDONLY)) >= 0) {
                int res = 0;
                struct hidraw_devinfo info;

                res = ioctl(usb_fd, HIDIOCGRAWINFO, &info);
                if (res < 0) {
                    Thread::Err("ioctl error (HIDIOCGRAWINFO) on %s\n",dev_name.str().c_str());
                } else {
                    //Printf("%x %x\n", info.vendor, info.product);
                    if(info.vendor==0x054c && info.product==0x0268) {
                        Printf("successfully opened %s\n",dev_name.str().c_str());
                        Printf("Press PS button to turn the controller on\n");
                        break;
                    }

                }
                close(usb_fd);
            }
        }
        if(i==255) {
            Thread::Err("sixad-raw::open(hidrawX) - failed to open hidraw device\n");
            return;
        }

        // block until PS button is pressed
        if ((nr=read(usb_fd, buf, sizeof(buf))) < 0) {
            Thread::Err("sixad-raw::read(fd) - failed to read from device\n");
        }

        if (nr < 49 || nr > 50) {
            Thread::Err("sixad-raw::read(fd) - not a sixaxis (nr = %i )\n",nr);
        }
    }
    ledmask=0;
}

DualShock3::~DualShock3() {
    if(connectionType==Usb) {
        close(usb_fd);
    }
    if(connectionType==Bluetooth) {
        if (!popen("/etc/init.d/bluetooth restart","r"))
            Thread::Warn("Could not restart bluetooth service\n");
    }
}

string DualShock3::GetAxisDescription(unsigned int axis) {
    string description;

    switch(axis) {
    case 0:
        description="left stick x-axis";
        break;
    case 1:
        description="left stick y-axis";
        break;
    case 2:
        description="right stick x-axis";
        break;
    case 3:
        description="right stick y-axis";
        break;
    }
    return description;
}

string DualShock3::GetButtonDescription(unsigned int button) {
    switch(button) {
    case 0:
        return "start";break;
    case 1:
        return "select";break;
    case 2:
        return "square";break;
    case 3:
        return "triangle";break;
    case 4:
        return "circle";break;
    case 5:
        return "cross";break;
    case 6:
        return "left 1";break;
    case 7:
        return "left 2";break;
    case 8:
        return "left 3";break;
    case 9:
        return "right 1";break;
    case 10:
        return "right 2";break;
    case 11:
        return "right 3";break;
    case 12:
        return "up";break;
    case 13:
        return "down";break;
    case 14:
        return "left";break;
    case 15:
        return "right";break;
    }
}

bool DualShock3::IsDataFrameReady() {
    unsigned char report[256];
    unsigned char tmp_report[256];

    if(!enabled->IsChecked()) {
        meaningfulDataAvailable=false;
        usleep(100000);
        return false;
    }
    now=GetTime();
    if(connectionType==Bluetooth) {
        fd_set fds;
        FD_ZERO(&fds);
        int fdmax = 0;/*
        if ( isk >= 0 ) FD_SET(isk, &fds);
        if ( isk > fdmax ) fdmax = isk;
*/
        FD_SET(dev->isk, &fds);
        if ( dev->isk > fdmax ) fdmax = dev->isk;

        if ( select(fdmax+1,&fds,NULL,NULL,NULL) < 0 ) fatal("select");

        // Incoming input report ?
        if ( FD_ISSET(dev->isk, &fds) ) {
            int nr;
            int recv_result;
            bool flushed=false;
            while(!flushed) {
                recv_result=recv(dev->isk, tmp_report, sizeof(report), MSG_DONTWAIT);
                if (recv_result<=0) {
                    if ((errno!=EAGAIN)&&(errno!=EWOULDBLOCK)) {
                        fprintf(stderr, "%d disconnected\n", dev->index);
                        close(dev->csk);
                        close(dev->isk);
                        free(dev);
                        return false;
                    } else {
                        flushed=true;
                        //fprintf(stderr, "\n");
                        continue;
                    }
                } else {
                    //fprintf(stderr, ".");
                    nr=recv_result;
                    memcpy(report,tmp_report,nr);
                }
            }
            if (report[0] == 0xa1) {
              return parse_report_sixaxis_ds3(report+1, nr-1);
            }
        }
        return false;

    } else if(connectionType==Usb) {
        int nr =read(usb_fd, report, sizeof(report));
        return parse_report_sixaxis_ds3(report, nr);
    }
    return false;
}

bool DualShock3::parse_report_sixaxis_ds3(unsigned char *r, int len) {
    if ( r[0]==0x01 && r[1]==0 &&len>=49 ) {
        datas[0]=r[2];
        datas[1]=r[3];
        datas[2]=compute_dead_zone(0,r[6]);
        datas[3]=compute_dead_zone(1,r[7]);
        datas[4]=compute_dead_zone(2,r[8]);
        datas[5]=compute_dead_zone(3,r[9]);

        if(GetTime()>(last_voltage_time+5*(Time)1000000000)) {
            //toute les 5 secondes
            //report battery charge level
            if(connectionType==Bluetooth) {
                batteryChargeLevel->SetText("battery: %i/5",r[30]);
            }
            if(connectionType==Usb) {
                batteryChargeLevel->SetText("battery: usb connected");
            }
            last_voltage_time=GetTime();
        }

        return true;
    }
    return false;
}

void DualShock3::GetAxisData() {

    axis->GetMutex();
//    axis->SetValueNoMutex(0, 0,datas[2]/(float)X_AXIS_RANGE); //left stick x-axis
//    axis->SetValueNoMutex(1, 0,datas[3]/(float)Y_AXIS_RANGE); //left stick y-axis
//    axis->SetValueNoMutex(2, 0,datas[4]/(float)X_AXIS_RANGE); //right stick x-axis
//    axis->SetValueNoMutex(3, 0,datas[5]/(float)Y_AXIS_RANGE); //right stick y-axis
    axis->SetValueNoMutex(0, 0,datas[2]); //left stick x-axis
    axis->SetValueNoMutex(1, 0,datas[3]); //left stick y-axis
    axis->SetValueNoMutex(2, 0,datas[4]); //right stick x-axis
    axis->SetValueNoMutex(3, 0,datas[5]); //right stick y-axis
    axis->ReleaseMutex();
    axis->SetDataTime(now);
}

void DualShock3::GetButtonData() {
    //static uint8_t old_start_button=0;
    button->GetMutex();
    button->SetValueNoMutex(0, 0,(datas[0]&0x08)==0?0:1); //start
/*
    uint8_t start_button=datas[0]&0x08;
    if (start_button!=old_start_button) {
        if (start_button==0) {
            Thread::Info("Debug: start button released\n");
        } else {
            Thread::Info("Debug: start button pressed\n");
        }
        old_start_button=start_button;
    }
*/
    button->SetValueNoMutex(1, 0,(datas[0]&0x01)==0?0:1);  //select
    button->SetValueNoMutex(2, 0,(datas[1]&0x80)==0?0:1);  //square
    button->SetValueNoMutex(3, 0,(datas[1]&0x10)==0?0:1);  //triangle
    button->SetValueNoMutex(4, 0,(datas[1]&0x20)==0?0:1);  //circle
    button->SetValueNoMutex(5, 0,(datas[1]&0x40)==0?0:1);  //cross
    button->SetValueNoMutex(6, 0,(datas[1]&0x04)==0?0:1);  //left 1
    button->SetValueNoMutex(7, 0,(datas[1]&0x01)==0?0:1);  //left 2
    button->SetValueNoMutex(8, 0,(datas[0]&0x02)==0?0:1);  //left 3
    button->SetValueNoMutex(9, 0,(datas[1]&0x08)==0?0:1);  //right 1
    button->SetValueNoMutex(10, 0,(datas[1]&0x02)==0?0:1); //right 2
    button->SetValueNoMutex(11, 0,(datas[0]&0x04)==0?0:1); //right 3
    button->SetValueNoMutex(12, 0,(datas[0]&0x10)==0?0:1); //up
    button->SetValueNoMutex(13, 0,(datas[0]&0x40)==0?0:1); //down
    button->SetValueNoMutex(14, 0,(datas[0]&0x80)==0?0:1); //left
    button->SetValueNoMutex(15, 0,(datas[0]&0x20)==0?0:1); //right
    button->ReleaseMutex();
    button->SetDataTime(now);
}

void DualShock3::ProcessMessage(core::Message *controllerAction) {
    ControllerAction action;
    memcpy(&action,controllerAction->buffer,sizeof(ControllerAction));
    if (action==ControllerAction::SetLedOn) {
        Thread::Info("LedOn action request\n");
    } else if (action==ControllerAction::SetLedOff) {
        Thread::Info("LedOff action request\n");
    } else if (action==ControllerAction::Rumble) {
        Thread::Info("Rumble action request\n");
    } else if (action==ControllerAction::FlashLed) {
        Thread::Info("FlashLed action request\n");
    }
// (char *msg, int msgSize)
/*    for (unsigned int i=0; i<4; i++) {
        if(msg[4+2*i]!=0 || msg[5+2*i]!=0) set_led(i+1,msg[4+2*i],msg[5+2*i]);

    }
    if(msg[0]!=0 || msg[2]!=0) rumble(msg[0],msg[1],msg[2],msg[3]);
        */
}

// ----------------------------------------------------------------------
// Replacement for libbluetooth

int DualShock3::mystr2ba(const char *s, bdaddr_t *ba) {
    if ( strlen(s) != 17 ) return 1;
    for ( int i=0; i<6; ++i ) {
        int d = strtol(s+15-3*i, NULL, 16);
        if ( d<0 || d>255 ) return 1;
        ba->b[i] = d;
    }
    return 0;
}

char *DualShock3::myba2str(const bdaddr_t *ba) {
    static char buf[2][18];  // Static buffer valid for two invocations.
    static int index = 0;
    index = (index+1)%2;
    sprintf(buf[index], "%02x:%02x:%02x:%02x:%02x:%02x",
            ba->b[5], ba->b[4], ba->b[3], ba->b[2], ba->b[1], ba->b[0]);
    return buf[index];
}

void DualShock3::fatal(const char *msg) {
    if ( errno ) perror(msg);
    else fprintf(stderr, "%s\n", msg);
    exit(1);
}

/**********************************************************************/
// Bluetooth HID devices
// Incoming connections.

int DualShock3::l2cap_listen(const bdaddr_t *bdaddr, unsigned short psm) {
    int sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
    if ( sk < 0 ) fatal("socket");

    struct sockaddr_l2 addr;
    addr.l2_family = AF_BLUETOOTH;
    addr.l2_bdaddr = *BDADDR_ANY;
    addr.l2_psm = htobs(psm);
    addr.l2_cid = 0;

    if ( bind(sk, (struct sockaddr *)&addr, sizeof(addr)) < 0 )
    {
        close(sk);
        fatal("bind");
    }

    if ( listen(sk, 5) < 0 ) fatal("listen");
    return sk;
}

struct motion_dev *DualShock3::accept_device(int csk, int isk) {
    Printf("Incoming connection...\n");
    struct motion_dev *dev = (motion_dev*)malloc(sizeof(struct motion_dev));
    if (!dev) fatal("malloc");

    dev->csk = accept(csk, NULL, NULL);
    if ( dev->csk < 0 ) fatal("accept(CTRL)");
    dev->isk = accept(isk, NULL, NULL);
    if ( dev->isk < 0 ) fatal("accept(INTR)");

    struct sockaddr_l2 addr;
    socklen_t addrlen = sizeof(addr);
    if ( getpeername(dev->isk, (struct sockaddr *)&addr, &addrlen) < 0 ) fatal("getpeername");
    dev->addr = addr.l2_bdaddr;

    // Distinguish SIXAXIS / DS3
    unsigned char resp[64];
    char get03f2[] = { HIDP_TRANS_GET_REPORT | HIDP_DATA_RTYPE_FEATURE | 8,
                       (char)0xf2, sizeof(resp), sizeof(resp)>>8
                     };
    send(dev->csk, get03f2, sizeof(get03f2), 0);  // 0301 is interesting too.
    int nr = recv(dev->csk, resp, sizeof(resp), 0);

    dev->type = (resp[13]==0x40) ? SIXAXIS : DUALSHOCK3;  // My guess.

    return dev;
}

/**********************************************************************/
// Device setup

#define IR0 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x90, 0x00, 0x41
#define IR1 0x40, 0x00

#define BIT1 2

void DualShock3::hidp_trans(int csk, char *buf, int len) {
    if ( send(csk, buf, len, 0) != len ) fatal("send(CTRL)");
    char ack;
    int nr = recv(csk, &ack, sizeof(ack), 0);
    if ( nr!=1 || ack!=0 ) fatal("ack");
}

void DualShock3::setup_device(struct motion_dev *dev) {

    switch ( dev->type ) {
    case SIXAXIS:
    case DUALSHOCK3:
        // Enable reporting
        char set03f4[] = { HIDP_TRANS_SET_REPORT | HIDP_DATA_RTYPE_FEATURE, (char)0xf4,
                           0x42, 0x03, 0x00, 0x00
                        };
        hidp_trans(dev->csk, set03f4, sizeof(set03f4));

        break;
    }
}

/**********************************************************************/
// Reports

//faire un reglage station sol pour dead zone
int8_t DualShock3::compute_dead_zone(int axis,unsigned char value)
{
    float tmp;

    if(value>128+deadZone->Value()) {
        tmp=(value-deadZone->Value()-128.)*128./(128.-deadZone->Value());
    } else if(value<128-deadZone->Value()) {
        //return value+DEAD_ZONE-128;
        tmp=(value+deadZone->Value()-128.)*127./(128.-deadZone->Value());
    } else {
        return 0;
    }

    if(tmp>127) return 127;
    if(tmp<-127) return -127;
    if(tmp>((int8_t)tmp+.5) && tmp>0) return (int8_t)(tmp+1);
    if(tmp<((int8_t)tmp-.5) && tmp<0) return (int8_t)(tmp-1);

    return (int8_t)tmp;
}

/**********************************************************************/
// USB functions

void DualShock3::usb_pair_device(struct usb_device *dev, int itfnum)
{

    usb_dev_handle *devh = usb_open(dev);
    if ( ! devh ) fatal("usb_open");
    usb_detach_kernel_driver_np(devh, itfnum);
    int res = usb_claim_interface(devh, itfnum);
    if ( res < 0 ) fatal("usb_claim_interface");

    bdaddr_t current_ba;  // Current pairing address.

    switch ( dev->descriptor.idProduct ) {
    case PRODUCT_SIXAXIS_DS3: {
        //remote_printf("USB: SIXAXIS/DS3\n");
        char msg[8];
        res = usb_control_msg
              (devh, USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
               USB_GET_REPORT, 0x03f5, itfnum, msg, sizeof(msg), 5000);
/*
        unsigned char msg[8];
        res = usb_control_msg
              (devh, USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
               USB_GET_REPORT, 0x03f5, itfnum, (void*)msg, sizeof(msg), 5000);*/
        if ( res < 0 ) fatal("usb_control_msg(read master)");
        for ( int i=0; i<6; ++i ) current_ba.b[i] = (uint8_t)msg[7-i];
        break;
    }
    }

    // New pairing address
    int dev_id;
    dev_id = hci_get_route(NULL);
    struct hci_dev_info di;
    hci_devinfo(dev_id, &di);


    // Perform pairing.
    if ( ! bacmp(&current_ba, &di.bdaddr) )
    {
        printf("  Already paired to %s\n", myba2str(&di.bdaddr));
    }
    else
    {
        printf("  Changing master from %s to %s\n",
               myba2str(&current_ba), myba2str(&di.bdaddr));
        switch ( dev->descriptor.idProduct )
        {
        case PRODUCT_SIXAXIS_DS3:
        {
            char msg[8] =
            { 0x01, 0x00, (char)di.bdaddr.b[5],(char)di.bdaddr.b[4],(char)di.bdaddr.b[3],(char)di.bdaddr.b[2],(char)di.bdaddr.b[1],(char)di.bdaddr.b[0] };
            res = usb_control_msg
                  (devh, USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
                   USB_SET_REPORT, 0x03f5, itfnum, msg, sizeof(msg), 5000);
            if ( res < 0 ) fatal("usb_control_msg(write master)");
            break;
        }
        }
    }

    if ( dev->descriptor.idProduct == PRODUCT_SIXAXIS_DS3 )
        printf("  Now unplug the USB cable and press the PS button.\n");
    else
        printf("  Now press the PS button.\n");
}

void DualShock3::usb_scan()
{
    usb_init();
    if ( usb_find_busses() < 0 ) fatal("usb_find_busses");
    if ( usb_find_devices() < 0 ) fatal("usb_find_devices");
    struct usb_bus *busses = usb_get_busses();
    if ( ! busses ) fatal("usb_get_busses");

    struct usb_bus *bus;
    for ( bus=busses; bus; bus=bus->next )
    {
        struct usb_device *dev;
        for ( dev=bus->devices; dev; dev=dev->next)
        {
            struct usb_config_descriptor *cfg;
            for ( cfg = dev->config;
                    cfg < dev->config + dev->descriptor.bNumConfigurations;
                    ++cfg )
            {
                int itfnum;
                for ( itfnum=0; itfnum<cfg->bNumInterfaces; ++itfnum )
                {
                    struct usb_interface *itf = &cfg->interface[itfnum];
                    struct usb_interface_descriptor *alt;
                    for ( alt = itf->altsetting;
                            alt < itf->altsetting + itf->num_altsetting;
                            ++alt )
                    {
                        if ( dev->descriptor.idVendor == VENDOR_SONY &&
                                (dev->descriptor.idProduct == PRODUCT_SIXAXIS_DS3) &&
                                alt->bInterfaceClass == 3 )
                            usb_pair_device(dev, itfnum);
                    }
                }
            }
        }
    }
}


void DualShock3::rumble(uint8_t left_force,uint8_t left_timeout,uint8_t right_force,uint8_t right_timeout)
{
    // printf("rumble\n");

    unsigned char datas[] =
    {
        0x52 /* HIDP_TRANS_SET_REPORT|HIDP_DATA_RTYPE_OUPUT */ ,
        0x01,
        0x00, 0x00, 0x00, 0x00, 0x00, //rumble values
        0x00, 0x00, 0x00, 0x00,(unsigned char)ledmask, // 0x10=LED1 .. 0x02=LED4

        0xff, 0x27, led4_on, led4_off, 0x32,
        0xff, 0x27, led3_on, led3_off, 0x32,
        0xff, 0x27, led2_on, led2_off, 0x32,
        0xff, 0x27, led1_on, led1_off, 0x32,
        0x00, 0x00, 0x00, 0x00, 0x00,
    };

    datas[5] = left_timeout;     // left timeout
    datas[6] = left_force;  // left force
    datas[3] = right_timeout;     // right timeout
    datas[4] = right_force;  // right force

    if(connectionType==Bluetooth) {
        hidp_trans(dev->csk, (char *)datas, sizeof(datas));
    }

}

void DualShock3::set_led(uint8_t led,uint8_t on_timeout,uint8_t off_timeout)
{
    uint8_t mask;

    switch(led) {
    case 1:
        led1_on=on_timeout;
        led1_off=off_timeout;
        mask=2;
        break;
    case 2:
        led2_on=on_timeout;
        led2_off=off_timeout;
        mask=4;
        break;
    case 3:
        led3_on=on_timeout;
        led3_off=off_timeout;
        mask=8;
        break;
    case 4:
        led4_on=on_timeout;
        led4_off=off_timeout;
        mask=16;
        break;
    }

    if(on_timeout!=0) {
        ledmask|=mask;
    } else {
        ledmask&=~mask;
    }
    /*
        printf("led %x\n",ledmask);
        printf("1:%i %i\n",led1_on,led1_off);
        printf("2:%i %i\n",led2_on,led2_off);
        printf("3:%i %i\n",led3_on,led3_off);
        printf("4:%i %i\n",led4_on,led4_off);*/


    unsigned char datas[] = {
        0x52 /* HIDP_TRANS_SET_REPORT|HIDP_DATA_RTYPE_OUPUT */ ,
        0x01,
        0x00, 0x00, 0x00, 0x00, 0x00, //rumble values
        0x00, 0x00, 0x00, 0x00,(unsigned char)ledmask, // 0x10=LED1 .. 0x02=LED4

        0xff, 0x27, led4_on, led4_off, 0x32,
        0xff, 0x27, led3_on, led3_off, 0x32,
        0xff, 0x27, led2_on, led2_off, 0x32,
        0xff, 0x27, led1_on, led1_off, 0x32,
        0x00, 0x00, 0x00, 0x00, 0x00,
    };

    if(connectionType==Bluetooth) {
        hidp_trans(dev->csk, (char *)datas, sizeof(datas));
    }
}

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