Index: trunk/FACT++/src/biasctrl.cc
===================================================================
--- trunk/FACT++/src/biasctrl.cc	(revision 11851)
+++ trunk/FACT++/src/biasctrl.cc	(revision 11852)
@@ -25,5 +25,5 @@
 class ConnectionBias : public ConnectionUSB
 {
-    vector<char> fBuffer;
+    vector<uint8_t> fBuffer;
 
     bool fIsVerbose;
@@ -38,9 +38,10 @@
     enum Command_t
     {
-        kCmdReset = 0,
-        kCmdRead  = 1,
-        kCmdWrite = 3
+        kCmdReset      = 0,
+        kCmdRead       = 1,
+        kCmdGlobalSet  = 2,
+        kCmdChannelSet = 3,
+        kCmdPrint      = 4
     };
-
 
      // Resistance in Ohm for voltage correction
@@ -49,32 +50,28 @@
 protected:
 
-    vector<uint16_t> fVolt;        // Voltage in DAC units
+    vector<uint16_t> fVolt;        // Voltage in DAC units (12bit = 90V)
     vector<uint16_t> fRefVolt;
 
-    vector<uint16_t> fCurrent;     // Current in ADC units
-    vector<uint16_t> fRefCurrent;
-
-    vector<bool>     fOC;
+    vector<int16_t>  fCurrent;     // Current in ADC units (12bit = 5mA)
+    vector<int16_t>  fRefCurrent;
+
     vector<bool>     fPresent;
 
-    bool fResetHit;
+    int fWrapCounter;
+
 
     virtual void UpdateA()
     {
-        if (!fIsVerbose)
-            return;
-
-        for (int c=0; c<kNumChannels; c++)
-        {
-            Out() << setw(2) << c << ":";
-            for (int b=0; b<kNumBoards; b++)
-                Out() << " " << setprecision(2) << fixed << setw(5) << fCurrent[b+c*kNumBoards];
-            Out() << endl;
-        }
     }
 
 private:
-    void HandleReceivedData(const bs::error_code& err, size_t bytes_received, int /*type*/)
-    {
+    void HandleReceivedData(const bs::error_code& err, size_t bytes_received, int type)
+    {
+        if (type==kCmdPrint && bytes_received==0 && !err)
+        {
+            Print();
+            return;
+        }
+
         // Do not schedule a new read if the connection failed.
         if (bytes_received==0 || err)
@@ -106,59 +103,87 @@
         if (fIsVerbose)
         {
-            Out() << endl << kBold << "Data received:" << endl;
-            Out() << Converter::GetHex<uint8_t>(fBuffer, 32) << endl;
-        }
-
-        int wrapcnt = -1;
-
-        // === Check/update all wrap counter ===
-        for (unsigned int i=0; i<fBuffer.size(); i += 3)
-        {
-            const int old = wrapcnt;
-            wrapcnt = (fBuffer[i]>>4)&7;
-
-            if (wrapcnt==-1 || (old+1)%8 == wrapcnt)
-                continue;
-
-            Error("WrapCnt wrong");
-            // Error receiving proper answer!
-            return;
-        }
-
-        // Success with received answer
-
-        if (fBuffer.size()!=kNumChannels*3)
-            return;
-
-        /*
-         data[0] = (cmd<<5) | (board<<1) | (((channel&16)>>4) & 1);
-         data[1] = (channel<<4) | (cmdval>>8);
-         data[2] = val&0xff;
-         */
-
-        // ################## Read all channels status ##################
-
-        // Evaluate data returned from crate
-        for (int i=0; i<kNumChannels; i++)
-        {
-            fOC[i]      =  fBuffer[i*3]&0x80;
-            fCurrent[i] =  fBuffer[i*3+1] | ((fBuffer[i*3]&0xf)<<8);
-            fPresent[i] =  fBuffer[i*3+2]&0x70 ? false : true;
-
-            // FIXME FIXME FIXME
-            fResetHit   = fBuffer[i*3+2] & 0x80;
-
-            if (i==2*kNumChannelsPerBoard+19)
-                fOC[i] = false;
-        }
-
-        UpdateA();
-    }
-
-    void HandleTransmittedData(size_t n)
-    {
-        fBuffer.resize(n);
-        AsyncRead(ba::buffer(fBuffer));
-        AsyncWait(fInTimeout, 50, &ConnectionUSB::HandleReadTimeout);
+            //Out() << endl << kBold << "Data received (size=" << bytes_received << "):" << endl;
+            //Out() << Converter::GetHex<uint8_t>(fBuffer, 32) << endl;
+            // FIXME: Check why more is printed than expected
+        }
+
+        const uint16_t command = type&0xf;
+        const uint16_t id      = type>>4;
+        const uint16_t status  = (fBuffer[0]>>7)&1;
+        const uint16_t wrap    = (fBuffer[0]>>4)&7;
+        const uint16_t ddd     = ((uint16_t(fBuffer[0])&0xf)<<8) | fBuffer[1];
+        const uint16_t error   = (fBuffer[2]>>4)&0xf;
+        const uint16_t board   =  fBuffer[2]&0xf;
+
+        if (fWrapCounter>=0 && (fWrapCounter+1)%8 != wrap)
+        {
+            Error("Corrupted answer (wrap counter not as it ought to be.");
+            return;
+        }
+        fWrapCounter = wrap;
+
+        if (error==0x8) // No device
+        {
+            ostringstream out;
+            out << "HV down requested!";
+            Fatal(out);
+
+            GlobalSetDac(0);
+
+            // Resynchronize input and output
+            // SystemReset and status request
+            PostClose(true);
+
+            return;
+        }
+
+        if (command==kCmdReset)
+        {
+            if (status==0 && ddd==0 && error==0 && board==0)
+            {
+                Message("Reset successfully executed.");
+                return;
+            }
+
+            Warn("Answer to 'reset' command contains unexpected data.");
+            return;
+        }
+
+        if (command==kCmdGlobalSet)
+        {
+            if (status==0 && ddd==0 && error==0 && board==0)
+            {
+                Message("GlobalSet successfully executed.");
+                return;
+            }
+
+            Warn("Answer to 'global set' command contains unexpected data.");
+            return;
+        }
+
+        if (command==kCmdRead || command==kCmdChannelSet)
+        {
+            if (error==0x7 || error==0xf)
+            {
+                fPresent[board] = false;
+                fCurrent[id]    = 0x8000;
+                return;
+            }
+
+            if (board!=id/kNumChannelsPerBoard)
+            {
+                ostringstream out;
+                out << "Talked to board " << id/kNumChannelsPerBoard << " but board " <<  board << " answered.";
+                Error(out);
+                return;
+            }
+
+            fCurrent[id]    = status ? -ddd : ddd;
+            fPresent[board] = true;
+
+            UpdateA();
+
+            return;
+        }
     }
 
@@ -166,4 +191,9 @@
     void ConnectionEstablished()
     {
+        fWrapCounter = -1;
+        fBuffer.resize(3);
+
+        SystemReset();
+        ReadAllChannelsStatus();
     }
 
@@ -181,5 +211,4 @@
             PostClose();
             return;
-
         }
 
@@ -203,10 +232,4 @@
     }
 
-
-    void SystemReset()
-    {
-        PostMessage(GetCmd(0, kCmdReset));
-    }
-
     vector<char> GetCmd(uint16_t id, Command_t cmd, uint16_t dac=0)
     {
@@ -239,26 +262,13 @@
     void GlobalSetDac(uint16_t dac)
     {
-        PostMessage(GetCmd(0, kCmdWrite, dac));
-        /*
-        // On success
-        if (fBuffer.size() == 3)
-        {
-            for (int i=0; i<MAX_NUM_BOARDS; i++)
-                for (int j=0; j<NUM_CHANNELS; j++)
-                {
-                    DAC[i][j]     = SetPoint;
-                    Volt[i][j]    = Voltage;
-                    RefVolt[i][j] = Voltage;
-                }
-        }
-        */
-    }
-
+        PostMessage(GetCmd(0, kCmdGlobalSet, dac));
+        AsyncRead(ba::buffer(fBuffer), kCmdGlobalSet);
+    }
+
+    /*
     void SetChannels(const map<uint16_t, uint16_t> &vals)
     {
         if (vals.empty())
             return;
-
-        vector<char> data;
 
         // Build and execute commands
@@ -266,27 +276,13 @@
              it!=vals.end(); it++)
         {
-            //const uint16_t dac = it->second/90.0*0xfff;
-
             // If DAC value unchanged, do not send command
             if (fVolt[it->first] == it->second)
                 continue;
 
-            const vector<char> cmd = GetCmd(it->first, kCmdWrite, it->second);
-            data.insert(data.end(), cmd.begin(), cmd.end());
-        }
-
-        PostMessage(data);
-
-        /*
-        // On success
-        if (Data.size() == Buf.size())
-        {
-            for (map<unsigned int, double>::const_iterator it = V.begin(); it != V.end(); ++it) {
-                DAC[it->first/NUM_CHANNELS][it->first%NUM_CHANNELS] = (unsigned int) (it->second/90.0*0x0fff);
-                Volt[it->first/NUM_CHANNELS][it->first%NUM_CHANNELS] = it->second;
-                RefVolt[it->first/NUM_CHANNELS][it->first%NUM_CHANNELS] = it->second;
-        }
-        */
-    }
+            const vector<char> cmd = GetCmd(it->first, kCmdChannelSet, it->second);
+            PostMessage(cmd);
+            AsyncRead(ba::buffer(fBuffer), kCmdChannelSet|(it->first<<4));
+        }
+    }*/
 
     /*
@@ -314,6 +310,5 @@
         fCurrent(kNumChannels),
         fRefCurrent(kNumChannels),
-        fOC(kNumChannels),
-        fPresent(kNumChannels)
+        fPresent(kNumBoards)
     {
         SetLogStream(&imp);
@@ -322,25 +317,42 @@
     bool GlobalSet(double voltage)
     {
-        if (voltage<0 || voltage>90)
+        const uint16_t dac = voltage*4096/90;
+        if (dac>0xfff)
             return false;
 
-        GlobalSetDac(voltage/90.0*0xfff);
+        GlobalSetDac(dac);
 
         return true;
     }
 
+    bool ChannelSet(uint16_t ch, double voltage)
+    {
+        const uint16_t dac = voltage*4096/90;
+        if (dac>0xfff)
+            return false;
+
+        if (ch>=kNumChannels)
+            return false;
+
+        PostMessage(GetCmd(ch, kCmdChannelSet, dac));
+        AsyncRead(ba::buffer(fBuffer), kCmdChannelSet|(ch<<4));
+
+        return true;
+    }
+
     void ReadAllChannelsStatus()
     {
-        vector<char> data;
-        data.reserve(kNumChannels*3);
+        Message("Requesting full system status.");
 
         // Prepare command to read all channels
         for (int i=0; i<kNumChannels; i++)
-            {
-                const vector<char> cmd = GetCmd(i, kCmdRead);
-                data.insert(data.end(), cmd.begin(), cmd.end());
-            }
-
-        PostMessage(data);
+        {
+            const vector<char> cmd = GetCmd(i, kCmdRead);
+            PostMessage(cmd);
+            AsyncRead(ba::buffer(fBuffer), kCmdRead|(i<<4));
+        }
+
+        //vector<char> buf;
+        //AsyncRead(ba::buffer(buf), kCmdPrint);
     }
 
@@ -348,35 +360,40 @@
     void AdaptVoltages()
     {
-        map<uint16_t, uint16_t> values;
-
         for (int i=0; i<kNumChannels; i++)
         {
-            if (fRefVolt[i]==0)
+            if (fRefVolt[i]==0 || fCurrent[i]<0 || fRefCurrent[i]<0)
                 continue;
 
-            // Calculate difference and convert ADC units to Amps
-            const double diffcur = (fRefCurrent[i]-fCurrent[i])*1.22;
+            // Calculate difference and convert ADC units to Amp
+            // const double diffcur = (fRefCurrent[i]-fCurrent[i])*5000/4096
+            //const int32_t diffcur = int32_t(fRefCurrent[i]-fCurrent[i])*5000;
+            const int32_t diffvolt = (fRefCurrent[i]-fCurrent[i])*5;
 
             // Calculate voltage difference
-            const double diffvolt = diffcur*RESISTOR/1e6;
+            // #define RESISTOR 1000 // Ohm
+            //const double diffvolt = diffcur*RESISTOR/1e6;
 
             // Calculate new vlaue by onverting voltage difference to DAC units
-            const int32_t dac = fRefVolt[i] + diffvolt/90.0*0xfff;
-
-            if (dac<0 || dac>0xfff)
+            //const int32_t dac = fRefVolt[i] + diffvolt*4096/90.0;
+            int32_t dac = fRefVolt[i] + diffvolt/90;
+
+            if (dac<0)
+                dac = 0;
+            if (dac>0xfff)
+                dac = 0xfff;
+
+            if (fVolt[i] == dac)
                 continue;
 
-            values[i] = fRefVolt[i] + dac;
-        }
-
-        SetChannels(values);
-
-        /*
-        static int LastUpdate = 0;
-        if (time(NULL)-LastUpdate > 5)
-        {
-            LastUpdate = time(NULL);
-            UpdateDIM();
-        }*/
+            PostMessage(GetCmd(i, kCmdChannelSet, dac));
+            AsyncRead(ba::buffer(fBuffer), kCmdChannelSet|(i<<4));
+        }
+    }
+
+    void SystemReset()
+    {
+        Message("Sending system reset.");
+        PostMessage(GetCmd(0, kCmdReset));
+        AsyncRead(ba::buffer(fBuffer), kCmdReset);
     }
 
@@ -389,4 +406,38 @@
     {
         fIsVerbose = b;
+    }
+
+    void PrintLine(int b, int ch)
+    {
+        Out() << setw(2) << b << "|";
+
+        for (int c=ch; c<ch+8; c++)
+        {
+            const int id = c+kNumChannelsPerBoard*b;
+            Out() << " " << setw(7) << abs(fCurrent[id])*5000/4096.;
+            if (fCurrent[id]<0)
+                Out() << "!";
+            else
+                Out() << " ";
+        }
+        Out() << endl;
+
+    }
+    void Print()
+    {
+        Out() << dec << setprecision(2) << fixed;
+        for (int b=0; b<kNumBoards; b++)
+        {
+            if (!fPresent[b])
+            {
+                Out() << setw(2) << b << "-" << endl;
+                continue;
+            }
+
+            PrintLine(b,  0);
+            PrintLine(b,  8);
+            PrintLine(b, 16);
+            PrintLine(b, 24);
+        }
     }
 };
@@ -469,4 +520,15 @@
         if (!fBias.GlobalSet(evt.GetFloat()))
             T::Error("Supplied voltage out of range (0-90)");
+
+        return T::GetCurrentState();
+    }
+
+    int SetChannel(const EventImp &evt)
+    {
+        if (!CheckEventSize(evt.GetSize(), "SetChannel", 6))
+            return false;
+
+        if (!fBias.ChannelSet(evt.GetUShort(), evt.Get<float>(2)))
+            T::Error("Value out of range");
 
         return T::GetCurrentState();
@@ -565,34 +627,38 @@
 
         AddEvent("REQUEST_STATUS", kStateConnected)
-            (Wrap(bind(&ConnectionBias::ReadAllChannelsStatus, &fBias)))
+            (Wrapper(bind(&ConnectionBias::ReadAllChannelsStatus, &fBias)))
             ("");
 
-        AddEvent("SET_GLOBAL", "F:1", kStateConnected)
+        AddEvent("SET_GLOBAL_VOLTAGE", "F:1", kStateConnected)
             (bind(&StateMachineBias::SetGlobal, this, _1))
             ("");
 
+        AddEvent("SET_CHANNEL_VOLTAGE", "S:1;F:1", kStateConnected)
+            (bind(&StateMachineBias::SetChannel, this, _1))
+            ("");
+
         AddEvent("RESET", kStateConnected)
-            (Wrap(bind(&ConnectionBias::SystemReset, &fBias)))
+            (Wrapper(bind(&ConnectionBias::SystemReset, &fBias)))
             ("");
 
         AddEvent("SET_REFERENCE_CURRENT", kStateConnected)
-            (Wrap(bind(&ConnectionBias::SetReferenceCurrent, &fBias)))
+            (Wrapper(bind(&ConnectionBias::SetReferenceCurrent, &fBias)))
             ("");
 
         AddEvent("ADAPT_VOLTAGES", kStateConnected)
-            (Wrap(bind(&ConnectionBias::AdaptVoltages, &fBias)))
+            (Wrapper(bind(&ConnectionBias::AdaptVoltages, &fBias)))
             ("");
-    }
-
-    void SetEndpoint(const string &url)
-    {
-        fBias.SetEndpoint(url);
+
+        AddEvent("PRINT", kStateConnected)
+            (Wrapper(bind(&ConnectionBias::Print, &fBias)))
+            ("");
     }
 
     int EvalOptions(Configuration &conf)
     {
-        SetEndpoint(conf.Get<string>("addr"));
-
         fBias.SetVerbose(!conf.Get<bool>("quiet"));
+
+        fBias.SetEndpoint(conf.Get<string>("dev"));
+        T::Message("Setting device to "+fBias.URL());
 
         fBias.Connect();
@@ -617,5 +683,5 @@
     control.add_options()
         ("no-dim,d",      po_bool(),  "Disable dim services")
-        ("addr,a",        var<string>("ttysS0"),  "Device address of USB port to bias-power supply")
+        ("dev",           var<string>("FTE00FOH"),  "Device address of USB port to bias-power supply")
         ("quiet,q",       po_bool(),  "Disable printing contents of all received messages (except dynamic data) in clear text.")
         ;