#include "qt_pi.h"

volatile long left_enc_pos = 0L;
volatile long right_enc_pos = 0L;
static const int8_t ENC_STATES [] = {0,1,-1,0,-1,0,0,1,1,0,0,-1,0,-1,1,0};
  
ISR (PCINT1_vect){
  
  static uint8_t enc_last=0;
      
  enc_last <<= 2;
  enc_last |= (PINC & (3 << 2)) >> 2;

  left_enc_pos += ENC_STATES[(enc_last & 0x0f)];

}

ISR (PCINT2_vect){
      
  static uint8_t enc_last=0;
          
  enc_last <<=2;
  enc_last |= (PIND & (3 << 2)) >> 2;

  right_enc_pos += ENC_STATES[(enc_last & 0x0f)];

}

void init_enc(){
  
  DDRC &= ~(1<<LEFT_ENC_PIN_A);
  DDRC &= ~(1<<LEFT_ENC_PIN_B);
  DDRD &= ~(1<<RIGHT_ENC_PIN_A);
  DDRD &= ~(1<<RIGHT_ENC_PIN_B);

  //enable pull up resistors
  PORTC |= (1<<LEFT_ENC_PIN_A);
  PORTC |= (1<<LEFT_ENC_PIN_B);
  PORTD |= (1<<RIGHT_ENC_PIN_A);
  PORTD |= (1<<RIGHT_ENC_PIN_B);

  // tell pin change mask to listen to left encoder pins
  PCMSK1 |= (1 << LEFT_ENC_PIN_A)|(1 << LEFT_ENC_PIN_B);
  // tell pin change mask to listen to right encoder pins
  PCMSK2 |= (1 << RIGHT_ENC_PIN_A)|(1 << RIGHT_ENC_PIN_B);

  // enable PCINT1 and PCINT2 interrupt in the general interrupt mask
  PCICR |= (1 << PCIE1) | (1 << PCIE2);

}

long readEncoder(int i) {
  
  if (i == LEFT_WHEEL) 
      return left_enc_pos;
    
    else 
      return right_enc_pos;
  
  }

void resetEncoders() {
  
  left_enc_pos=0L;
  right_enc_pos=0L;
  resetPID();
}

uint8_t bufodom[8 + com_preamble_len];

void sendOdom(){

  bufodom[COMB] = ODOM_DATA;
  bufodom[CIDB] = 0xFF;
  
  memcpy(bufodom + DATASTRB, &left_enc_pos, 4);
  memcpy(bufodom + DATASTRB + 4, &right_enc_pos, 4);

  sendData(8 + com_preamble_len, bufodom);

}
