// Monitor shared memory and make control decisions

#include <stdio.h>
#include <fcntl.h>
#include <sys/mman.h>
#include <sys/types.h>
#include <sys/ipc.h>
#include <sys/shm.h>
#include "../io/io2.h"

#define NOMODE 0
#define OILMODE 1
#define WOODMODE 2
#define TANKMODE 3
#define HWSCAVENGE 4
#define HTSCAVENGE 5
#define TANKSCAVENGE 6

#define TANK_BOTTOM shm->advalue[0]
#define TANK_MIDDLE shm->advalue[1]
#define TANK_TOP shm->advalue[2]
#define SOLAR_IN shm->advalue[3]

#define WOOD_OUT shm->advalue[4]
#define WOOD_IN shm->advalue[5]
#define OIL_OUT shm->advalue[6]
#define HOT_WATER shm->advalue[7]

#define FLUE shm->advalue[8]
#define COMBUSTION shm->advalue[9]

main(){
  float inlimit, outlimit, hysteresis, outgoal;
  int mode, mode2, woodcirc, recirc, tankzv, dhwzv, htzv, woodphase;
  int i,j,demand,bbdemand;
  byte htwarm, dhwwarm, htcomfort, oil_wood_sw, last_oil_wood_sw, testbit;
  byte dio0, dio1, dio2, dio3, dio5, rmask, newbit, oldbit;
  byte bfwarm, mfwarm, tfwarm;
    
  struct timeval now;
  long start_usec;
  long now_usec;
  long wood_end_time;
  long recovery_end_time;
  long pump_end_time;
  long wait_end_time;
  //time_t timestamp;

  int shmid;
  key_t key;
  char *shmat();
  //short *shm, *s;
  shmstruct *shm;
   
  // Key for our shared memory is 9700
  
  key = 9700;
  
  // Create our shared memory
  if ((shmid = shmget(key, sizeof(shmstruct), 0666)) < 0) {
    perror("shmget");
    exit(1);
  }
  
  // Link to our shared memory
  if ((shm=shmat(shmid, NULL, 0)) == (char *) -1) {
    perror("shmat");
    exit(1);
  }
  
  gettimeofday(&now,NULL);
  start_usec = now.tv_sec * 1000000 + now.tv_usec;

  // Set direction bytes
  
  // dio 0 is input
  shm->diodir[0] = 0;
     
  // dio 1 is output
  shm->diodir[1] = 255;
     
  // dio 2 is input
  shm->diodir[2] = 0;
     
  // dio 3 is output
  shm->diodir[3] = 255;
     
  // dio 5 is output
  shm->diodir[5] = 255;
     
  // Intialaize mode to nomode
  
  mode = NOMODE;
  last_oil_wood_sw = 0;

  woodcirc = 3;
  recirc = 0;
  tankzv = 0;
  dhwzv = 0;
  htzv = 0;
  woodphase = 0;
  
  gettimeofday(&now,NULL);
  wood_end_time = now.tv_sec -1;
      
  // Do forever
  while (1){
    //timestamp = time(NULL);   
    gettimeofday(&now,NULL);

    // Read input byte(s)
    dio0 = shm->diovalue[0];
    
    // Strip out nodemand bit
    demand = dio0 & 0x01;
    // Strip out hot tub demand bit
    htwarm = dio0 & 0x02;
    // Strip out hot tub comfort (not econ)  bit
    htcomfort = dio0 & 0x04;
    // Strip out oil/wood switch  bit
    oil_wood_sw = dio0 & 0x08;
    // Strip out DHW Warm bit
    dhwwarm = dio0 & 0x10;
    // Strip out top floor Warm bit
    tfwarm = dio0 & 0x20;
    // Strip out main floor Warm bit
    mfwarm = dio0 & 0x40;
    // Strip out bottom floor Warm bit
    bfwarm = dio0 & 0x80;
    
    // Read output byte(s) to get current state
    dio1 = shm->diovalue[1];
    dio3 = shm->diovalue[3];
    dio5 = shm->diovalue[5];
    //printf("Starting dio5 = %d\n",dio5);
    
    // ************************************************************************************************ 
    // Rule details are in documentation for each bit.
    // In general, there may be different rules depending on the heat source.
    // In addition to detailed rules, there is a set of general priorities:
    //
    // In wood mode:
    // 1) Keep output temp within bounds. Open recirc and limit active zones to raise temp. Open more
    //    zones to lower temp.
    // 2) Heat Domestic Hot Water if it's cold enough to ask for heat.
    // 3) Heat the hot tub if it's cold and 'comfort' is selected
    // 4) Heat the living space
    // 5) Heat the hot tub
    // 6) Heat DHW to 60 degrees C
    // 7) Heat the storage tank
    //
    // In tank mode:
    // 1) Heat Domestic Hot Water if it's cold enough to ask for heat and the tank is hot enough to help.
    // 2) Heat the hot tub if it's cold and 'comfort' is selected and the tank is hot enough to help.
    // 3) Heat the living space.
    // ************************************************************************************************ 
    
    // ************************************************************************************************ 
    // Figure out mode   
    // If wood boiler outlet is hot, wait an hour before giving up on wood
    // Get oil+wood switch and compare to previous state
    // If oil/wood is closed and was not closed on previous cycle, reset end time.
    // ************************************************************************************************ 
    
    // Baseboard demand means main and/or top floor need heat
    bbdemand = !mfwarm || !tfwarm; 
    
    // If wood/oil switch was cycled from auto to wood since last cycle, set wood mode
    if ((last_oil_wood_sw != 0) && (oil_wood_sw == 0)){
      wood_end_time = now.tv_sec + 3600;
    }
    last_oil_wood_sw = oil_wood_sw;
   
    // If wood outlet is hot, we're in wood mode for the next hour.
    if(WOOD_OUT > 55){
      wood_end_time = now.tv_sec + 3600;
    }

    // Wood phases:
    // 1 = startup from cold
    // 2 = ramp from established gasification to operating range
    // 3 = normal operation
    // 4 = tank charging (no other demand)
    // 5 = dying fire heat recovery

    // If flue is over 100 and combustion is less than 50, we just lighted a fire. Set woodphase=1;
    if ((FLUE > 100) && (COMBUSTION < 50)){
      woodphase = 1;
      wood_end_time = now.tv_sec + 3600;
      recovery_end_time = now.tv_sec + 1800;
    }

    // If we are in woodphase 0 or 1 and outlet reaches 55, we transition to phase 2.
    if ((woodphase < 2) && (WOOD_OUT > 55)){
      woodphase = 2;
      recovery_end_time = now.tv_sec + 1800;
    }

    // If wood out > 65 and we are not in phase 4, then we are in phase 3
    if ((woodphase !=4) && (WOOD_OUT > 65)){
      woodphase = 3;
      recovery_end_time = now.tv_sec + 900;
    }

    // If wood out > 65 and there is any demand then we are in phase 3
    if ((WOOD_OUT > 65) && (demand || !bfwarm || htzv || dhwzv)){
      woodphase = 3;
      recovery_end_time = now.tv_sec + 900;
    }

    // If we're in regular burn or tank charge and we couldn't get hot enough, go to recovery
    // Give ourselves half an hour
    if ((woodphase < 5) && (recovery_end_time < now.tv_sec)){
      woodphase = 5;
      recovery_end_time = now.tv_sec + 1800;
    }

    // If boiler is hot and there's no other high-temp demand, then we're in phase 4 - tank charging
    // Give ourselves 15 minutes to get hot enough to do some good
    if ((woodphase > 1) && (woodphase < 4)){
      if (!demand && bfwarm && !dhwzv){
        woodphase = 4;
        recovery_end_time = now.tv_sec + 900;
      }
    }

    printf ("mode = %d, phase = %d ",mode,woodphase);

    // Hysteresis - normal
    if (mode == TANKMODE){
      outlimit = 49;
    }else{
      outlimit = 50;
    }

    
    // Hysteresis - early switch to oil
    if (mode == TANKMODE){
      //outlimit = 65;
    }else{
      //outlimit = 66;
    }
    
    // If woodmode timer has expired, check if tank is hot enough to be useful...
    if (wood_end_time < now.tv_sec){
      if (TANK_TOP > outlimit){
        mode = TANKMODE;
      }else{
        mode = OILMODE;
      }
    }else{
      mode = WOODMODE;
    }
    
    // ************************************************************************************************ 
    // DIO 5 bit 0 & 1: Wood circ A & B relay control
    // 
    //   A  B
    //   0  0  On / High            Temps above 75 or wood circ control not enabled
    //   0  1  On / Medium          65-75
    //   1  0  On / Low             55 -65
    //   1  1  Off / Low            < 55 or not in woodmode
    //
    // ************************************************************************************************ 
    
    // Create mask, capture old bit value
    rmask = 3;
    oldbit = dio5 & rmask;
    

    outgoal = 70;

    if (woodphase == 4){
      outgoal = TANK_MIDDLE + 5;
    }

    // Hysteresis
    if(oldbit){
      hysteresis = 0;
    }else{
      hysteresis = 2;
    }

    newbit = 3;             // Off

    // Faster speeds at higher temps
    if ((mode == WOODMODE)){
      if(WOOD_OUT > (55 + hysteresis)){
        newbit = 2;
      }
      if(WOOD_OUT > (outgoal - 5 + hysteresis)){
        newbit = 1;
      }
      if(WOOD_OUT > (outgoal+hysteresis)){
          newbit = 0;
      }
    }

    // Input protection trumps output temp rules
    // If inlet is low and pump is medium or fast, slow pump down
    if (WOOD_IN < 55 && newbit < 2){
      newbit++;
    }


    // Change existing setting by no more than 1
    if (newbit > woodcirc){
      woodcirc++;
    }

    if (newbit < woodcirc){
      woodcirc--;
    }

    
    newbit = woodcirc;

    // Recovery phase (woddphase 5) has special rules:
    // - wait for 10 minutes or until woodout > 60
    // - if outlet < 50, done
    // - Pump on for three minutes or until woodout < 40
    // - repeat

    if (woodphase == 5){
      if (demand){
        // wait for 20 minutes or until hot enough
        if ((wait_end_time < now.tv_sec) || (WOOD_OUT > 60)){
          // If there's still heat, reset timers and turn on pump
          if (WOOD_OUT > 50) {
            wait_end_time < now.tv_sec + 1800;
            recovery_end_time < now.tv_sec + 2400;
            wood_end_time < now.tv_sec + 2400;
            pump_end_time < now.tv_sec + 300;
            // We're turning pump on. Speed is based on delta to tank middle.
            newbit = 2;   // Slow
            if ((WOOD_OUT - TANK_MIDDLE) > 5){
              newbit = 1; // Medium
            }
            if ((WOOD_OUT - TANK_MIDDLE) > 10){
              newbit = 0; // fast
            }
          }
        }else{
          // We're waiting OR wood out is < 60. Pump may be on
          // If pump timer has expired, turn it off.
          if ((pump_end_time < now.tv_sec) || (WOOD_OUT < 40)){
            newbit = 3;
          }
        }
      }else{ 
        // No demand - wait....
        recovery_end_time < now.tv_sec + 2400;
        wood_end_time < now.tv_sec + 2400;
        newbit = 3;
      }
    }

    // At cold start, run pump to heat boiler from tank.
    if (woodphase == 1){
      newbit = 1; // Medium
    }

    // If we're not controlling the pump, open relays. This duplicates the logic for the
    //wood circ control bit below (dio5 bit 2)

    if (mode != WOODMODE){
      newbit = 0;
    }

    woodcirc = newbit;

    printf ("pump speed = %d\n",woodcirc);

    // Insert new bit value
    dio5 = dio5 & (~rmask);
    dio5 = dio5 | newbit;
    
    
    // ************************************************************************************************ 
    // DIO 5 bit 2: Wood circ control
    // 
    //  Asserted when we want to second-guess EKO
    //
    // ************************************************************************************************ 
    
    // Create mask, capture old bit value
    rmask = 1 << 2;
    oldbit = dio5 & rmask;

    if (mode == WOODMODE){
      newbit = rmask;
    }else{
      newbit = 0;
    }

    // Insert new bit value
    dio5 = dio5 & (~rmask);
    dio5 = dio5 | newbit;
    
    
    // ************************************************************************************************ 
    // DIO 5 bit 3: EKO fan disable
    // 
    //  Asserted when we want fan not to run - when fire is out.
    //
    // ************************************************************************************************ 
    
    // Create mask, capture old bit value
    rmask = 1 << 3;
    oldbit = dio5 & rmask;

    // Not implemented yet
    newbit = 0;

    // Insert new bit value
    dio5 = dio5 & (~rmask);
    dio5 = dio5 | newbit;
    
    
    // ************************************************************************************************ 
    // DIO 3 bit 0: Wood Mode 
    // 
    // ************************************************************************************************ 
    
    // Create mask, capture old bit value
    rmask = 1 << 0;
    oldbit = dio3 & rmask;

    
    if (mode == WOODMODE){
      newbit = rmask;
    }else{
      newbit = 0;
    }
    
    // Insert new bit value
    dio3 = dio3 & (~rmask);
    dio3 = dio3 | newbit;
    
    // ************************************************************************************************ 
    // DIO 3 bit 1: Oil Mode 
    // 
    // ************************************************************************************************ 
    
    // Create mask, capture old bit value
    rmask = 1 << 1;
    oldbit = dio3 & rmask;
    
    if (mode == OILMODE){
      newbit = rmask;
    }else{
      newbit = 0;
    }
    
    // Insert new bit value
    dio3 = dio3 & (~rmask);
    dio3 = dio3 | newbit;
    
    // ************************************************************************************************ 
    // DIO 3 bit 2: Tank Mode 
    // 
    // ************************************************************************************************ 
    
    // Create mask, capture old bit value
    rmask = 1 << 2;
    oldbit = dio3 & rmask;
    
    if (mode == TANKMODE){
      newbit = rmask;
    }else{
      newbit = 0;
    }
    
    // Insert new bit value
    dio3 = dio3 & (~rmask);
    dio3 = dio3 | newbit;
    
    // ************************************************************************************************ 
    // DIO 3 bit 3: Stage 1
    // 
    // We use stage 1 / occupied for wood boiler - 72 degees
    // We use stage 2 / occupied for tank - 70 degrees
    // We use stage 1 / unoccupied for oil - 67 degrees
    // ************************************************************************************************ 
    
    // Create mask, capture old bit value
    rmask = 1 << 3;
    oldbit = dio3 & rmask;
    
    if (mode != TANKMODE){
      newbit = rmask;
    }else{
      newbit = 0;
    }
    
    // Insert new bit value
    dio3 = dio3 & (~rmask);
    dio3 = dio3 | newbit;
    
    // ************************************************************************************************ 
    // DIO 3 bit 4: Occupied
    // 
    // ************************************************************************************************ 
    
    // Create mask, capture old bit value
    rmask = 1 << 4;
    oldbit = dio3 & rmask;
    
    if (mode != OILMODE){
      newbit = rmask;
    }else{
      newbit = 0;
    }
    
    // Insert new bit value
    dio3 = dio3 & (~rmask);
    dio3 = dio3 | newbit;
    
    // ************************************************************************************************ 
    // DIO 1 bit 0: Hot water force
    // Assert demand for hot water: open hot water zone valve.
    // ************************************************************************************************ 
    
    // Create mask, capture old bit value
    rmask = 1 << 0;
    oldbit = dio1 & rmask;
    
    // Hysteresis
    if(oldbit){
      inlimit=70;
      outlimit=73;
    }else{
      inlimit=69;
      outlimit=75;
    }
    
    newbit = 0;
    
    // First rule: If there's demand for hot water, satisfy it first.
    if ((mode == WOODMODE) && !dhwwarm && (WOOD_OUT > 65)){
      newbit = rmask;
    }
  
    // If hot water needs more heat and wood boiler is hot enough....
    if ((mode == WOODMODE) && (HOT_WATER < inlimit) && (WOOD_OUT > (HOT_WATER + 2)) && (WOOD_OUT > inlimit)){
      // and the hot tub is warm and baseboards are warm
      if (!bbdemand && htwarm){
        newbit = rmask;
      }
      // or if wood is really hot
      if (WOOD_OUT > outlimit){
        newbit = rmask;
      }
    }
    
    // If tankmode and there's enough heat and hot water is getting cool
    if(oldbit){
      inlimit=47;
      outlimit=7;
    }else{
      inlimit=44;
      outlimit=8;
    }
    if (mode == TANKMODE){
      if (TANK_TOP > (HOT_WATER + outlimit) ) {
        if (HOT_WATER < inlimit) {
          newbit = rmask;
        }
      }
    }

    // If oilmode and hwscavenge, open zone valve
    if ((mode == OILMODE) && (mode2 == HWSCAVENGE )) {
      newbit = rmask;
    }

    dhwzv = newbit;

    // Insert new bit value
    dio1 = dio1 & (~rmask);
    dio1 = dio1 | newbit;
    
    // ************************************************************************************************ 
    // DIO 1 bit 1: Hot Tub Control
    // Asserts TS7260 control over hot tub zone valve
    // Asserted unless in oilmode and htcomfort is true
    // ************************************************************************************************ 
    
    // Create mask, capture old bit value
    rmask = 1 << 1;
    oldbit = dio1 & rmask;
    
    if (OILMODE && htcomfort){
      newbit = 0;
    }else{
      newbit = rmask;
    }

    // Insert new bit value
    dio1 = dio1 & (~rmask);
    dio1 = dio1 | newbit;
    
    // ************************************************************************************************ 
    // DIO 1 bit 2: Wood Mode force
    // This is actually the relay that disables oil demand - should be called 'oil disable'
    // 
    // ************************************************************************************************ 
    
    // Create mask, capture old bit value
    rmask = 1 << 2;
    oldbit = dio1 & rmask;
    
    // Must use 'wood mode force' when scavenging hot water from the oil boiler 
    // so that the hot ware zone valve closure does not turn the boiler back on.

    if ((mode == WOODMODE) || (mode == TANKMODE) || (mode2 == HWSCAVENGE) || (mode2 == HTSCAVENGE)){
      newbit = rmask;
    }else{
      newbit = 0;
    }
    
    // Insert new bit value
    dio1 = dio1 & (~rmask);
    dio1 = dio1 | newbit;
    
    // ************************************************************************************************ 
    // DIO 1 bit 3: Wood recirc 
    // Never if not woodmode
    // Never if wood is cold (<55)
    // On if either wood outlet or inlet is below target operating temps.
    // ************************************************************************************************ 
    
    // Create mask, capture old bit value
    rmask = 1 << 3;
    oldbit = dio1 & rmask;

    // Hysteresis
    if(oldbit){
      outlimit=67;
      inlimit=58;
    }else{
      outlimit=65;
      inlimit=56;
    }
              
    //if ((mode == WOODMODE) && ((WOOD_OUT < outlimit) || (WOOD_IN < inlimit)) && (WOOD_OUT > 55)){
    if ((WOOD_IN < inlimit) && (WOOD_OUT > 55)) {
      newbit = rmask;
    }else{
      newbit = 0;
    }
    
    // If wood circ wants to be on and we're not in woodmode, open recirc
    if ((woodcirc < 3) && (mode != WOODMODE) && (WOOD_OUT > 55)){
      newbit = rmask;
    }

    // If wood circ wants to be on and there's no demand, open recirc
    if ((woodcirc < 3) && !demand && htwarm && dhwwarm && !tankzv){
      newbit = rmask;
    }

    // If we're in woodphase 5 (recovery) close recirc
    if (woodphase == 5){
      newbit = 0;
    }

    // If we're in woodphase 1 (startup) open recirc
    if (woodphase == 1){
      newbit = rmask;
    }

    recirc = newbit;

    // Insert new bit value
    dio1 = dio1 & (~rmask);
    dio1 = dio1 | newbit;
        
    // ************************************************************************************************ 
    // DIO 1 bit 4: Heat storage tank circulator pump 
    // Test: If testbit is zero, turn on circ
    // ************************************************************************************************ 
    
    // Create mask, capture old bit value
    rmask = 1 << 4;
    oldbit = dio1 & rmask;
    // Hysteresis
    if(oldbit){
      outlimit=2;
    }else{
      outlimit=3;
    }
    
    newbit = 0;
    
    if (mode == TANKMODE){
      // Need to not turn on pump for DHW demand. Hack at present, need better logic.
      //if (demand  && dhwwarm){
      if (bbdemand || dhwzv){
        newbit = rmask;
      }
      if (!htwarm && htcomfort){
        newbit = rmask;
      }
    }

    // If we're in woodphase 1 (cold start) run circ to heat up zone water
    if (woodphase == 1){
      newbit = rmask;
    }


    // Insert new bit value
    dio1 = dio1 & (~rmask);
    dio1 = dio1 | newbit;
    
    // ************************************************************************************************ 
    // DIO 1 bit 5: Heat storage tank zone valve relay 
    // If boiler outlet is more than 10 degrees C above the tank middle and there is no demand and the hot tub is warm, turn on ht zv relay
    // ************************************************************************************************ 
    
    // Create mask, capture old bit value
    rmask = 1 << 5;
    oldbit = dio1 & rmask;
    // Hysteresis
    if(oldbit){
      outlimit=75;
    }else{
      outlimit=76;
    }
              
    newbit = 0;
    
    if(mode == WOODMODE){
      
      // If overheating, dump to tank.
      if ((WOOD_OUT > outlimit)){
        newbit = rmask;
      }
      
      // If there's no other demand, dump to tank conditionally
      if ((WOOD_OUT > 60) && htwarm && !demand) {
         
        // If there's enough temperature difference to be useful
        if (WOOD_OUT > (TANK_MIDDLE+2)) {
        
          // If there's no hot water demand
          if(HOT_WATER >= 60) {
            newbit = rmask;
          }
        }
      }
    }
    
   
    if(mode2 == TANKSCAVENGE){
      newbit = rmask;
    }

    tankzv = newbit;

    // Insert new bit value
    dio1 = dio1 & (~rmask);
    dio1 = dio1 | newbit;
    
    // ************************************************************************************************ 
    // DIO 1 bit 6: hot tub zone valve relay 
    // If boiler outlet is above 45 degrees C and there is no demand and the hot tub is cold, turn on ht zv relay
    // ************************************************************************************************ 
    
    // Create mask, capture old bit value
    rmask = 1 << 6;
    oldbit = dio1 & rmask;
    
    // Hysteresis
    if(oldbit){
      outlimit=73;
    }else{
      outlimit=74;
    }
    
    newbit = 0;
    
    if (mode == OILMODE && !htwarm){
      if ((OIL_OUT > 45) && !demand){
        newbit = rmask;
      }
    }
    
    if ((mode == WOODMODE) && !htwarm){
      if ((WOOD_OUT > outlimit) || (!bbdemand && (WOOD_OUT > (outlimit-10)))){
        newbit = rmask;
      }
    }

    // Not from tank for now
    if (mode == TANKMODE){
      newbit = 0;
    }
    
    // Hack for hot tub 'normal' mode
    
    if (!htwarm && htcomfort){
      newbit = rmask;
    }
    
    // If oilmode and htscavenge, open zone valve
    if ((mode == OILMODE) && (mode2 == HTSCAVENGE )) {
      newbit = rmask;
    }

    htzv = newbit;

    // Insert new bit value
    dio1 = dio1 & (~rmask);
    dio1 = dio1 | newbit;
    
    // ************************************************************************************************ 
    // DIO 1 bit 7: oil circ force enable
    // If boiler outlet is above 40 degrees C and there is no demand, scavenge heat:
    // 1) If hot water is < 54 and boiler is > 60, heat hot water (mode2=hwscavenge)
    // 2) If hot tub needs heat and boiler is > 45, heat hot tub (mode2=htscavenge)
    // 3) If hot tub is warm and boiler is > 3 degrees above hot water, heat hot water (mode2=hwscavenge)
    // 4) 
    // ************************************************************************************************ 
    
    // Create mask, capture old bit value
    rmask = 1 << 7;
    oldbit = dio1 & rmask;
    newbit = 0;
    mode2 = NOMODE;

    if (mode == OILMODE && (OIL_OUT > 40) && dhwwarm && !bbdemand & !(!htwarm && htcomfort)){
      //printf("oil out = %f, hot water = %f\n",OIL_OUT,HOT_WATER);

      // We might be able to scavenge. Figure out mode
      
      // If we're really hot, overheat domestic hot water tank (we can use lower temp water for hot tub)

      if ( (HOT_WATER < 54) && (OIL_OUT > 60) ){
        newbit = rmask;
        mode2 = HWSCAVENGE;
      }

      // If we're not heating hot water and hot tub needs heat, give it some....

      if ( (mode2 == NOMODE) && !htwarm && (OIL_OUT > 45)){
          newbit = rmask;
          mode2 = HTSCAVENGE;
      }

      // If hot tub is done, try using colder water to heat DHW tank if we can


      if ( (mode2 == NOMODE) && ((OIL_OUT -6) > HOT_WATER) ){
        newbit = rmask;
        mode2 = HWSCAVENGE;
      }

      // Finally, try putting the extra heat in the outside tank

      if ( (mode2 == NOMODE) && ((OIL_OUT -6) > TANK_MIDDLE) ){
      // disable for winter.....
        //newbit = rmask;
        //mode2 = TANKSCAVENGE;
      }


    }
    
    //printf("mode2 = %d\n",mode2);

    // Insert new bit value
    dio1 = dio1 & (~rmask);
    dio1 = dio1 | newbit;
    
    // ************************************************************************************************ 
    // write results to shared memory
    // sleep until next 35 second interval
    // ************************************************************************************************ 
    
    shm->diovalue[1] = dio1;
    shm->diovalue[3] = dio3;
    shm->diovalue[5] = dio5;
    //printf("Ending dio5 = %d\n\n",dio5);
        
    // Sleep for 35 second interval
    start_usec += 35000000;
    gettimeofday(&now,NULL);
    now_usec = now.tv_sec * 1000000 + now.tv_usec;
    
    usleep(start_usec - now_usec);
  }
}