Index: /trunk/FACT++/src/HeadersMCP.h
===================================================================
--- /trunk/FACT++/src/HeadersMCP.h	(revision 14221)
+++ /trunk/FACT++/src/HeadersMCP.h	(revision 14222)
@@ -17,4 +17,8 @@
             kConfiguring2,
             kConfiguring3,
+            kCrateReset0,
+            kCrateReset1,
+            kCrateReset2,
+            kCrateReset3,
             kConfigured,
             kTriggerOn,
Index: /trunk/FACT++/src/mcp.cc
===================================================================
--- /trunk/FACT++/src/mcp.cc	(revision 14221)
+++ /trunk/FACT++/src/mcp.cc	(revision 14222)
@@ -33,4 +33,15 @@
 {
 private:
+    vector<bool> fFadConnected;
+    vector<bool> fFadNeedsReset;
+
+    vector<bool> fFadCratesForReset;
+    vector<bool> fFadBoardsForConnection;
+
+    uint16_t fNumConnectedFtu;
+    uint16_t fNumConnectedFad;
+
+    uint16_t fNumReset;
+
     DimVersion fDim;
     DimDescribedState fDimFTM;
@@ -41,4 +52,59 @@
     DimDescribedService fService;
 
+    Time fFadTimeout;
+
+    int HandleFadConnections(const EventImp &d)
+    {
+        if (d.GetSize()!=41)
+            return GetCurrentState();
+
+        const uint8_t *ptr = d.Ptr<uint8_t>();
+
+        fNumConnectedFad = 0;
+        fFadConnected.assign(40, false);
+
+        vector<bool> reset(4);
+
+        for (int i=0; i<40; i++)
+        {
+            const uint8_t stat1 = ptr[i]&3;
+            const uint8_t stat2 = ptr[i]>>3;
+
+            // disconnected: ignore
+            if (stat1==0 && stat2==0)
+                continue;
+
+            fFadConnected[i] = true;
+
+            if (stat1>=2 && stat2==8)
+                fNumConnectedFad++;
+
+            // Does not need reset
+            if (stat1>2 && stat2==8)
+                continue;
+
+            // Not configured (stat1==2?kLedGreen:kLedGreenCheck)
+            // Connection problem (stat1==1&&stat2==1?kLedRed:kLedOrange)
+            reset[i/10] = true;
+        }
+        return GetCurrentState();
+    }
+
+    int HandleFtmStaticData(const EventImp &d)
+    {
+        if (d.GetSize()!=sizeof(FTM::DimStaticData))
+            return GetCurrentState();
+
+        const FTM::DimStaticData &sdata = d.Ref<FTM::DimStaticData>();
+
+        fNumConnectedFtu = 0;
+        for (int i=0; i<40; i++)
+        {
+            if (sdata.IsActive(i))
+                fNumConnectedFtu++;
+        }
+        return GetCurrentState();
+    }
+
     int Print() const
     {
@@ -57,5 +123,5 @@
     }
 
-    int StopRun(const EventImp &)
+    int StopRun()
     {
 	if (fDimFTM.state()==FTM::State::kTriggerOn)
@@ -76,10 +142,16 @@
     }
 
-    int Reset(const EventImp &)
-    {
+    int Reset()
+    {
+        if (GetCurrentState()<MCP::State::kConfiguring1 ||
+            GetCurrentState()>MCP::State::kConfigured)
+            return GetCurrentState();
+
         fRunType = "";
 	Message("Reseting configuration states of FAD and FTM");
-	Dim::SendCommandNB("FTM_CONTROL/RESET_CONFIGURE");
+
+        Dim::SendCommandNB("FTM_CONTROL/RESET_CONFIGURE");
 	Dim::SendCommandNB("FAD_CONTROL/RESET_CONFIGURE");
+
         Update(MCP::State::kIdle);
         return MCP::State::kIdle;
@@ -134,4 +206,6 @@
         fNumEvents = evt.Get<int64_t>(8);
         fRunType   = evt.Ptr<char>(16);
+
+        fNumReset  = 0;
 
         ostringstream str;
@@ -204,93 +278,10 @@
             fDimFAD.state() >= FAD::State::kConnected &&
             fDimLog.state() >= kSM_Ready)
-        {
-            if (GetCurrentState()==MCP::State::kConfiguring1)
-            {
-		if (fDimLog.state()<30/*kSM_WaitForRun*/)
-		{
-		    Message("Starting datalogger");
-                    Dim::SendCommandNB("DATA_LOGGER/START_RUN_LOGGING");
-		}
-		Message("Configuring Trigger (FTM)");
-                Dim::SendCommandNB("FTM_CONTROL/CONFIGURE", fRunType);
-                Update(MCP::State::kConfiguring2);
-                return MCP::State::kConfiguring2;
-            }
-
-            if (GetCurrentState()==MCP::State::kConfiguring2)
-            {
-                // FIMXE: Reset in case of error
-                if ((/*fDimFTM.state() != FTM::State::kConfiguring2 &&*/
-                     fDimFTM.state() != FTM::State::kConfigured) ||
-                    fDimLog.state()<30 || fDimLog.state()>0xff)
-                    return GetCurrentState();
-
-                // FIMXE: This is to make sure that the rate control
-                // has received the correct trigger setup already...
-                //usleep(1000000);
-
-                Message("Starting Rate Control");
-                Dim::SendCommandNB("RATE_CONTROL/CALIBRATE");
-
-                ConfigureFAD();
-                Update(MCP::State::kConfiguring3);
-                return MCP::State::kConfiguring3;
-            }
-
-            if (GetCurrentState()==MCP::State::kConfiguring3)
-            {
-                if (fDimFTM.state() != FTM::State::kConfigured ||
-                    fDimFAD.state() != FAD::State::kConfigured ||
-                    fDimRC.state()  <  RateControl::State::kSettingGlobalThreshold)
-                    return GetCurrentState();
-
-                Message("Starting Trigger (FTM)");
-                Dim::SendCommandNB("FTM_CONTROL/START_TRIGGER");
-                Update(MCP::State::kConfigured);
-                return MCP::State::kConfigured;
-            }
-
-            if (GetCurrentState()==MCP::State::kConfigured)
-            {
-                if (fDimFTM.state() != FTM::State::kTriggerOn)
-                    return GetCurrentState();
-
-                Update(MCP::State::kTriggerOn);
-
-                return MCP::State::kTriggerOn;
-            }
-
-            if (GetCurrentState()==MCP::State::kTriggerOn)
-            {
-                if (fDimFAD.state() != FAD::State::kWritingData)
-                    return GetCurrentState();
-
-                Update(MCP::State::kTakingData);
-
-                return MCP::State::kTakingData;
-            }
-
-            if (GetCurrentState()==MCP::State::kTakingData)
-            {
-                if (fDimFTM.state()==FTM::State::kTriggerOn &&
-                    fDimFAD.state()==FAD::State::kWritingData)
-                    return MCP::State::kTakingData;
-
-                Update(MCP::State::kIdle);
-            }
-
-            return MCP::State::kIdle;
-        }
-
-        /*
-        if (fDimFTM.state() >= FTM::State::kConnected &&
-            fDimFAD.state() >= FAD::State::kConnected &&
-            fDimLog.state() >= kSM_Ready)
-            return MCP::State::kIdle;
-         */
+            return GetCurrentState();
+
         if (fDimFTM.state() >-2 &&
             fDimFAD.state() >-2 &&
             fDimLog.state() >-2 &&
-            fDimRC.state() >-2)
+            fDimRC.state()  >-2)
             return MCP::State::kConnected;
 
@@ -298,12 +289,204 @@
             fDimFAD.state() >-2 ||
             fDimLog.state() >-2 ||
-            fDimRC.state() >-2)
+            fDimRC.state()  >-2)
             return MCP::State::kConnecting;
 
         return MCP::State::kDisconnected;
+    }
+
+    int Execute()
+    {
+        // ========================================================
+
+        if (GetCurrentState()==MCP::State::kConfiguring1)
+        {
+            if (fDimLog.state()<30/*kSM_WaitForRun*/)
+            {
+                Message("Starting datalogger");
+                Dim::SendCommandNB("DATA_LOGGER/START_RUN_LOGGING");
+            }
+
+            Message("Configuring Trigger (FTM)");
+            Dim::SendCommandNB("FTM_CONTROL/CONFIGURE", fRunType);
+
+            Update(MCP::State::kConfiguring2);
+            return MCP::State::kConfiguring2;
+        }
+
+        // --------------------------------------------------------
+
+        if (GetCurrentState()==MCP::State::kConfiguring2)
+        {
+            // FIMXE: Reset in case of error
+            if ((/*fDimFTM.state() != FTM::State::kConfiguring2 &&*/
+                 fDimFTM.state() != FTM::State::kConfigured) ||
+                fDimLog.state()<30 || fDimLog.state()>0xff)
+                return MCP::State::kConfiguring2;
+
+            // FIMXE: This is to make sure that the rate control
+            // has received the correct trigger setup already...
+            //usleep(1000000);
+
+            Message("Starting Rate Control");
+            Dim::SendCommandNB("RATE_CONTROL/CALIBRATE");
+
+            ConfigureFAD();
+
+            fFadTimeout = Time();
+
+            Update(MCP::State::kConfiguring3);
+            return MCP::State::kConfiguring3;
+        }
+
+        // --------------------------------------------------------
+
+        if (GetCurrentState()==MCP::State::kConfiguring3)
+        {
+            // If everything is configured but the FADs
+            // we run into a timeout and some FAD need to be reset
+            // then we start an automatic crate reset
+            if (fDimFTM.state() == FTM::State::kConfigured &&
+                fDimFAD.state() != FAD::State::kConfigured &&
+                fDimRC.state()  >=  RateControl::State::kSettingGlobalThreshold &&
+                fFadTimeout+boost::posix_time::seconds(15)<Time() &&
+                count(fFadNeedsReset.begin(), fFadNeedsReset.end(), true)>0)
+            {
+                Update(MCP::State::kCrateReset0);
+                return MCP::State::kCrateReset0;
+            }
+
+            // If something is not yet properly configured: keep state
+            if (fDimFTM.state() != FTM::State::kConfigured ||
+                fDimFAD.state() != FAD::State::kConfigured ||
+                fDimRC.state()  <  RateControl::State::kSettingGlobalThreshold)
+                return MCP::State::kConfiguring3;
+
+            Message("Starting Trigger (FTM)");
+            Dim::SendCommandNB("FTM_CONTROL/START_TRIGGER");
+
+            Update(MCP::State::kConfigured);
+            return MCP::State::kConfigured;
+        }
+
+        // --------------------------------------------------------
+
+        if (GetCurrentState()==MCP::State::kConfigured)
+        {
+            if (fDimFTM.state() != FTM::State::kTriggerOn)
+                return MCP::State::kConfigured;
+
+            Update(MCP::State::kTriggerOn);
+            return MCP::State::kTriggerOn;
+        }
+
+        // --------------------------------------------------------
+
+        if (GetCurrentState()==MCP::State::kTriggerOn)
+        {
+            if (fDimFAD.state() != FAD::State::kWritingData)
+                return MCP::State::kTriggerOn;
+
+            Update(MCP::State::kTakingData);
+            return MCP::State::kTakingData;
+        }
+
+        // --------------------------------------------------------
+
+        if (GetCurrentState()==MCP::State::kTakingData)
+        {
+            if (fDimFTM.state()==FTM::State::kTriggerOn &&
+                fDimFAD.state()==FAD::State::kWritingData)
+                return MCP::State::kTakingData;
+
+            Update(MCP::State::kIdle);
+            return MCP::State::kIdle;
+        }
+
+        // ========================================================
+
+        if (GetCurrentState()==MCP::State::kCrateReset0)
+        {
+            static const struct Data { int32_t id; char on; } __attribute__((__packed__)) d = { -1, 0 };
+
+            Dim::SendCommandNB("FTM_CONTROL/ENABLE_FTU", &d, sizeof(Data));
+
+            fFadCratesForReset      = fFadNeedsReset;
+            fFadBoardsForConnection = fFadConnected;
+
+            for (int c=0; c<4; c++)
+                if (fFadNeedsReset[c])
+                    for (int b=0; b<10; b++)
+                        Dim::SendCommandNB("FAD_CONTROL/DISCONNECT", uint16_t(c*10+b));
+
+            fNumReset++;
+
+            Update(MCP::State::kCrateReset1);
+            return MCP::State::kCrateReset1;
+        }
+
+        // --------------------------------------------------------
+
+        if (GetCurrentState()==MCP::State::kCrateReset1)
+        {
+            if (fNumConnectedFtu>0 || count(fFadNeedsReset.begin(), fFadNeedsReset.end(), true)>0)
+                return MCP::State::kCrateReset1;
+
+            for (int i=0; i<4; i++)
+                if (fFadCratesForReset[i])
+                    Dim::SendCommandNB("FAD_CONTROL/RESET_CRATE", uint16_t(i));
+
+            fFadTimeout = Time();
+
+            Update(MCP::State::kCrateReset2);
+            return MCP::State::kCrateReset2;
+        }
+
+        // --------------------------------------------------------
+
+        if (GetCurrentState()==MCP::State::kCrateReset2)
+        {
+            if (fFadTimeout+boost::posix_time::seconds(45)>Time())
+                return MCP::State::kCrateReset2;
+
+            static const struct Data { int32_t id; char on; } __attribute__((__packed__)) d = { -1, 1 };
+
+            Dim::SendCommandNB("FTM_CONTROL/ENABLE_FTU", &d, sizeof(Data));
+
+            for (int c=0; c<4; c++)
+                if (fFadCratesForReset[c])
+                    for (int b=0; b<10; b++)
+                        if (fFadBoardsForConnection[c*10+b])
+                            Dim::SendCommandNB("FAD_CONTROL/CONNECT", uint16_t(c*10+b));
+
+            Update(MCP::State::kCrateReset3);
+            return MCP::State::kCrateReset3;
+        }
+
+        // --------------------------------------------------------
+
+        if (GetCurrentState()==MCP::State::kCrateReset3)
+        {
+            if (fNumConnectedFtu<40 || fFadBoardsForConnection!=fFadConnected)
+                return MCP::State::kCrateReset3;
+
+            if (count(fFadNeedsReset.begin(), fFadNeedsReset.end(), true)>0 && fNumReset<6)
+            {
+                Update(MCP::State::kCrateReset0);
+                return MCP::State::kCrateReset0;
+            }
+
+            // restart configuration
+            Update(MCP::State::kConfiguring1);
+            return MCP::State::kConfiguring1;
+        }
+
+        // ========================================================
+
+        return GetCurrentState();
     }
 
 public:
     StateMachineMCP(ostream &out=cout) : StateMachineDim(out, "MCP"),
+        fFadNeedsReset(4), fNumConnectedFtu(40),
         fDimFTM("FTM_CONTROL"),
         fDimFAD("FAD_CONTROL"),
@@ -334,35 +517,40 @@
         fDimRC.SetCallback(bind(&StateMachineMCP::HandleStateChange, this));
 
+        Subscribe("FAD_CONTROL/CONNECTIONS")
+            (bind(&StateMachineMCP::HandleFadConnections, this, placeholders::_1));
+        Subscribe("FTM_CONTROL/STATIC_DATA")
+            (bind(&StateMachineMCP::HandleFtmStaticData, this, placeholders::_1));
+
         // State names
         AddStateName(MCP::State::kDimNetworkNA, "DimNetworkNotAvailable",
                      "DIM dns server not available.");
-
         AddStateName(MCP::State::kDisconnected, "Disconnected",
                      "Neither ftmctrl, fadctrl, datalogger nor rate control online.");
-
         AddStateName(MCP::State::kConnecting, "Connecting",
                      "Either ftmctrl, fadctrl, datalogger or rate control not online.");
-
         AddStateName(MCP::State::kConnected, "Connected",
                      "All needed subsystems online.");
-
         AddStateName(MCP::State::kIdle, "Idle",
                      "Waiting for next configuration command");
-
         AddStateName(MCP::State::kConfiguring1, "Configuring1",
                      "Starting configuration procedure, checking Datalogger state");
-
         AddStateName(MCP::State::kConfiguring2, "Configuring2",
                      "Waiting for FTM and Datalogger to get ready");
-
         AddStateName(MCP::State::kConfiguring3, "Configuring3",
                      "Waiting for FADs and rate control to get ready");
 
+        AddStateName(MCP::State::kCrateReset0, "CrateReset0",
+                     "Disabling FTUs, disconnecting FADs");
+        AddStateName(MCP::State::kCrateReset1, "CrateReset1",
+                     "Waiting for FTUs to be disabled and for FADs to be disconnected");
+        AddStateName(MCP::State::kCrateReset2, "CrateReset2",
+                     "Waiting 45s");
+        AddStateName(MCP::State::kCrateReset3, "CrateReset3",
+                     "Waiting for FTUs to be enabled and for FADs to be re-connected");
+
         AddStateName(MCP::State::kConfigured, "Configured",
                      "Everything is configured, trigger will be switched on now");
-
         AddStateName(MCP::State::kTriggerOn, "TriggerOn",
                      "The trigger is switched on, waiting for FAD to receive data");
-
         AddStateName(MCP::State::kTakingData, "TakingData",
                      "The trigger is switched on, FADs are sending data");
@@ -377,9 +565,9 @@
 
         AddEvent("STOP")
-            (bind(&StateMachineMCP::StopRun, this, placeholders::_1))
+            (bind(&StateMachineMCP::StopRun, this))
             ("Stops the trigger (either disables the FTM trigger or the internal DRS trigger)");
 
-        AddEvent("RESET", MCP::State::kConfiguring1, MCP::State::kConfiguring2, MCP::State::kConfiguring3, MCP::State::kConfigured)
-            (bind(&StateMachineMCP::Reset, this, placeholders::_1))
+        AddEvent("RESET")
+            (bind(&StateMachineMCP::Reset, this))
             ("If a configuration blockes because a system cannot configure itself properly, "
              "this command can be called to leave the configuration procedure. The command "