// ----------------------------
// Group number:      6
// Student 1:
// Mattia Strocchi    4802454
// Student 2:
// Pietro Rustici     4888014
// ----------------------------
#include <ros.h>
#include <math.h>
#include <geometry_msgs/Twist.h>

// left motor control (reverse, enable, forward)
#define MOTOR_L_REV 7
#define MOTOR_L_EN 24
#define MOTOR_L_FWD 6

// right motor control (reverse, enable, forward)
#define MOTOR_R_REV 3
#define MOTOR_R_EN 25
#define MOTOR_R_FWD 2

// HC-05
// Arduino Serial1 is already defined on pin 18/19
// The settings of the bluetooth are 57600 baud, 8n1. The pin code for the Bluetooth is 1234 or 0000.

// HC-SR04 (distance sensor)
#define TRIG 23
#define ECHO 22
#define MIN_DISTANCE 10
#define MAX_DISTANCE 15

// LED
#define LED 13

// PWM settings
#define PWM_MIN 60
#define PWM_MAX 90
#define STEERING_FACTOR 35
// 2 seconds delta interval
#define DELTA_INTERVAL 2000


// ROS SUBSCRIBER AND CB
float leftMotor = 0;
float rightMotor = 0;
float distanceF = 0;
// time
double last_callback = 0;

void onNewMessage( const geometry_msgs::Twist& msg){
  float linear = msg.linear.x;
  float angular = msg.angular.z;
  if(msg.linear.x == 0) {
    leftMotor = 0;
    rightMotor = 0;
  } else {
    leftMotor = map(linear, 0, 1, PWM_MIN, PWM_MAX);
    rightMotor = map(linear, 0, 1, PWM_MIN, PWM_MAX);
  }

  if(angular != 0){
    leftMotor += STEERING_FACTOR * angular;
    rightMotor -= STEERING_FACTOR * angular;
  }
  // reset callback timer
  last_callback = millis();
}
/*
class NewHardware : public ArduinoHardware {
  public: NewHardware():ArduinoHardware(&Serial1, 57600)
    {};
}; ros::NodeHandle_<NewHardware> node;
*/

ros::NodeHandle node;

geometry_msgs::Twist message;
ros::Subscriber<geometry_msgs::Twist> sub("/cmd_vel", &onNewMessage);
ros::Publisher pub("/sensor_data", &message);

void setup() {
  initPins();
  Serial1.begin(57600);
  node.initNode();
  node.advertise(pub);
  node.subscribe(sub);
  // delay ease ros connection process
  delay(1000);
  initInterrupts();
}

void loop() {
  // check time elapsed tilll the last callback
  if((millis() - last_callback) > DELTA_INTERVAL) {
    leftMotor = 0;
    rightMotor = 0;
  }
  message.linear.x = leftMotor;
  message.linear.y = rightMotor;
  message.linear.z = distanceF;
  pub.publish(&message);
  node.spinOnce();
  delay(5);
}


void initPins() {

  // init distance sensor
  pinMode(TRIG, OUTPUT);
  pinMode(ECHO, INPUT);

  // init all MOTOR pins
  pinMode(MOTOR_L_EN, OUTPUT);
  pinMode(MOTOR_L_REV, OUTPUT);
  pinMode(MOTOR_L_FWD, OUTPUT);

  pinMode(MOTOR_R_EN, OUTPUT);
  pinMode(MOTOR_R_REV, OUTPUT);
  pinMode(MOTOR_R_FWD, OUTPUT);

   // enable motors
  digitalWrite(MOTOR_L_EN, HIGH);
  digitalWrite(MOTOR_R_EN, HIGH);

  // init led pin
  pinMode(LED, OUTPUT);
}

float getDistanceFactor() {
  // Clears the trigPin
  digitalWrite(TRIG, LOW);
  delayMicroseconds(2);
  // Sets the trigPin on HIGH state for 10 micro seconds
  digitalWrite(TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG, LOW);
  // Reads the echoPin, returns the sound wave travel time in microseconds
  long duration = pulseIn(ECHO, HIGH);
  // Calculating the distance
  float distance = duration*0.034/2;

  if(distance < MIN_DISTANCE) distance = 0;
  else if (distance > MAX_DISTANCE) distance = 1;
  else {
    distance = (distance - MIN_DISTANCE) / (MAX_DISTANCE - MIN_DISTANCE);
  }
  return distance;
}

void initInterrupts() {
  noInterrupts();
  TCCR1A = 0;
  TCCR1B = 0;
  TCNT1  = 0;
  OCR1A = 2500;             // 62500 / 2500 run 25 times a sec
  TCCR1B |= (1 << WGM12);   // CTC mode
  TCCR1B |= (1 << CS12);    // 256 prescaler
  TIMSK1 |= (1 << OCIE1A);  // enable timer compare interrupt
  interrupts();
}

ISR(TIMER1_COMPA_vect)          // timer compare interrupt service routine
{
  distanceF = getDistanceFactor();
  analogWrite(MOTOR_L_FWD, rightMotor * distanceF);
  analogWrite(MOTOR_R_FWD, leftMotor * distanceF);
  digitalWrite(LED, digitalRead(LED) ^ 1);
}

/*
TCNTx - Timer/Counter Register. The actual timer value is stored here.
OCRx - Output Compare Register
ICRx - Input Capture Register (only for 16bit timer)
TIMSKx - Timer/Counter Interrupt Mask Register. To enable/disable timer interrupts.
TIFRx - Timer/Counter Interrupt Flag Register. Indicates a pending timer interrupt.
Motors use timer 3 (pin 2) and timer 4 (pin 6)
*/