/*
 * rosserial Publisher Example
 * Prints "hello world!"
 */
#include "mbed.h"
#include <ros.h>
#include <string>
#include <std_msgs/String.h>


#include <sstream>



ros::NodeHandle  nh;

std_msgs::String str_msg;

ros::Publisher chatter("chatter", &str_msg);

char hello[100] = "hello, shit world!";

DigitalOut led = LED1;

int counter = 0;

int main()
{

    nh.initNode();
    nh.advertise(chatter);

    while (1) {
        std::stringstream ss;
        //counter++;
        led = !led;
        ss << "pos:x=" << counter <<",y="<< counter++ <<",r=" << counter++;
        //counter++;
        //ss << "y=" << counter;
        //counter++;
        //ss << "r=" << counter;
        const std::string tmp = ss.str();
        const char* cstr = tmp.c_str();

        str_msg.data=cstr;

        //str_msg.data=http://www.mamicode.com/hello;
        chatter.publish( &str_msg );
        nh.spinOnce();
        wait_ms(1000);
    }
}

#include "ros/ros.h"
#include <string>
#include "std_msgs/String.h"
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <vector>
#include <stdlib.h>
#include <sstream>


ros::Time current_time, last_time;
ros::Publisher odom_pub;

void split(const std::string& src, const std::string& separator, std::vector<std::string>& dest)
{
    std::string str = src;
    std::string substring;
    std::string::size_type start = 0, index;

    do
    {
        index = str.find_first_of(separator,start);
        if (index != std::string::npos)
        {    
            substring = str.substr(start,index-start);
            dest.push_back(substring);
            start = str.find_first_not_of(separator,index);
            if (start == std::string::npos) return;
        }
    }while(index != std::string::npos);
    
    //the last token
    substring = str.substr(start);
    dest.push_back(substring);
}

void processTest(const std_msgs::String::ConstPtr& msg){
    //char src[]  = "Accsvr:tcp     -h :  127.0.0.1 -p \t 20018   ";
    const char* base_message =  msg->data.c_str();
    printf("Origin str: %s\n\r",base_message);    
    
    std::string originStr(base_message);
    
    const int len = originStr.length();
    char* originChar= new char[len+1];
    strcpy(originChar,originStr.c_str());
    const char *d = ",";
    char *p;
    p = strtok(originChar,d);
    char * charX = p + 6;

    // = sprintf(charX, "%05X", 30); 
    int intX;
    std::stringstream ss;
    ss << charX;
    ss >> intX;
    printf("x: %d\n",intX);
    p = strtok(NULL,d);
    
    int intY;
    char * charY = p + 2;
    ss.clear();
    ss << charY;
    ss >> intY;
    printf("y: %d\n",intY);
    p = strtok(NULL,d);
        
    int intYaw;
    char * charYaw = p + 2;
    ss.clear();
    ss << charYaw;
    ss >> intYaw;
    printf("intYaw: %d\n",intYaw);
    //p = strtok(NULL,d);
    
    //int intX = std::atio(std::string(charX));
    //while(p)
    //{}
    
    //printf("p: %s\n",p);
    //printf("x: %s\n",charX);
    //printf("x: %d\n",intX);
    //p=strtok(NULL,d);


}

void processStr(const std_msgs::String::ConstPtr& msg)
{
    int splitter =‘x‘;
    char *pdest;
    const char* base_message =  msg->data.c_str();
    printf("Origin str: %s\n\r",base_message);    
    
    std::string originStr(base_message);
    
    char* originChar;
    char* charx;
    char* chary;
    char* charr;
    const int len = originStr.length();
    originChar = new char[len+1];
    strcpy(originChar,originStr.c_str());
    int result;
    pdest = strchr(originChar, splitter);
    printf("pdest is %s\n",pdest);
    result = pdest - originChar + 1;
    if( pdest != NULL ){
         printf("Result:\tfirst %c found at position %d\n",  splitter, result );
         
    }
    //printf("pdest is %s\n",pdest);
    
}

geometry_msgs::TransformStamped odom_trans;



void chatterCallback(const std_msgs::String::ConstPtr& msg)
{
    //printf("Origin str: %s\n\r",base_message);
    std::string base_message = msg->data.c_str();
    //printf("Origin str: %s\n\r",base_message);
    //processStr(msg);
    //const char* base_message =  msg->data.c_str();
    processTest(msg);
    //ROS_INFO("I heard: [%s]", base_message);
    ros::NodeHandle n;
    current_time = ros::Time::now();

    nav_msgs::Odometry odom;
    
    double x = 0.0;
    double y = 0.0;
    double th = 0.0;

    double vx = 0.1;
    double vy = -0.1;
    double vth = 0.1;

    double dt = (current_time - last_time).toSec();
    double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
    double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
    double delta_th = vth * dt;  

    x += delta_x;
    y += delta_y;
    th += delta_th;
 
    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
    //first, we‘ll publish the transform over tf

    odom_trans.header.stamp = current_time;
    odom_trans.header.frame_id = "odom";
    odom_trans.child_frame_id = "base_link";

    odom_trans.transform.translation.x = x;
    odom_trans.transform.translation.y = y;
    odom_trans.transform.translation.z = 0.0;
    odom_trans.transform.rotation = odom_quat;

    //send the transform

    //next, we‘ll publish the odometry message over ROS

    odom.header.stamp = current_time;
    odom.header.frame_id = "odom";

    //set the position
    odom.pose.pose.position.x = x;
    odom.pose.pose.position.y = y;
    odom.pose.pose.position.z = 0.0;
    odom.pose.pose.orientation = odom_quat;

    //set the velocity
    odom.child_frame_id = "base_link";
    odom.twist.twist.linear.x = vx;
    odom.twist.twist.linear.y = vy;
    odom.twist.twist.angular.z = vth;

     //publish the message
    odom_pub.publish(odom);
     
    last_time = current_time;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "listener");

  ros::NodeHandle n;

  ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);
 
  odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
  
  tf::TransformBroadcaster odom_broadcaster;
  ros::Rate r(100.0);
  //ros::spin();
  while(n.ok()){
     odom_broadcaster.sendTransform(odom_trans);
     ros::spinOnce();
     r.sleep(); 
  }

  return 0;
}