/************
* I/O Ports *
************/
#define ENCP1_DDR    DDRC       // From Rotary Encoder, Phase 1
#define ENCP1_POUT   PORTC
#define ENCP1_PINP   PINC
#define ENCP1_PNUM   PC0
#define ENCP2_DDR    DDRC       // From Rotary Encoder, Phase 2
#define ENCP2_POUT   PORTC
#define ENCP2_PINP   PINC
#define ENCP2_PNUM   PC1

/*********
* Macros *
*********/
#define SetPinInp(pinname)  ((pinname ## _DDR)   &= ~(_BV(pinname  ## _PNUM)))
#define CheckHI(pinname)    ((pinname ## _PINP)  & (_BV(pinname  ## _PNUM)))

/*************************
* Acceleration Constants *
*************************/
#define ENCODER_ACCEL_TOP    1536
#define ENCODER_ACCEL_INC    96
#define ENCODER_ACCEL_DEC    1

/*****************************
* Rotary Encoder State Table *
*****************************/
uint8_t EncStates[16] PROGMEM = {0,2,1,3,1,0,3,2,2,3,0,1,3,1,2,0};


/*******************
* Global Variables *
*******************/
volatile int16_t   EncSteps;
static uint16_t    EncAccel;
static uint8_t     LastEnc, NextEnc;
uint8_t            RotaryCount;

/*************
* Initialize *
*************/
void Initialize()
{
  SetPinInp(ENCP1);  // Pin Encoder Phase 1 = Input
  SetPinInp(ENCP2);  // Pin Encoder Phase 2 = Input

  LastEnc = 0;
  if (CheckHI(ENCP1))
    LastEnc |= 2;
  if (CheckHI(ENCP2))
    LastEnc |= 1;                  // Initial Encoder State
  
  RotaryCount = 0;

  TCCR0A = _BV(WGM01);             // T/C-0 Mode = CTC, TOV Set on MAX
  TCCR0B = _BV(CS02) | _BV(CS00);  // T/C-0 Clock = Prescaler/1024
  OCR0A = 3;                       // T/C-0 Output Compare Register = 3
  TIMSK0 |= _BV(OCIE0A);           // T/C-0 Compare Match A Int every 4096 Clocks (all 256 us)
}


/**************************
* Accumulate Rotary Count *
**************************/
void AccumulateRotaryCount()
{
  if (EncSteps)
  {
    cli();
    RotaryCount += EncSteps;
    EncSteps = 0;
    sei();
  }
}  


/****************************
* Timer-A-Interrupt-Handler *
****************************/
ISR(TIMER0_COMPA_vect)
{
  uint8_t  CurrEnc;

  CurrEnc = 0;
  if (CheckHI(ENCP1))
    CurrEnc |= 2;
  if (CheckHI(ENCP2))
    CurrEnc |= 1;
  EncAccel -= ENCODER_ACCEL_DEC;
  if (EncAccel & 0b1000000000000000)
    EncAccel = 0; // Zero if overflowed
  LastEnc <<= 2;
  LastEnc |= CurrEnc;
  LastEnc &= 0b00001111;
  switch (pgm_read_word(&EncStates[LastEnc]))
  {
    case 0: break;                            // No Step
    case 1: EncSteps -= 1+(EncAccel >> 8);    // Reverse
            if (EncAccel <= (ENCODER_ACCEL_TOP-ENCODER_ACCEL_INC))
              EncAccel += ENCODER_ACCEL_INC;
            break;
    case 2: EncSteps += 1+(EncAccel >> 8);    // Forward
            if (EncAccel <= (ENCODER_ACCEL_TOP-ENCODER_ACCEL_INC))
              EncAccel += ENCODER_ACCEL_INC;
            break;
    case 3: LastEnc = CurrEnc;                // Bad Step
            EncSteps = 0;
            EncAccel = 0;
  }
}