#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 "emqx-client/mqttmsg.h"
#include "emqx-client/paho_client.h"
#include <unistd.h>
#include <chrono>
#include <random>
#include "timedlockdata.hpp"
// All the webots classes are defined in the "webots" namespace
using namespace webots;

Paho_client* client_= nullptr;
TimedLockData<NavMessage::ToAgvCmd> g_speed;

void SpeedChangeCallback(std::string topic,int QOS,MqttMsg& msg,void* context)
{
  NavMessage::ToAgvCmd speed;
  msg.toProtoMessage(speed);
  g_speed.reset(speed,0.3);
}

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);
}

void Vrtical(double velocity,double wmg,double w,double l,double& R1,double& R2,double& R3,double& R4,
             double& L1,double& L2,double& L3,double& L4) {
  if (fabs(wmg) < 0.0001) {
    R1 = R2 = R3 = R4 = 0;
    L1 = L2 = L3 = L4 = velocity;
    printf("virtical111 R1-R4:%.5f %.5f %.5f %.5f  L1-L4:%.5f %.5f %.5f %.5f \n",
           R1,R2,R3,R4,L1,L2,L3,L4);
  } else {//有角速度
    double base = pow(velocity / wmg, 2) - (l * l / 4);
    //满足运动方程
    if (base >= 0.0001) {
      double BaseR = sqrt(base) - w / 2;

      if (BaseR < 0.05)
        BaseR = 0.05;
      //printf("BaseR : %f \n",BaseR);
      if (BaseR > 0) {
        if (wmg * velocity >= 0) {
          R1 = atan(l / BaseR);
          R2 = atan(l / (BaseR + w));
          L3 = BaseR * wmg;
          L4 = (BaseR + w) * wmg;
          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) * wmg;
          L4 = -BaseR * wmg;
          R3 = 0;
          R4 = 0;
          L1 = L3 / cos(R1);
          L2 = L4 / cos(R2);
        }
        printf("virtical222 R1-R4:%.5f %.5f %.5f %.5f  L1-L4:%.5f %.5f %.5f %.5f \n",
           R1,R2,R3,R4,L1,L2,L3,L4);

      }
    }
  }
}

void Horizontal(double velocity,double wmg,double w,double l,double& R1,double& R2,double& R3,double& R4,
                double& L1,double& L2,double& L3,double& L4)
{
  Vrtical(velocity,wmg,w,l,R3,R1,R4,R2,L3,L1,L4,L2);
  if (fabs(wmg) < 0.0001) {
    R1 = -M_PI / 2.0;
    R2 = M_PI / 2.0;
    R3 = M_PI / 2.0;
    R4 = -M_PI / 2.0;
    L1 = -velocity;
    L2 = velocity;
    L3 = velocity;
    L4 = -velocity;
  }else{
    R1-=M_PI/2.0;
    R2+=M_PI/2.0;
    R3+=M_PI/2.0;
    R4-=M_PI/2.0;
    L1=-L1;
    L4=-L4;
  }

}





int main(int argc, char **argv) {
  //init mqtt
  if(false==init_mqtt("127.0.0.1",1883,"webots-child","ChildPLC/speedcmd"))
  {
    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.
  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);

  /*RM1->setTorque(100);
  RM2->setTorque(100);
  RM3->setTorque(100);
  RM4->setTorque(100);*/

  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.007);
    double y=gps->getValues()[0] + generae_random(generator,0,0.007);
    double theta=imu->getRollPitchYaw()[2]+generae_random(generator,0,0.1*M_PI/180.0);

    //获取gps速度，判断朝向
    double vm1=R3_l->getVelocity()*radius;
    double vm2=-R2_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;
      NavMessage::LidarOdomStatu odom;
      odom.set_x(x);
      odom.set_y(y);
      odom.set_theta(theta);
      odom.set_v(v);
      odom.set_vth(vth);
      msg.fromProtoMessage(odom);
      client_->publish("lidar_odom/child",1,msg);

      NavMessage::AgvStatu speed;
      speed.set_v(v);
      speed.set_w(vth);
      speed.set_clamp(0);
      speed.set_clamp_other(0);
      msg.fromProtoMessage(speed);
      client_->publish("ChildPLC/speed",1,msg);
    }

    if(g_speed.timeout())
    {
      RM1->setPosition(0);
      RM2->setPosition(0);
      RM3->setPosition(0);
      RM4->setPosition(0);

      R1_l->setVelocity(0);
      R2_l->setVelocity(0);
      R3_l->setVelocity(0);
      R4_l->setVelocity(0);
      continue;
    }

    //变速----------------------------------
    double cmd_v=g_speed.Get().v();
    double cmd_vth=g_speed.Get().w();

    //初始值
    double R1=0,R2=0,R3=0,R4=0;
    double L1=cmd_v;
    double L2=cmd_v;
    double L3=cmd_v;
    double L4=cmd_v;

   if(g_speed.Get().t()==1) //原地旋转
    {
      printf("旋转:");
      if (fabs(cmd_vth) > 0.0001)
        Rotating(cmd_vth, w / 2.0, l / 2.0, R1, R2, R3, R4, L1, L2, L3, L4);
    }
    else if(g_speed.Get().t()==2)    //横移
    {
      printf("横移:");
      if (fabs(cmd_vth) > 0.4) {
        cmd_vth = cmd_vth > 0 ? 0.4 : -0.4;
      }
      Horizontal(cmd_v,cmd_vth,l,w,R1, R2, R3, R4, L1, L2, L3, L4);
    }
    else if(g_speed.Get().t()==3){    //巡线/前进
      printf("前进:");
      if (fabs(cmd_vth) > 0.4) {
        cmd_vth = cmd_vth > 0 ? 0.4 : -0.4;
      }

      Vrtical(cmd_v,cmd_vth,w,l,R1,R2,R3,R4,L1,L2,L3,L4);
    }


    printf("Child 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,cmd_v,cmd_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;
  return 0;
}