/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% LEGS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */

/* Used to control the bottom engines */

#include "Arduino.h"
#include <ros.h>
#include <std_msgs/UInt16.h>
#include <std_msgs/Empty.h>
#include <std_msgs/String.h>
#include <std_msgs/MultiArrayLayout.h>
#include <std_msgs/MultiArrayDimension.h>
#include <std_msgs/Int8MultiArray.h>

#include <Wire.h>
#include <MD23.h>

MD23 md23;

void md23_cb( const std_msgs::Empty& cmd_msg){
  //md23.write(cmd_msg.data); //set servo angle, should be from 0-180
  //digitalWrite(13, HIGH-digitalRead(13));  //toggle led
  forward();
  fstop();
  delay(1000);
  backward();
  bstop();
  delay(1000);
}

ros::Subscriber<std_msgs::Empty> motorSub("MotorControl", md23_cb);


/*%%%%%%%%%%%%%%% SETUP AND LOOP %%%%%%%%%%%%%%%%%%%%%%%%%*/

ros::NodeHandle nh;

int incomingByte = 0;   // for incoming serial data

void setup(){

  Serial.begin(57600);

  pinMode(13, OUTPUT);
  nh.initNode();
  //Setup for legs!
  Wire.begin(); // you have to init the i2c bus by yourself
  nh.subscribe(motorSub);
  delay(1000);

}

void loop(){
  nh.spinOnce();
  delay(300);
  forward();


}

int i = 129;

void forward()
{
  //while (1) {
    for (i; i<150; i++)
    {
      md23.setMotor1Speed(i); // slow forwad, first motor
      md23.setMotor2Speed(i);
      //drive(i);
      delay(100);
    }

    /*
    for (i; i>128; i--)
    {
      md23.setMotor1Speed(i); // slow forwad, first motor
      md23.setMotor2Speed(i);
      //drive(i);
      delay(100);
    }
    */

    while (1){
      md23.setMotor1Speed(150); // slow forwad, first motor
      md23.setMotor2Speed(150);
      delay(100);
    }

  //}


}

void backward()
{
   for (i; i>78; i--)
  {
    md23.setMotor1Speed(i); // slow forwad, first motor
    md23.setMotor2Speed(i);
    delay(100);
  }

}

void fstop()
{
  i = md23.getMotor1Speed();


  for(i; i>129; i--)
  {
    md23.setMotor1Speed(i); // slow forwad, first motor
    md23.setMotor2Speed(i);
    delay(100);
  }

  md23.setMotor1Speed(128); // stop the first motor
  md23.setMotor2Speed(128); // stop the second motor

}

void bstop()
{
  i = md23.getMotor1Speed();


  for(i; i<129; i++)
  {
    md23.setMotor1Speed(i); // slow forwad, first motor
    md23.setMotor2Speed(i);
    delay(100);
  }

  md23.setMotor1Speed(128); // stop the first motor
  md23.setMotor2Speed(128); // stop the second motor

}