#include <ros.h>
//#include <mypackage/Encoder.h>
//#include <std_msgs/String.h>
#include <std_msgs/Int32.h>
#include <std_msgs/Float32.h>
#include <digitalWriteFast.h>
// Quadrature encoders
// Left encoder
#define c_LeftEncoderInterruptA 0
#define c_LeftEncoderInterruptB 1
#define c_LeftEncoderPinA 2
#define c_LeftEncoderPinB 3
#define LeftEncoderIsReversed

volatile bool _LeftEncoderASet;
volatile bool _LeftEncoderBSet;
volatile bool _LeftEncoderAPrev;
volatile bool _LeftEncoderBPrev;
volatile long _LeftEncoderTicks = 0;
volatile long _RightEncoderTicks = 0;
long _PastRightEncoderTicks = 0;
long _PastLeftEncoderTicks = 0;

//int currentLoopTime = 0;
//int pastLoopTime = -50;

ros::NodeHandle nh;

//mypackage::Encoder encoder_msg;
//std_msgs::String str_msg;
std_msgs::Float32 lwheel_rate_data;
std_msgs::Float32 rwheel_rate_data;
std_msgs::Int32 lwheel_ticks_data;
std_msgs::Int32 rwheel_ticks_data;

ros::Publisher lwheel_ticks("lwheel_ticks", &lwheel_ticks_data);
ros::Publisher rwheel_ticks("rwheel_ticks", &rwheel_ticks_data);
ros::Publisher lwheel_rate("lwheel_rate", &lwheel_rate_data);
ros::Publisher rwheel_rate("rwheel_rate", &rwheel_rate_data);

void setup()
{
  Serial.begin(9600);
  //nh.initNode()j;
  //nh.advertise(encoder);
  // Quadrature encoders
  // Left encoder
  pinMode(c_LeftEncoderPinA, INPUT);      // sets pin A as input
  digitalWrite(c_LeftEncoderPinA, LOW);  // turn on pullup resistors
  pinMode(c_LeftEncoderPinB, INPUT);      // sets pin B as input
  digitalWrite(c_LeftEncoderPinB, LOW);  // turn on pullup resistors
  attachInterrupt(c_LeftEncoderInterruptA, HandleLeftMotorInterruptA, CHANGE);
  attachInterrupt(c_LeftEncoderInterruptB, HandleLeftMotorInterruptB, CHANGE);
  nh.initNode();
  nh.advertise(lwheel_ticks);
  nh.advertise(rwheel_ticks);
  nh.advertise(lwheel_rate);
  nh.advertise(rwheel_rate);
}

void loop()
{

  lwheel_ticks_data.data = _LeftEncoderTicks;
  rwheel_ticks_data.data = _RightEncoderTicks;
  lwheel_ticks.publish( &lwheel_ticks_data );
  rwheel_ticks.publish( &rwheel_ticks_data );
  lwheel_rate.publish( &lwheel_rate_data );
  rwheel_rate.publish( &rwheel_rate_data );

  nh.spinOnce();
  delay(100);
}

// Interrupt service routines for the left motor's quadrature encoder
void HandleLeftMotorInterruptA(){
  _LeftEncoderBSet = digitalReadFast(c_LeftEncoderPinB);
  _LeftEncoderASet = digitalReadFast(c_LeftEncoderPinA);

  _LeftEncoderTicks+=ParseEncoder();

  _LeftEncoderAPrev = _LeftEncoderASet;
  _LeftEncoderBPrev = _LeftEncoderBSet;
}

// Interrupt service routines for the right motor's quadrature encoder
void HandleLeftMotorInterruptB(){
  // Test transition;
  _LeftEncoderBSet = digitalReadFast(c_LeftEncoderPinB);
  _LeftEncoderASet = digitalReadFast(c_LeftEncoderPinA);

  //_PastLeftEncoderTicks = _LeftEncoderTicks;
  _LeftEncoderTicks+=ParseEncoder();

  _LeftEncoderAPrev = _LeftEncoderASet;
  _LeftEncoderBPrev = _LeftEncoderBSet;
}

int ParseEncoder(){
  if(_LeftEncoderAPrev && _LeftEncoderBPrev){
    if(!_LeftEncoderASet && _LeftEncoderBSet) return 1;
    if(_LeftEncoderASet && !_LeftEncoderBSet) return -1;
  }else if(!_LeftEncoderAPrev && _LeftEncoderBPrev){
    if(!_LeftEncoderASet && !_LeftEncoderBSet) return 1;
    if(_LeftEncoderASet && _LeftEncoderBSet) return -1;
  }else if(!_LeftEncoderAPrev && !_LeftEncoderBPrev){
    if(_LeftEncoderASet && !_LeftEncoderBSet) return 1;
    if(!_LeftEncoderASet && _LeftEncoderBSet) return -1;
  }else if(_LeftEncoderAPrev && !_LeftEncoderBPrev){
    if(_LeftEncoderASet && _LeftEncoderBSet) return 1;
    if(!_LeftEncoderASet && !_LeftEncoderBSet) return -1;
  }
}