#include <webots/Robot.hpp>
#include <webots/Motor.hpp>
#include <webots/PositionSensor.hpp>
#include <webots/GPS.hpp>
#include <webots/Gyro.hpp>
#include <webots/InertialUnit.hpp>
#include <webots/Supervisor.hpp>
#include "emqx-client/mqttmsg.h"
#include "emqx-client/paho_client.h"
#include <unistd.h>
#include <chrono>
#include <random>
// All the webots classes are defined in the "webots" namespace
using namespace webots;

Paho_client* client_= nullptr;
double g_v=0,g_vth=0;


void SpeedChangeCallback(std::string topic,int QOS,MqttMsg& msg,void* context)
{
  msg.toSpeed(g_v,g_vth);
}

double generae_random(std::default_random_engine& generator,double mean,double sigma)
{
  std::normal_distribution<double> distribution(mean, sigma);
  return distribution(generator);
}

bool init_mqtt(std::string ip,int port,std::string nodeId,std::string subtopic)
{
  if(client_!= nullptr)
  {
    client_->disconnect();
    delete client_;
  }
  client_=new Paho_client(nodeId);
  bool ret= client_->connect(ip,port);
  if(ret)
  {
    while(!client_->isconnected()) usleep(1000);
    client_->subcribe(subtopic,1,SpeedChangeCallback, nullptr);
  }
  return ret;
}

void Rotating(double wmg,double w,double l,
        double& R1,double& R2,double& R3,double& R4,
              double& L1,double& L2,double& L3,double& L4)
{
  double theta=atan(l/w);
  R1=(-theta);
  R2=(theta);
  R3=(theta);
  R4=(-theta);

  double velocity=wmg*sqrt(w*w+l*l);
  L1=(-velocity);
  L2=(velocity);
  L3=(-velocity);
  L4=(velocity);
}


int main(int argc, char **argv) {
  //init mqtt
  if(false==init_mqtt("127.0.0.1",1883,"webots-AGV","AgvCmd/cmd1"))
  {
    printf(" Init mqtt failed...\n");
    return -1;
  }
  //随机数

  unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
  std::default_random_engine generator(seed);

  // create the Robot instance.
  webots::Supervisor* super=new webots::Supervisor();
  Node* root=super->getRoot();
  Node* worldInfo=root->getField("children")->getMFNode(0);


  Robot *robot = new Robot();

  // get the time step of the current world.
  int timeStep = (int)robot->getBasicTimeStep();

  webots::Motor* RM1=robot->getMotor("R1_motor");
  webots::Motor* RM2=robot->getMotor("R2_motor");
  webots::Motor* RM3=robot->getMotor("R3_motor");
  webots::Motor* RM4=robot->getMotor("R4_motor");

  //R1->getPositionSensor()->enable(timeStep);

  webots::Motor* R1_l=robot->getMotor("L1_motor");
  webots::Motor* R2_l=robot->getMotor("L2_motor");
  webots::Motor* R3_l=robot->getMotor("L3_motor");
  webots::Motor* R4_l=robot->getMotor("L4_motor");


  RM1->setPosition(0);
  RM2->setPosition(0);
  RM3->setPosition(0);
  RM4->setPosition(0);

  R1_l->setPosition(INFINITY);
  R2_l->setPosition(INFINITY);
  R3_l->setPosition(INFINITY);
  R4_l->setPosition(INFINITY);

  R1_l->setVelocity(0.0);
  R2_l->setVelocity(0.0);
  R3_l->setVelocity(0.0);
  R4_l->setVelocity(0.0);

  webots::GPS* gps=robot->getGPS("gps");
  webots::InertialUnit* imu=robot->getInertialUnit("imu");
  webots::Gyro* gyr=robot->getGyro("gyro");
  gyr->enable(timeStep);
  gps->enable(timeStep);
  imu->enable(timeStep);

  double w=2.5;
  double l=1.3;
  double radius=0.1915;
  double K=1.0/radius;

  auto last=std::chrono::steady_clock::now();
  while (robot->step(timeStep) != -1) {

    auto now=std::chrono::steady_clock::now();
    auto duration = std::chrono::duration_cast<std::chrono::microseconds>(now - last);
    double time = double(duration.count()) * std::chrono::microseconds::period::num /
            std::chrono::microseconds::period::den;

    ////发布：
    //增加高斯分布
    double x=gps->getValues()[2] + generae_random(generator,0,0.001);
    double y=gps->getValues()[0] + generae_random(generator,0,0.001);
    double theta=imu->getRollPitchYaw()[2]+generae_random(generator,0,0.3*M_PI/180.0);

    //获取gps速度，判断朝向
    double vm1=R3_l->getVelocity()*radius;
    double vm2=-R4_l->getVelocity()*radius;
    double v=gps->getSpeed();
    if(vm1+vm2<0.0)
      v=-v;
    //陀螺仪角速度
    double vth=gyr->getValues()[1];

    if(time>0.2)
    {
      printf(" publish delay................  time:%f -------------------------------------\n",time);
    }
    /*printf("dt:%f x y:%f  %f,  yaw:%f   velocity:%f vth:%f  %f  %f\n",time,
            x,y,theta,
           v,gyr->getValues()[0],gyr->getValues()[1],gyr->getValues()[2]);*/
    last=now;

    if(client_)
    {
      MqttMsg msg;
      msg.fromStatu(x,y,theta,
                    v,vth);
      client_->publish("AgvStatu/robot1",1,msg);
    }

    //变速，角速度封顶
    if(fabs(g_vth)>0.4)
    {
      g_vth=g_vth>0?0.4:-0.4;
    }
    //初始值
    double R1=0,R2=0,R3=0,R4=0;
    double L1=g_v;
    double L2=g_v;
    double L3=g_v;
    double L4=g_v;
    double theta1=0;
    double theta2=0;

    //无角速度，直线
    if(fabs(g_vth)<0.0001)
    {
      R1=R2=R3=R4=0;
      L1=L2=L3=L4=g_v;
    }
    else  //有角速度
    {
      if (fabs(g_v) < 0.00001)
      {
        //原地旋转
        if (fabs(g_vth) > 0.0001)
          Rotating(g_vth, w / 2.0, l / 2.0, R1, R2, R3, R4, L1, L2, L3, L4);
      }
      else
      {
        //默认原地旋转
        Rotating(g_vth, w / 2.0, l / 2.0, R1, R2, R3, R4, L1, L2, L3, L4);
        double base = pow(g_v / g_vth,2)-(l*l/4);
        //满足运动方程
        if(base>=0.1)
        {
          double BaseR=sqrt(base)-w/2;
          if(BaseR>0)
          {
            if(g_vth*g_v>=0)
            {
              R1=atan(l/BaseR);
              R2=atan(l/(BaseR+w));
              L3=BaseR*g_vth;
              L4=(BaseR+w)*g_vth;
              R3=0;
              R4=0;
              L1=L3/cos(R1);
              L2=L4/cos(R2);

            }
            else
            {
              R1=-atan(l/(BaseR+w));
              R2=-atan(l/BaseR);
              L3=-(BaseR+w)*g_vth;
              L4=-BaseR*g_vth;
              R3=0;
              R4=0;
              L1=L3/cos(R1);
              L2=L4/cos(R2);
            }

          }
        }

      }
    }

    //printf(" R1-R4:%.5f %.5f %.5f %.5f  L1-L4:%.5f %.5f %.5f %.5f  V:%.5f  Vth:%.5f\n",
     //       R1,R2,R3,R4,L1,L2,L3,L4,v,vth);
    RM1->setPosition(R1);
    RM2->setPosition(R2);
    RM3->setPosition(R3);
    RM4->setPosition(R4);

    R1_l->setVelocity(L1*K);
    R2_l->setVelocity(-L2*K);
    R3_l->setVelocity(L3*K);
    R4_l->setVelocity(-L4*K);

  };

  // Enter here exit cleanup code.

  delete robot;
  delete super;
  return 0;
}