/*------------------------------------------------------------------------------

Measurement automatization for GSK PAT Line

Copyright: i-red GmbH, 2010
Author:    Peter Hintenaus

Change history:

date       performed by         description
June 2010  Hintenaus            initial release

------------------------------------------------------------------------------*/

PROGRAM

IMPORT CANopen;
IMPORT Spectrometer;
IMPORT TimeStampClock;
IMPORT DataStore;
IMPORT ChemometryEngine;
IMPORT Chemometry;
IMPORT LogBook;
IMPORT Console

CONST
   DStatVTrggr := 256;
   DStatSClsd := DStatVTrggr * 2;

   StatOk := 1;
   StatNOk := StatOk * 2;
   StatId := StatNOk * 2;
   StatPart := StatId * 2;
   StatVis := StatPart * 2;
   StatHum := StatVis * 2;

   SetFModel := 0;
   SetBModel := 1;
   SetFRef := 2;
   SetBRef := 3;

   nothing := -1;
   endVial := -2;
   noModel := 65535;

   Idle := 0;   
   Production := 1;
   GatherModel := 2;
   Calibration := 3;

   SpectralonModel := 9999;

   VialCnt := 5;

   MinSpectraPerS := 4;
   AreaLimit := 50000.0

TYPE
   VialStore = 
      RECORD OF
         INTEGER in;
         ARRAY[VialCnt] OF
            RECORD OF
               INTEGER no;
               TIMESTAMP strt
            END
         recent
      END;

   Measurement =
      RECORD OF
         INTEGER VialNo;
         FLOAT Humidity, Identity, Particle, VisAspect, Area 
      END;

   BackGround = 
      RECORD OF
         BOOL valid;
         BLOB data
      END

VAR 
   CANopen FtIrCANopen;
   Spectrometer FtIr;
   TimeStampClock FtIrClock;
   DataStore Spectra;
   ChemometryEngine FtIrChemometryEngine;
   Chemometry VialAnalysis;
   LogBook FtIrLog;
   Console DebugOut;

   // -------------- vial transport --------------------------------------------
   VialStore Vials;
   BOOL ShutterOpen;
   TIMESTAMP lastShutterClsdT;      
   INTEGER OpMode;

   // -------------- system status ---------------------------------------------
   CANopen.BoolOdEntry
      SysModel MAP TO FtIrCANopen.EntryA080b[0],
      SysOk MAP TO FtIrCANopen.EntryA080b[1],
      SysLampFail MAP TO FtIrCANopen.EntryA080b[2],
      SysBckgndOK MAP TO FtIrCANopen.EntryA080b[3],
      SysTgl MAP TO FtIrCANopen.EntryA080b[4],
      SysClppng MAP TO FtIrCANopen.EntryA080b[5],
      SysMirror MAP TO FtIrCANopen.EntryA080b[6],
      SysCalibNOk MAP TO FtIrCANopen.EntryA080b[7],
      SysChemoErr MAP TO FtIrCANopen.EntryA080b[8];
   CANopen.IntegerOdEntry CurrentModel MAP TO FtIrCANopen.EntryA100u16[4];
   BOOL ModelLoaded;

   // -------------- limiting values -------------------------------------------
   FLOAT HumidityMax;

   // -------------- Spectrometer control --------------------------------------
   INTEGER SpectraCnt;
   ARRAY[2] OF BackGround Spectralon

// -----------------------------------------------------------------------------

TASK TriggerObserver ON FtIrCANopen.RPDOrcvd[0]
VAR
   CANopen.IntegerOdEntry DiscStat MAP TO FtIrCANopen.EntryA580u16[0];
   CANopen.IntegerOdEntry VialNo MAP TO FtIrCANopen.EntryA580u16[1];
   CANopen.IntegerOdEntry HumidityThresh MAP TO FtIrCANopen.EntryA580u16[2];
   VialStore Vs;
   INTEGER vn, psc, sc;
   BOOL Trigger
BEGIN
   IF ShutterOpen THEN
      IF Trigger AND DiscStat.Data & DStatVTrggr = 0 THEN
         Trigger := FALSE;
         psc := -5
      ELSIF NOT Trigger AND DiscStat.Data & DStatVTrggr # 0 THEN
         DebugOut.writeString("vial: "); DebugOut.writeInteger(VialNo.Data);
         Trigger := TRUE;
         Vs.in := (Vs.in + 1) MOD VialCnt;
         Vs.recent[Vs.in].strt := RcvdTime;
         IF VialNo.Data # vn THEN 
            vn := VialNo.Data;
            Vs.recent[Vs.in].no := vn 
         ELSE
            DebugOut.writeString("last vial"); DebugOut.writeln();
            vn := endVial;
            Vs.recent[Vs.in].no := vn
         END;
         Vials := Vs;
         HumidityMax := HumidityThresh.Data / 10000.0;
         sc := SpectraCnt;
         IF OpMode # Calibration THEN         // Calibration works with lots of averaging, don't want an error
            SysMirror.Data :=  sc - psc < 2
         END;
         psc := sc;  
         FtIrCANopen.TPDOwrite(1)
      END
   END;
   IF DiscStat.Data & DStatSClsd # 0 AND ShutterOpen THEN
      ShutterOpen := FALSE;
      lastShutterClsdT := RcvdTime;
      FtIr.SwitchMirrorControl(TRUE);
      IF vn = endVial THEN
         FOR i := 0 TO VialCnt - 1 DO
            Vs.recent[i].strt := EOT; Vs.recent[i].no := nothing
         END;
         Vs.in := 0;
         Vials := Vs
      END;
      DebugOut.writeString("Shutter closed"); DebugOut.writeln()
   ELSIF DiscStat.Data & DStatSClsd = 0 AND NOT ShutterOpen THEN
      ShutterOpen := TRUE;
      lastShutterClsdT := EOT;      
      FtIr.SwitchMirrorControl(FALSE);
      DebugOut.writeString("Shutter open"); DebugOut.writeln()
   END
END

// -----------------------------------------------------------------------------

TASK ModelChanger ON FtIrCANopen.RPDOrcvd[1]
VAR 
   CANopen.IntegerOdEntry 
      ModelNo MAP TO FtIrCANopen.EntryA580u16[4],
      OM MAP TO FtIrCANopen.EntryA580u16[5];
   INTEGER M;
   ARRAY[2] OF BackGround BckGnd
BEGIN
   IF NOT ShutterOpen THEN
      M := ModelNo.Data;
      IF OpMode # OM.Data & 3 THEN
         IF OpMode = GatherModel OR OpMode = Calibration THEN
            Spectra.checkpoint()
         END;
         OpMode := OM.Data & 3
      END;
      SysClppng.Data := FALSE;
      DebugOut.writeString("OpMode: "); DebugOut.writeInteger(OpMode); DebugOut.writeln();
      IF CurrentModel.Data # M THEN
         ModelLoaded := FALSE;
         SysModel.Data := FALSE; SysChemoErr.Data := FALSE;
         TRY
            FtIrChemometryEngine.loadModel("VialAnalysis", M);
            BckGnd := Spectralon;
            CurrentModel.Data := M;
            ModelLoaded := TRUE;
            SysModel.Data := TRUE;
            IF BckGnd[0].valid AND BckGnd[1].valid AND M # SpectralonModel THEN
               VialAnalysis.setBackgroundF(BckGnd[Spectrometer.Forward].data);
               VialAnalysis.setBackgroundB(BckGnd[Spectrometer.Backward].data)
            END
         CATCH
            CurrentModel.Data := noModel
         END;
         DebugOut.writeString("ModelNumber: "); DebugOut.writeInteger(CurrentModel.Data); DebugOut.writeln()
      END
   END;
   FtIrCANopen.TPDOwrite(1)
END

TASK ModelReporter PERIODIC 1000
BEGIN
   FtIrCANopen.TPDOwrite(1);
   SysTgl.Data := NOT SysTgl.Data
END

// -----------------------------------------------------------------------------

TASK Analyst ON FtIr.SpectrumAvailable
VAR
   ARRAY[2] OF BLOB ModelSpec MAP TO Spectra.BlobStore[SetFModel];
   ARRAY[2] OF INTEGER VialNo MAP TO Spectra.IntegerStore[SetFModel];
   ARRAY[2] OF BLOB ReferenceSpec MAP TO Spectra.BlobStore[SetFRef];

   CANopen.IntegerOdEntry 
      ResStatus MAP TO FtIrCANopen.EntryA100u16[0],
      ResVialNo MAP TO FtIrCANopen.EntryA100u16[1],
      ResHumidity MAP TO FtIrCANopen.EntryA100u16[2]; 

   ARRAY[2] OF Measurement Result;

   ARRAY[2] OF BackGround BckGnd;
   ARRAY[2] OF INTEGER BckGndCnt;
   BOOL lowSpeed, enter80Hz;

   INTEGER cnt, calCntr;
   ARRAY[2] OF FLOAT cal, calArea;
   BOOL repCal

//------------ production mode -------------------------------------------------

PROCEDURE doProduction(INTEGER me)
VAR
   VialStore V;
   FLOAT h, i, p, v, a;
   BOOL ChemoOk;
   INTEGER vn, other
BEGIN
   V := Vials;
   other := (me + 1) MOD 2;
   vn := findVialNo(V, mStart);
   IF vn # nothing THEN
      ChemoOk := TRUE; 
      TRY
         IF me = Spectrometer.Forward THEN
            [h, i, p, v, a] := VialAnalysis.analyzeF(Spectrum)
         ELSE
            [h, i, p, v, a] := VialAnalysis.analyzeB(Spectrum)
         END
      CATCH
        ChemoOk := FALSE;
         SysChemoErr.Data := TRUE;
         FtIrCANopen.TPDOwrite(1)
      END;
      IF ChemoOk THEN
         SysChemoErr.Data := FALSE;
         IF vn # Result[me].VialNo THEN
            IF Result[me].VialNo # nothing THEN
               reportResults(Result[me], Result[other])
            END;
            Result[0].VialNo := vn; Result[0].Area := 0.0;
            Result[1].VialNo := vn; Result[1].Area := 0.0
         END;
         updateMeasurement(Result[me], h, i, p, v, a)
      END
   END
END

PROCEDURE findVialNo(VAR VialStore Vs, TIMESTAMP strt) : INTEGER
VAR
   INTEGER r, p
BEGIN
   r := nothing; 
   p := (Vs.in + 1) MOD VialCnt; 	// start with the oldest entry
   FOR i := 0 TO VialCnt - 1 DO
      IF Vs.recent[p].strt < strt THEN
         r := Vs.recent[p].no
      END;
      p := (p + 1) MOD VialCnt
   END;
   RETURN r
END

PROCEDURE updateMeasurement(VAR Measurement M, FLOAT h, FLOAT i, FLOAT p, FLOAT v, FLOAT a)
BEGIN
   IF M.Area < a THEN
      M.Humidity := h; M.Identity := i; M.Particle := p; M.VisAspect := v;
      M.Area := a
   END
END

PROCEDURE reportResults(VAR Measurement m1, VAR Measurement m2) 
VAR
   FLOAT h, i, p, v, a;
   INTEGER status, hum
BEGIN
   IF m1.VialNo = nothing THEN
      RETURN
   END;
   IF m1.Area > m2.Area OR m1.VialNo # m2.VialNo THEN 
      h := m1.Humidity; i := m1.Identity; p := m1.Particle; v := m1.VisAspect; a := m1.Area
   ELSE
      h := m2.Humidity; i := m2.Identity; p := m2.Particle; v := m2.VisAspect; a := m2.Area
   END;

   status := StatOk | StatId | StatPart | StatVis | StatHum;
   IF a < AreaLimit THEN 
      status := status & ~StatOk
   END;
   IF i < 1.0 THEN
      status := (status | StatNOk) & ~(StatOk | StatId)
   END;
   IF p > 1.0 THEN
      status := (status | StatNOk) & ~(StatOk | StatPart)
   END;
   IF v < 1.0 THEN
     status := (status | StatNOk) & ~(StatOk | StatVis)
   END;
   IF h < 0.0  OR h > HumidityMax THEN
     status := (status | StatNOk) & ~(StatOk | StatHum)
   END;

   DebugOut.writeString(" Vial back: "); DebugOut.writeInteger(m1.VialNo);
   DebugOut.writeString(" Hum: "); DebugOut.writeFloat(h);
   DebugOut.writeString(" Area: "); DebugOut.writeFloat(a); DebugOut.writeln();

   ResStatus.Data := status; ResVialNo.Data := m1.VialNo;
   hum := INTEGER(h * 10000.0);
   IF hum < 0 OR hum > 65535 THEN
      hum := 65535
   END;
   ResHumidity.Data := hum;
   FtIrCANopen.TPDOwrite(0)
END

//------------ chemometric modelling mode --------------------------------------

PROCEDURE doGatherModel(INTEGER me)
VAR
   VialStore V;
   INTEGER vn
BEGIN
   V := Vials;
   vn := findVialNo(V, mStart);
   IF vn # nothing THEN
      VialNo[me] := vn;
      ModelSpec[me] := Spectrum;
      Spectra.writeDataset(SetFModel + me)
   END
END

//------------ calibration mode ------------------------------------------------

PROCEDURE doCalibration(INTEGER me)
BEGIN
   ReferenceSpec[me] := Spectrum;
   Spectra.writeDataset(SetFRef + me);
   BckGndCnt[me] := BckGndCnt[me] + 1;
   IF BckGndCnt[me] = 2 THEN
      BckGnd[me].valid := TRUE; BckGnd[me].data := Spectrum;
      TRY
         IF me = Spectrometer.Forward THEN
            [cal[me], calArea[me]] := VialAnalysis.calibrateF(Spectrum)
         ELSE
            [cal[me], calArea[me]] := VialAnalysis.calibrateB(Spectrum)
         END
      CATCH
         SysChemoErr.Data := TRUE;
         FtIrCANopen.TPDOwrite(1)
      END
   END;
   IF BckGnd[0].valid AND BckGnd[1].valid THEN
      enter80Hz := TRUE;
      SysChemoErr.Data := FALSE;
      IF CurrentModel.Data = SpectralonModel THEN
         Spectralon := BckGnd;
         SysBckgndOK.Data := TRUE;
         FtIrCANopen.TPDOwrite(1)
      END
   END
END

PROCEDURE reportCalibration()
VAR
   FLOAT c
BEGIN
   IF calArea[0] > calArea[1] THEN
      c := cal[0]
   ELSE
      c := cal[1]
   END;
   DebugOut.writeString(" Cal: "); DebugOut.writeFloat(c); DebugOut.writeln();
   IF c < 1.0 THEN
      SysCalibNOk.Data := FALSE;
      ResStatus.Data := StatOk
   ELSE
      SysCalibNOk.Data := TRUE;
      ResStatus.Data := StatNOk
   END;
   FtIrCANopen.TPDOwrite(1);
   ResHumidity.Data := 65535;
   ResVialNo.Data := calCntr;
   calCntr := calCntr + 1;
   calArea[0] := 0.0; calArea[1] := 0.0;
   FtIrCANopen.TPDOwrite(0)
END

//------------ analysis states -------------------------------------------------

BEGIN
   cnt := cnt + 1;
   SpectraCnt := cnt;
   IF OpMode # Calibration THEN
      lowSpeed := FALSE; enter80Hz := FALSE
   END;
   IF mEnd < lastShutterClsdT AND ModelLoaded AND OpMode = Production THEN
      doProduction(MirrorDir)
   ELSIF ShutterOpen AND OpMode = GatherModel THEN
      doGatherModel(MirrorDir)
   ELSIF ShutterOpen AND OpMode = Calibration THEN
      doCalibration(MirrorDir);
      repCal := TRUE
   ELSIF NOT ShutterOpen AND OpMode = Calibration THEN
      IF NOT lowSpeed AND NOT enter80Hz THEN
         lowSpeed := TRUE;
         FtIr.setAverageCount(40);
         BckGndCnt[0] := 0; BckGndCnt[1] := 0;
         BckGnd[0].valid := FALSE; BckGnd[1].valid := FALSE;
         IF CurrentModel.Data = SpectralonModel THEN
            Spectralon := BckGnd;
	    SysBckgndOK.Data := FALSE;
            FtIrCANopen.TPDOwrite(1)
         END
      ELSIF enter80Hz THEN
         lowSpeed := FALSE;
         FtIr.setAverageCount(1)
      END;
      IF repCal THEN
         repCal := FALSE;
         reportCalibration()
      END
   ELSIF NOT ShutterOpen THEN
      Result[0].VialNo := nothing; Result[1].VialNo := nothing;
      Result[0].Area := 0.0; Result[1].Area := 0.0
   END
END

TASK SpectraCounter PERIODIC 1000
VAR 
   INTEGER acc, cnt, sps
BEGIN
   cnt := SpectraCnt;
   sps := cnt - acc;
   DebugOut.writeString("Spectra per sec.: "); DebugOut.writeInteger(sps); DebugOut.writeln();
   IF sps < MinSpectraPerS AND OpMode # Calibration THEN
      SysOk.Data := FALSE;
      FtIrCANopen.TPDOwrite(1)
   ELSE
      SysOk.Data := TRUE
   END;
   acc := cnt	
END

// -----------------------------------------------------------------------------

TASK NmtReporter ON FtIrCANopen.LocalNmtStateChange
BEGIN
   IF NewNmtState = CANopen.NmtOperational THEN
      FtIrCANopen.TPDOwrite(0)
   END
END

TASK ClippingReporter ON FtIr.Status
BEGIN
   IF ShutterOpen THEN
      IF status & Spectrometer.OverloadMsk # 0 THEN
         SysClppng.Data := TRUE; 
         FtIrCANopen.TPDOwrite(1)
      ELSE
         SysClppng.Data := FALSE
      END
   END
END
         
TASK Reseter ON FtIr.Watchdog
BEGIN
   FtIr.Reset()
END 

TASK LightControler PERIODIC 1000
VAR
   CANopen.IntegerOdEntry
      LightCtrl MAP TO FtIrCANopen.EntryA040u8[1]
BEGIN
   LightCtrl.Data := 3;
   FtIrCANopen.TPDOwrite(2)
END

// TASK LightObserver ON FtIrCANopen.RPDOrcvd[2]
// VAR
//   CANopen.IntegerOdEntry
//      LightStatus MAP TO FtIrCANopen.EntryA4C0u8[0]
// BEGIN
//   SysLampFail.Data := LightStatus.Data # 48;
//   IF SysLampFail.Data THEN
//      FtIrCANopen.TPDOwrite(1)
//   END
// END

BEGIN
   Spectra.configure(10000000, 2);
   Spectra.addBlobToDataset(Spectra.BlobStore[0], SetFModel);
   Spectra.addIntegerToDataset(Spectra.IntegerStore[0], SetFModel);
   Spectra.addBlobToDataset(Spectra.BlobStore[1], SetBModel);
   Spectra.addIntegerToDataset(Spectra.IntegerStore[1], SetBModel);
   Spectra.addBlobToDataset(Spectra.BlobStore[2], SetFRef);
   Spectra.addBlobToDataset(Spectra.BlobStore[3], SetBRef);

   FtIrCANopen.startBus(125, 1, "GlaxoSlave.bin");

   FtIr.setIrGain(9);
   FtIr.configInterferogramAverager(64, 0.5, 1.5, 1, 5, 0, 30000, 700);
   FtIr.SwitchMirrorControl(TRUE);

   FOR i := 0 TO VialCnt - 1 DO
      Vials.recent[i].strt := EOT; Vials.recent[i].no := nothing
   END;
   Vials.in := 0;

   lastShutterClsdT := BOT;
   CurrentModel.Data := noModel

END.
