Index: trunk/lib/FlairSensorActuator/src/HostEthController.cpp
===================================================================
--- trunk/lib/FlairSensorActuator/src/HostEthController.cpp	(revision 3)
+++ trunk/lib/FlairSensorActuator/src/HostEthController.cpp	(revision 15)
@@ -11,5 +11,6 @@
 //  version:    $Id: $
 //
-//  purpose:    Base class for host side remote controls that talks to target side through ethernet connection
+//  purpose:    Base class for host side remote controls that talks to target
+//  side through ethernet connection
 //
 //
@@ -37,225 +38,265 @@
 using std::string;
 
-namespace flair
-{
-namespace sensor
-{
-
-HostEthController::HostEthController(const FrameworkManager* parent,string name,string _address,int _port,uint32_t period,uint32_t _bitsPerAxis,uint8_t priority) :
-    Thread(parent,name,priority),IODevice(parent,name),tab(new Tab(parent->GetTabWidget(),name)),axisNumber(0),buttonNumber(0),targetAddress(_address),targetPort(_port),
-    bitsPerAxis(_bitsPerAxis),dataFrameBuffer(NULL),meaningfulDataAvailable(false) {
-    tabWidget=new TabWidget(tab->NewRow(),name);
-
-    const bool blocking=true;
-    controlSocket=new TcpSocket((Thread*)this,"eth controller control socket", blocking, !blocking);
-    dataSocket=new Socket((Thread*)this,"eth controller data socket",_address+":"+std::to_string(_port+1));
-    dataSender=new DataSender((Thread*)this, this, "eth controller data sender thread");
-    dataSender->SetPeriodMS(period);
-    dataSender->Start();
-
-    //test binary
-    /*
-    uint16_t testValue=0b0111011010; //0x1DA
-    char buffer[3]={(char)0b10101011, (char)0b01100101, (char)0b10110110}; // 0xAB65B6
-    writeBits(testValue,10,buffer,7); // 0b1010101 0 11101101 0 0110110 0xAAED36
-    Thread::Info("Debug: buffer after bits written=%X %X\n",buffer[0],buffer[1]);
-    */
-    connectionEstablishedMutex=new Mutex((Thread*)this);
+namespace flair {
+namespace sensor {
+
+HostEthController::HostEthController(const FrameworkManager *parent,
+                                     string name, string _address, int _port,
+                                     uint32_t period, uint32_t _bitsPerAxis,
+                                     uint8_t priority)
+    : Thread(parent, name, priority), IODevice(parent, name),
+      tab(new Tab(parent->GetTabWidget(), name)), axisNumber(0),
+      buttonNumber(0), targetAddress(_address), targetPort(_port),
+      bitsPerAxis(_bitsPerAxis), dataFrameBuffer(NULL),
+      meaningfulDataAvailable(false) {
+  tabWidget = new TabWidget(tab->NewRow(), name);
+
+  const bool blocking = true;
+  controlSocket = new TcpSocket((Thread *)this, "eth controller control socket",
+                                blocking, !blocking);
+  dataSocket = new Socket((Thread *)this, "eth controller data socket",
+                          _address + ":" + std::to_string(_port + 1));
+  dataSender =
+      new DataSender((Thread *)this, this, "eth controller data sender thread");
+  dataSender->SetPeriodMS(period);
+  dataSender->Start();
+
+  // test binary
+  /*
+  uint16_t testValue=0b0111011010; //0x1DA
+  char buffer[3]={(char)0b10101011, (char)0b01100101, (char)0b10110110}; //
+  0xAB65B6
+  writeBits(testValue,10,buffer,7); // 0b1010101 0 11101101 0 0110110 0xAAED36
+  Thread::Info("Debug: buffer after bits written=%X %X\n",buffer[0],buffer[1]);
+  */
+  connectionEstablishedMutex = new Mutex((Thread *)this);
 }
 
 HostEthController::~HostEthController() {
+  SafeStop();
+  Join();
+
+  if (!getFrameworkManager()->ConnectionLost())
+    delete tab;
+}
+
+void HostEthController::DrawUserInterface() {
+  Tab *plotTab = new Tab(tabWidget, "Measures");
+  axisPlot = new DataPlot1D *[axisNumber];
+  for (unsigned int i = 0; i < axisNumber; i++) {
+    // Start a new row or add up to the current row? We try to keep a 4/3 ratio
+    unsigned int columns = sqrt(4.0 * axisNumber / 3.0);
+    LayoutPosition *position;
+    if (i % columns == 0) {
+      position = plotTab->NewRow();
+    } else {
+      position = plotTab->LastRowLastCol();
+    }
+    axisPlot[i] = new DataPlot1D(position, axis->Name(i, 0),
+                                 -(1 << (nativeBitsPerAxis - 1)) * 1.2,
+                                 (1 << (nativeBitsPerAxis - 1)) * 1.5);
+    axisPlot[i]->AddCurve(axis->Element(i));
+  }
+  // we don't plot the button state for now
+}
+
+string HostEthController::GetAxisDescription(unsigned int axis) {
+  return string("axis") + std::to_string(axis);
+}
+
+string HostEthController::GetButtonDescription(unsigned int button) {
+  return string("button") + std::to_string(button);
+}
+
+bool HostEthController::ControllerInitialization() {
+
+  buttonOffset = (axisNumber * bitsPerAxis) / 8;
+  if ((axisNumber * bitsPerAxis) % 8 != 0)
+    buttonOffset++;
+  dataFrameSize = buttonOffset + (buttonNumber / 8) * sizeof(uint8_t);
+  if ((buttonNumber % 8) != 0)
+    dataFrameSize++;
+  dataFrameBuffer = new char[dataFrameSize];
+  return true;
+}
+
+void HostEthController::SendControllerInfo() {
+  // send axis info
+  controlSocket->WriteUInt32((uint32_t)axisNumber, 0);
+  controlSocket->WriteUInt32(bitsPerAxis, 0);
+  for (unsigned int i = 0; i < axisNumber; i++) {
+    // Thread::Info("Debug: sending axis name for axis %d = %s (takes up %d
+    // bytes)\n",i,GetAxisDescription(i).c_str(),GetAxisDescription(i).length());
+    int stringLength = GetAxisDescription(i).length();
+    controlSocket->WriteUInt32((uint32_t)stringLength, 0);
+    controlSocket->SendMessage(GetAxisDescription(i).c_str(), stringLength, 0);
+  }
+
+  // send button info
+  controlSocket->WriteUInt32((uint32_t)buttonNumber, 0);
+  for (unsigned int i = 0; i < buttonNumber; i++) {
+    int stringLength = GetButtonDescription(i).length();
+    controlSocket->WriteUInt32((uint32_t)stringLength, 0);
+    controlSocket->SendMessage(GetButtonDescription(i).c_str(), stringLength,
+                               0);
+  }
+}
+
+bool HostEthController::ConnectedWithTarget() {
+  char message[1024];
+  ssize_t sent, received;
+  static bool connectionEstablished = false;
+
+  connectionEstablishedMutex->GetMutex();
+  if (!connectionEstablished &&
+      controlSocket->Connect(targetPort, targetAddress, 10)) {
+    Thread::Info("Connected to %s:%d\n", targetAddress.c_str(), targetPort);
+    SendControllerInfo();
+    connectionEstablished = true;
+  }
+  connectionEstablishedMutex->ReleaseMutex();
+  return connectionEstablished;
+}
+
+void HostEthController::Run() {
+  static int divider = 0;
+  Message *msgControllerAction = new Message(1024);
+  if (getFrameworkManager()->ErrorOccured() || !ControllerInitialization()) {
     SafeStop();
-    Join();
-
-    if(!getFrameworkManager()->ConnectionLost()) delete tab;
-}
-
-void HostEthController::DrawUserInterface() {
-    Tab* plotTab=new Tab(tabWidget,"Measures");
-    axisPlot=new DataPlot1D *[axisNumber];
-    for (unsigned int i=0; i<axisNumber;i++) {
-        //Start a new row or add up to the current row? We try to keep a 4/3 ratio
-        unsigned int columns=sqrt(4.0*axisNumber/3.0);
-        LayoutPosition *position;
-        if (i%columns==0) {
-            position=plotTab->NewRow();
-        } else {
-            position=plotTab->LastRowLastCol();
+    Thread::Err("Une erreur a eu lieu, on ne lance pas la boucle\n");
+  }
+
+  if (buttonNumber % 8 != 0) {
+    SafeStop();
+    Thread::Err("Button number is not multiple of 8\n");
+  }
+
+  while (!ToBeStopped()) {
+    // Thread::Info("Debug: entering acquisition loop\n");
+    if (getFrameworkManager()->ConnectionLost() == true)
+      SafeStop();
+
+    if (IsDataFrameReady()) { // wait for next data frame
+      meaningfulDataAvailable = true;
+      GetAxisData();
+      GetButtonData();
+
+      if (ConnectedWithTarget()) {
+        // read for commands from the target (remote control state change
+        // requests such as rumble or led on/off)
+        ssize_t bytesReceived =
+            controlSocket->RecvMessage((char *)msgControllerAction->buffer,
+                                       msgControllerAction->bufferSize);
+        if (bytesReceived > 0) {
+          // if the message is a cnx exit request we manage it here, if not it
+          // will be managed by the derived class
+          ControllerAction action;
+          memcpy(&action, msgControllerAction->buffer,
+                 sizeof(ControllerAction));
+          if (action == ControllerAction::Exit) {
+            // Thread::Info("Debug: exit request received from server\n");
+            SafeStop();
+          }
+          ProcessMessage(msgControllerAction);
         }
-        axisPlot[i]=new DataPlot1D(position,axis->Name(i,0),-(1<<(nativeBitsPerAxis-1))*1.2,(1<<(nativeBitsPerAxis-1))*1.5);
-        axisPlot[i]->AddCurve(axis->Element(i));
-    }
-    //we don't plot the button state for now
-}
-
-string HostEthController::GetAxisDescription(unsigned int axis) {
-    return string("axis")+std::to_string(axis);
-}
-
-string HostEthController::GetButtonDescription(unsigned int button) {
-    return string("button")+std::to_string(button);
-}
-
-bool HostEthController::ControllerInitialization() {
-
-    buttonOffset=(axisNumber*bitsPerAxis)/8;
-    if ((axisNumber*bitsPerAxis)%8!=0) buttonOffset++;
-    dataFrameSize=buttonOffset+(buttonNumber/8)*sizeof(uint8_t);
-    if ((buttonNumber%8)!=0) dataFrameSize++;
-    dataFrameBuffer=new char[dataFrameSize];
-    return true;
-}
-
-void HostEthController::SendControllerInfo() {
-    //send axis info
-    controlSocket->WriteUInt32((uint32_t)axisNumber,0);
-    controlSocket->WriteUInt32(bitsPerAxis,0);
-    for (unsigned int i=0; i<axisNumber; i++) {
-        //Thread::Info("Debug: sending axis name for axis %d = %s (takes up %d bytes)\n",i,GetAxisDescription(i).c_str(),GetAxisDescription(i).length());
-        int stringLength=GetAxisDescription(i).length();
-        controlSocket->WriteUInt32((uint32_t)stringLength,0);
-        controlSocket->SendMessage(GetAxisDescription(i).c_str(), stringLength,0);
-    }
-
-    //send button info
-    controlSocket->WriteUInt32((uint32_t)buttonNumber,0);
-    for (unsigned int i=0; i<buttonNumber; i++) {
-        int stringLength=GetButtonDescription(i).length();
-        controlSocket->WriteUInt32((uint32_t)stringLength,0);
-        controlSocket->SendMessage(GetButtonDescription(i).c_str(), stringLength,0);
-    }
-}
-
-bool HostEthController::ConnectedWithTarget() {
-    char message[1024];
-    ssize_t sent,received;
-    static bool connectionEstablished=false;
-
-    connectionEstablishedMutex->GetMutex();
-    if (!connectionEstablished && controlSocket->Connect(targetPort,targetAddress,10)) {
-        Thread::Info("Connected to %s:%d\n", targetAddress.c_str(), targetPort);
-        SendControllerInfo();
-        connectionEstablished=true;
-    }
-    connectionEstablishedMutex->ReleaseMutex();
-    return connectionEstablished;
-}
-
-void HostEthController::Run() {
-    static int divider=0;
-    Message *msgControllerAction=new Message(1024);
-    if(getFrameworkManager()->ErrorOccured()||!ControllerInitialization()) {
-        SafeStop();
-        Thread::Err("Une erreur a eu lieu, on ne lance pas la boucle\n");
-    }
-
-    if(buttonNumber%8!=0) {
-        SafeStop();
-        Thread::Err("Button number is not multiple of 8\n");
-    }
-
-    while(!ToBeStopped()) {
-        //Thread::Info("Debug: entering acquisition loop\n");
-        if(getFrameworkManager()->ConnectionLost()==true) SafeStop();
-
-        if (IsDataFrameReady()) {  //wait for next data frame
-            meaningfulDataAvailable=true;
-            GetAxisData();
-            GetButtonData();
-
-            if (ConnectedWithTarget()) {
-                //read for commands from the target (remote control state change requests such as rumble or led on/off)
-                ssize_t bytesReceived=controlSocket->RecvMessage((char*)msgControllerAction->buffer,msgControllerAction->bufferSize);
-                if(bytesReceived>0) {
-                    //if the message is a cnx exit request we manage it here, if not it will be managed by the derived class
-                    ControllerAction action;
-                    memcpy(&action,msgControllerAction->buffer,sizeof(ControllerAction));
-                    if (action==ControllerAction::Exit) {
-                        //Thread::Info("Debug: exit request received from server\n");
-                        SafeStop();
-                    }
-                    ProcessMessage(msgControllerAction);
-                }
-            }
-        } else {//try to connect even if host is not sending anything
-            ConnectedWithTarget();
-        }
-        //Thread::Info("Debug: exiting acquisition loop\n");
-    }
-}
-
-bool HostEthController::writeBits(uint16_t value,uint8_t valueSizeInBits,char *buffer,uint8_t offsetInBits) {
-    if (valueSizeInBits>16) return false;
-    uint8_t remainingBitsToWrite=valueSizeInBits;
-    //skip first bytes
-    buffer+=offsetInBits/8;
-    offsetInBits-=(offsetInBits/8)*8;
-    while (remainingBitsToWrite>0) {
-        uint8_t remainingBitsInByteBeforeWrite=8-offsetInBits;
-        uint8_t bitsToWrite=remainingBitsToWrite<remainingBitsInByteBeforeWrite?remainingBitsToWrite:remainingBitsInByteBeforeWrite;
-        uint8_t remainingBitsInByteAfterWrite=remainingBitsInByteBeforeWrite-bitsToWrite;
-        //write in the current byte
-        uint8_t byteMask=((1<<bitsToWrite)-1)<<remainingBitsInByteAfterWrite;
-        (*buffer)&=~byteMask;
-        uint16_t valueMask=(1<<remainingBitsToWrite)-1;
-        (*buffer)|=((value&valueMask)>>(remainingBitsToWrite-bitsToWrite))<<remainingBitsInByteAfterWrite;
-        //update state
-        remainingBitsToWrite-=bitsToWrite;
-        offsetInBits=(offsetInBits+bitsToWrite)%8;
-        buffer++;
-    }
-    return true;
+      }
+    } else { // try to connect even if host is not sending anything
+      ConnectedWithTarget();
+    }
+    // Thread::Info("Debug: exiting acquisition loop\n");
+  }
+}
+
+bool HostEthController::writeBits(uint16_t value, uint8_t valueSizeInBits,
+                                  char *buffer, uint8_t offsetInBits) {
+  if (valueSizeInBits > 16)
+    return false;
+  uint8_t remainingBitsToWrite = valueSizeInBits;
+  // skip first bytes
+  buffer += offsetInBits / 8;
+  offsetInBits -= (offsetInBits / 8) * 8;
+  while (remainingBitsToWrite > 0) {
+    uint8_t remainingBitsInByteBeforeWrite = 8 - offsetInBits;
+    uint8_t bitsToWrite = remainingBitsToWrite < remainingBitsInByteBeforeWrite
+                              ? remainingBitsToWrite
+                              : remainingBitsInByteBeforeWrite;
+    uint8_t remainingBitsInByteAfterWrite =
+        remainingBitsInByteBeforeWrite - bitsToWrite;
+    // write in the current byte
+    uint8_t byteMask = ((1 << bitsToWrite) - 1)
+                       << remainingBitsInByteAfterWrite;
+    (*buffer) &= ~byteMask;
+    uint16_t valueMask = (1 << remainingBitsToWrite) - 1;
+    (*buffer) |= ((value & valueMask) >> (remainingBitsToWrite - bitsToWrite))
+                 << remainingBitsInByteAfterWrite;
+    // update state
+    remainingBitsToWrite -= bitsToWrite;
+    offsetInBits = (offsetInBits + bitsToWrite) % 8;
+    buffer++;
+  }
+  return true;
 }
 
 void HostEthController::BuildDataFrame() {
-    //int16_t testValue[4]={-120,-43,27,98}; //0x 88 d5 1b 62
-    for (unsigned int i=0;i<axisNumber;i++) {
-        //We shift value to always be positive (so that division/multiplication by power of 2 can easily be done with bit shifts)
-        uint16_t shiftedNativeAxisValue=axis->Value(i,0)+(1<<(nativeBitsPerAxis-1));
-        //int16_t nativeAxisValue=testValue[i];
-        uint16_t scaledAxisValue;
-        if (bitsPerAxis>nativeBitsPerAxis) {
-            scaledAxisValue=shiftedNativeAxisValue<<(bitsPerAxis-nativeBitsPerAxis);
-        } else {
-            scaledAxisValue=shiftedNativeAxisValue>>(nativeBitsPerAxis-bitsPerAxis);
-        }
-        //Thread::Info("Debug: shiftedNativeAxisValue=%#x, scaled axis value=%#x\n",shiftedNativeAxisValue,scaledAxisValue);
-        unsigned int offsetInBits=i*bitsPerAxis;
-        writeBits(scaledAxisValue,bitsPerAxis,dataFrameBuffer,offsetInBits);
-    }
-    //Thread::Info("Buffer après: %x %x %x\n", dataFrameBuffer[0], dataFrameBuffer[1], dataFrameBuffer[2]);
-
-    int currentButton=0;
-    uint8_t buttonArray[buttonNumber/8];
-    for (unsigned int i=0;i<buttonNumber/8;i++) {
-        buttonArray[i]=0;
-        for(unsigned int j=0;j<8;j++) {
-            bool buttonValue=button->Value(currentButton,0);
-            if(buttonValue) buttonArray[i]+=1<<j;
-            currentButton++;
-        }
-
-        dataSocket->HostToNetwork((char *)&buttonArray[i],sizeof(uint8_t));
-        memcpy(dataFrameBuffer+buttonOffset+i*sizeof(uint8_t),&buttonArray[i],sizeof(uint8_t));
-    }
-}
-
-HostEthController::DataSender::DataSender(Object *parent,HostEthController* _hostEthController,string name,uint8_t priority):Thread(parent,name,priority),hostEthController(_hostEthController) {
-
-}
+  // int16_t testValue[4]={-120,-43,27,98}; //0x 88 d5 1b 62
+  for (unsigned int i = 0; i < axisNumber; i++) {
+    // We shift value to always be positive (so that division/multiplication by
+    // power of 2 can easily be done with bit shifts)
+    uint16_t shiftedNativeAxisValue =
+        axis->Value(i, 0) + (1 << (nativeBitsPerAxis - 1));
+    // int16_t nativeAxisValue=testValue[i];
+    uint16_t scaledAxisValue;
+    if (bitsPerAxis > nativeBitsPerAxis) {
+      scaledAxisValue = shiftedNativeAxisValue
+                        << (bitsPerAxis - nativeBitsPerAxis);
+    } else {
+      scaledAxisValue =
+          shiftedNativeAxisValue >> (nativeBitsPerAxis - bitsPerAxis);
+    }
+    // Thread::Info("Debug: shiftedNativeAxisValue=%#x, scaled axis
+    // value=%#x\n",shiftedNativeAxisValue,scaledAxisValue);
+    unsigned int offsetInBits = i * bitsPerAxis;
+    writeBits(scaledAxisValue, bitsPerAxis, dataFrameBuffer, offsetInBits);
+  }
+  // Thread::Info("Buffer après: %x %x %x\n", dataFrameBuffer[0],
+  // dataFrameBuffer[1], dataFrameBuffer[2]);
+
+  int currentButton = 0;
+  uint8_t buttonArray[buttonNumber / 8];
+  for (unsigned int i = 0; i < buttonNumber / 8; i++) {
+    buttonArray[i] = 0;
+    for (unsigned int j = 0; j < 8; j++) {
+      bool buttonValue = button->Value(currentButton, 0);
+      if (buttonValue)
+        buttonArray[i] += 1 << j;
+      currentButton++;
+    }
+
+    dataSocket->HostToNetwork((char *)&buttonArray[i], sizeof(uint8_t));
+    memcpy(dataFrameBuffer + buttonOffset + i * sizeof(uint8_t),
+           &buttonArray[i], sizeof(uint8_t));
+  }
+}
+
+HostEthController::DataSender::DataSender(Object *parent,
+                                          HostEthController *_hostEthController,
+                                          string name, uint8_t priority)
+    : Thread(parent, name, priority), hostEthController(_hostEthController) {}
 
 void HostEthController::DataSender::Run() {
-    if(getFrameworkManager()->ErrorOccured()==true) {
-        SafeStop();
-    }
-
-    while(!ToBeStopped()) {
-        WaitPeriod();
-        if (hostEthController->meaningfulDataAvailable && hostEthController->ConnectedWithTarget()) {
-            //send the data
-            hostEthController->BuildDataFrame();
-            hostEthController->dataSocket->SendMessage(hostEthController->dataFrameBuffer,hostEthController->dataFrameSize);
-        }
-    }
+  if (getFrameworkManager()->ErrorOccured() == true) {
+    SafeStop();
+  }
+
+  while (!ToBeStopped()) {
+    WaitPeriod();
+    if (hostEthController->meaningfulDataAvailable &&
+        hostEthController->ConnectedWithTarget()) {
+      // send the data
+      hostEthController->BuildDataFrame();
+      hostEthController->dataSocket->SendMessage(
+          hostEthController->dataFrameBuffer, hostEthController->dataFrameSize);
+    }
+  }
 }
 
