S-Function to code Interrupts in Mega2560 doesn't give second encoder data correctly

%%S-Function Parameters%%:

enc uint8 (0) 
pinA uint8 (20) 
pinB uint8 (21) 

encm uint8 (1) 
pinAm uint8 (18)
pinBm uint8 (19) 

%%Libraries%%

# ifndef MATLAB_MEX_FILE

# include <Arduino.h>

typedef struct { int pinA; int pinB; int pos; int del; int pinAm; int pinBm; int posm;} Encoder;    
volatile Encoder Enc[3] = {{0,0,0,0}, {0,0,0,0}, {0,0,0,0}};

/* auxiliary function to handle encoder attachment           */
int getIntNum(int pin) {
/* returns the interrupt number for a given interrupt pin 
   see http://arduino.cc/it/Reference/AttachInterrupt        */
switch(pin) {
  case 2:
    return 0;
  case 3:
    return 1;
  case 21:
    return 2;
  case 20:
    return 3;
  case 19:
    return 4;
  case 18:
    return 5;   
  default:
    return -1;
  }
}

/* auxiliary debouncing function                             */
void debounce(int del) {

}

/* Interrupt Service Routine: change on pin A for Encoder 0  */
void irsPinAEn0(){

    /* read pin B right away                                   */
    int drB = digitalRead(Enc[0].pinB);

    /* possibly wait before reading pin A, then read it        */
    debounce(Enc[0].del);
    int drA = digitalRead(Enc[0].pinA);

    /* this updates the counter                                */
    if (drA == HIGH) {   /* low->high on A? */

      if (drB == LOW) {  /* check pin B */
      Enc[0].pos++;  /* going clockwise: increment         */
      } else {
      Enc[0].pos--;  /* going counterclockwise: decrement  */
      }

    } else {                       /* must be high to low on A */

      if (drB == HIGH) { /* check pin B */
      Enc[0].pos++;  /* going clockwise: increment         */
      } else {
      Enc[0].pos--;  /* going counterclockwise: decrement  */
      }

    } /* end counter update                                    */

} /* end ISR pin A Encoder 0                                 */

/* Interrupt Service Routine: change on pin B for Encoder 0  */
void isrPinBEn0(){ 

    /* read pin A right away                                   */
    int drA = digitalRead(Enc[0].pinA);

    /* possibly wait before reading pin B, then read it        */
    debounce(Enc[0].del);
    int drB = digitalRead(Enc[0].pinB);

    /* this updates the counter                                */
    if (drB == HIGH) {   /* low->high on B? */

      if (drA == HIGH) { /* check pin A */
      Enc[0].pos++;  /* going clockwise: increment         */
      } else {
      Enc[0].pos--;  /* going counterclockwise: decrement  */
      }

    } else {                       /* must be high to low on B */

      if (drA == LOW) {  /* check pin A */
      Enc[0].pos++;  /* going clockwise: increment         */
      } else {
      Enc[0].pos--;  /* going counterclockwise: decrement  */
      }

    } /* end counter update */

} /* end ISR pin B Encoder 0  */

/* Interrupt Service Routine: change on pin A for Encoder 1  */
void irsPinAmEn1(){

    /* read pin B right away                                   */
    int drBm = digitalRead(Enc[1].pinBm);

    /* possibly wait before reading pin A, then read it        */
    debounce(Enc[1].del);
    int drAm = digitalRead(Enc[1].pinAm);

    /* this updates the counter                                */
    if (drAm == HIGH) {   /* low->high on A? */

      if (drBm == LOW) {  /* check pin B */
      Enc[1].pos++;  /* going clockwise: increment         */
      } else {
      Enc[1].pos--;  /* going counterclockwise: decrement  */
      }

    } else { /* must be high to low on A                       */

      if (drBm == HIGH) { /* check pin B */
      Enc[1].pos++;  /* going clockwise: increment         */
      } else {
      Enc[1].pos--;  /* going counterclockwise: decrement  */
      }

    } /* end counter update                                    */

} /* end ISR pin A Encoder 1                                 */

/* Interrupt Service Routine: change on pin B for Encoder 1  */
void isrPinBmEn1(){ 

    /* read pin A right away                                   */
    int drAm = digitalRead(Enc[1].pinAm);

    /* possibly wait before reading pin B, then read it        */
    debounce(Enc[1].del);
    int drBm = digitalRead(Enc[1].pinBm);

    /* this updates the counter                                */
    if (drBm == HIGH) {   /* low->high on B? */

      if (drAm == HIGH) { /* check pin A */
      Enc[1].pos++;  /* going clockwise: increment         */
      } else {
      Enc[1].pos--;  /* going counterclockwise: decrement  */
      }

    } else { /* must be high to low on B                       */

      if (drAm == LOW) {  /* check pin A */
      Enc[1].pos++;  /* going clockwise: increment         */
      } else {
      Enc[1].pos--;  /* going counterclockwise: decrement  */
      }

    } /* end counter update                                    */

} /* end ISR pin B Encoder 1                                 */

/* Interrupt Service Routine: change on pin A for Encoder 2  */
void irsPinAEn2(){

    /* read pin B right away                                   */
    int drB = digitalRead(Enc[2].pinB);

    /* possibly wait before reading pin A, then read it        */
    debounce(Enc[2].del);
    int drA = digitalRead(Enc[2].pinA);

    /* this updates the counter                                */
    if (drA == HIGH) {   /* low->high on A? */

      if (drB == LOW) {  /* check pin B */
      Enc[2].pos++;  /* going clockwise: increment         */
      } else {
      Enc[2].pos--;  /* going counterclockwise: decrement  */
      }

    } else { /* must be high to low on A                       */

      if (drB == HIGH) { /* check pin B */
      Enc[2].pos++;  /* going clockwise: increment         */
      } else {
      Enc[2].pos--;  /* going counterclockwise: decrement  */
      }

    } /* end counter update                                    */

} /* end ISR pin A Encoder 2                                 */

/* Interrupt Service Routine: change on pin B for Encoder 2  */
void isrPinBEn2(){ 

    /* read pin A right away                                   */
    int drA = digitalRead(Enc[2].pinA);

    /* possibly wait before reading pin B, then read it        */
    debounce(Enc[2].del);
    int drB = digitalRead(Enc[2].pinB);

    /* this updates the counter                                */
    if (drB == HIGH) {   /* low->high on B? */

      if (drA == HIGH) { /* check pin A */
      Enc[2].pos++;  /* going clockwise: increment         */
      } else {
      Enc[2].pos--;  /* going counterclockwise: decrement  */
      }

    } else { /* must be high to low on B                       */

      if (drA == LOW) {  /* check pin A */
      Enc[2].pos++;  /* going clockwise: increment         */
      } else {
      Enc[2].pos--;  /* going counterclockwise: decrement  */
      }

    } /* end counter update                                    */

} /* end ISR pin B Encoder 2                                 */

# endif

%%Outputs:%%
/* wait until after initialization is done */
if (xD[0]==1) {

      /* don't do anything for mex file generation */
      # ifndef MATLAB_MEX_FILE

          /* get encoder position and set is as output */
          pos[0]=Enc[enc[0]].pos;

      # endif

}

if (xD[1]==1) {

      /* don't do anything for mex file generation */
      # ifndef MATLAB_MEX_FILE

          /* get encoder position and set is as output */

          pos[1]=Enc[encm[1]].pos;

      # endif

}

%%Discrete Update%%:
if (xD[0]!=1) {

      /* don't do anything for MEX-file generation */
      # ifndef MATLAB_MEX_FILE

          /* enc[0] is the encoder number and it can be 0,1 or 2   */
          /* if other encoder blocks are present in the model      */
          /* up to a maximum of 3, they need to refer to a         */
          /* different encoder number                              */

          /* store pinA and pinB in global encoder structure Enc   */
          /* they will be needed later by the interrupt routine    */
          /* that will not be able to access s-function parameters */   

          Enc[enc[0]].pinA=pinA[0];      /* set pin A              */
          Enc[enc[0]].pinB=pinB[0];      /* set pin B              */

          /* set encoder pins as inputs                            */
          pinMode(Enc[enc[0]].pinA, INPUT); 
          pinMode(Enc[enc[0]].pinB, INPUT); 

          /* turn on pullup resistors                              */
          digitalWrite(Enc[enc[0]].pinA, HIGH); 
          digitalWrite(Enc[enc[0]].pinB, HIGH); 

          /* attach interrupts                                     */
          switch(enc[0]) {
            case 0:
              attachInterrupt(getIntNum(Enc[0].pinA), irsPinAEn0, CHANGE);
              attachInterrupt(getIntNum(Enc[0].pinB), isrPinBEn0, CHANGE);
              break;  
            case 1:
              attachInterrupt(getIntNum(Enc[1].pinAm), irsPinAmEn1, CHANGE);
              attachInterrupt(getIntNum(Enc[1].pinBm), isrPinBmEn1, CHANGE);
              break;  
            case 2:
              attachInterrupt(getIntNum(Enc[2].pinA), irsPinAEn2, CHANGE);
              attachInterrupt(getIntNum(Enc[2].pinB), isrPinBEn2, CHANGE);
              break;  
            }

          # endif

      /* initialization done */ 
      xD[0]=1;
  }

if (xD[1]!=1) {

      /* don't do anything for MEX-file generation */
      # ifndef MATLAB_MEX_FILE

          /* enc[0] is the encoder number and it can be 0,1 or 2   */
          /* if other encoder blocks are present in the model      */
          /* up to a maximum of 3, they need to refer to a         */
          /* different encoder number                              */

          /* store pinA and pinB in global encoder structure Enc   */
          /* they will be needed later by the interrupt routine    */
          /* that will not be able to access s-function parameters */   

          Enc[encm[1]].pinAm=pinAm[1];      /* set pin A              */
          Enc[encm[1]].pinBm=pinBm[1];      /* set pin B              */

          /* set encoder pins as inputs                            */
          pinMode(Enc[encm[1]].pinAm, INPUT); 
          pinMode(Enc[encm[1]].pinBm, INPUT); 

          /* turn on pullup resistors                              */
          digitalWrite(Enc[encm[1]].pinAm, HIGH); 
          digitalWrite(Enc[encm[1]].pinBm, HIGH); 

          /* attach interrupts                                     */
          switch(encm[1]) {
            case 0:
              attachInterrupt(getIntNum(Enc[0].pinA), irsPinAEn0, CHANGE);
              attachInterrupt(getIntNum(Enc[0].pinB), isrPinBEn0, CHANGE);
              break;  
            case 1:
              attachInterrupt(getIntNum(Enc[1].pinAm), irsPinAmEn1, CHANGE);
              attachInterrupt(getIntNum(Enc[1].pinBm), isrPinBmEn1, CHANGE);
              break;  
            case 2:
              attachInterrupt(getIntNum(Enc[2].pinA), irsPinAEn2, CHANGE);
              attachInterrupt(getIntNum(Enc[2].pinB), isrPinBEn2, CHANGE);
              break;  
            }

          # endif

      /* initialization done */ 
      xD[1]=1;
  }