#include <Arduino.h>
#include <AccelStepper.h>
// #include <FlexyStepper.h>

#define db double
#define pi acos(-1.0)

const int X_STEP_PIN = PB7;    
const int X_DIR_PIN = PB6;
const int Y_STEP_PIN = PA8;
const int Y_DIR_PIN = PB5;

AccelStepper stepper(AccelStepper::DRIVER,PB7,PB6);
AccelStepper stepper2(AccelStepper::DRIVER,PA8,PB5);

FlexyStepper stepperX, stepperY;

int PULSE_PER_REV = 400;       // 电机步数/转
const db LEAD_SCREW_PITCH = 5.0;  // 丝杠导程(mm/rev)
int PULSE_PER_MM = PULSE_PER_REV / LEAD_SCREW_PITCH; // 80 pulse/mm
const int PULSE_WIDTH_US = 20;       // 脉冲宽度(μs) 注：至少20μs保证驱动器识别
const int DIR_DELAY_US = 10;         // 方向信号稳定时间(μs)

db cur_x=0,cur_y=0;
bool flag=0;

db x[1000],y[1000],tot=0;

db mmToSteps(db mm) {
  return mm * (PULSE_PER_REV / LEAD_SCREW_PITCH);
}

db mmPerSecToStepsPerSec(db mm_s) {
  return mm_s * (PULSE_PER_REV / LEAD_SCREW_PITCH);
}

db stepstomm(db steps) {
  return steps/PULSE_PER_REV*LEAD_SCREW_PITCH;
}

bool emergencyStop = false; // 全局急停标志

db dist(db x,db y,db xx,db yy) {
  db dx=x-xx,dy=y-yy;
  return sqrt(dx*dx+dy*dy);
}

void jc() {
  if(Serial1.available()>=4 && Serial1.readString() == "STOP") {
    emergencyStop = 1;
  }
}

void moveMotorX_Uniform(bool direction, db distance_mm, db speed_mm_s) {
  if (distance_mm <= 0 || speed_mm_s <= 0 || emergencyStop) return;
  
  digitalWrite(X_DIR_PIN, direction ? LOW : HIGH);
  
  int total_pulses = round(distance_mm * PULSE_PER_MM);//总脉冲数
  db pulse_interval_us = 1000000.0 / (speed_mm_s * PULSE_PER_MM);//1e6/每秒脉冲数---每个脉冲数多少微秒
  db start=millis();
  for(int i = 0; i < total_pulses; i++) {//交替操作产生脉冲信号
    jc();
    if(emergencyStop) break;
    digitalWrite(X_STEP_PIN, HIGH);
    delayMicroseconds(pulse_interval_us / 2);
    digitalWrite(X_STEP_PIN, LOW);
    delayMicroseconds(pulse_interval_us / 2);
  }
  // Serial1.print(millis()-start);
  // Serial1.println("");
}

void moveMotorY_Uniform(bool direction, db distance_mm, db speed_mm_s) {
  if (distance_mm <= 0 || speed_mm_s <= 0 || emergencyStop) return;
  
  // 修正方向控制（true=正方向，false=负方向）
  digitalWrite(Y_DIR_PIN, direction ? LOW : HIGH); // 反转了LOW/HIGH
  
  int total_pulses = round(distance_mm * PULSE_PER_MM);
  db pulse_interval_us = 1000000.0 / (speed_mm_s * PULSE_PER_MM);
  db start=millis();
  for(int i = 0; i < total_pulses; i++) {
    jc();
    if(emergencyStop) break;
    digitalWrite(Y_STEP_PIN, HIGH);
    delayMicroseconds(pulse_interval_us / 2);
    digitalWrite(Y_STEP_PIN, LOW);
    delayMicroseconds(pulse_interval_us / 2);
  }
  // Serial1.print(millis()-start);
  // Serial1.println("");
}

void moveMotorX_Accel(bool direction, db distance_mm, db speed_mm_s) {
  if(direction==1) distance_mm*=-1;
  stepper.setCurrentPosition(0);
  stepper.setMaxSpeed(mmPerSecToStepsPerSec(2*speed_mm_s)); // 默认50mm/s
  stepper.setAcceleration(1000); // 加速度1000mm/s²
  stepper.moveTo(mmToSteps(distance_mm)); // 反向移动40mm
  db start=millis();
  db lst=0;
  while(stepper.distanceToGo() != 0) {
    jc();
    if(emergencyStop) {
      stepper.stop();
      break;
    }
    if(millis() - lst > 50) {
      lst=millis();
      out(cur_x+stepstomm(-stepper.currentPosition()),cur_y);
    }
    stepper.run();
  }
  // Serial1.print(millis()-start);
  // Serial1.println("");
}

void moveMotorY_Accel(bool direction, db distance_mm, db speed_mm_s) {
  if(direction==1) distance_mm*=-1;
  stepper2.setCurrentPosition(0);
  stepper2.setMaxSpeed(mmPerSecToStepsPerSec(2*speed_mm_s)); // 默认50mm/s
  stepper2.setAcceleration(1000); // 加速度1000mm/s²
  stepper2.moveTo(mmToSteps(distance_mm)); // 反向移动40mm
  db start=millis();
  db lst=0;
  while(stepper2.distanceToGo() != 0) {
    jc();
    if(emergencyStop) {
      stepper2.stop();
      break;
    }
    if(millis() - lst > 50) {
      lst=millis();
      out(cur_x,cur_y+stepstomm(-stepper2.currentPosition()));
    }
    stepper2.run();
  }
  // Serial1.print(millis()-start);
  // Serial1.println("");
}

void moveMotorX(bool direction,db distance_mm,db speed_mm_s) {
  if(distance_mm >= LEAD_SCREW_PITCH) moveMotorX_Accel(direction,distance_mm,speed_mm_s);
  else moveMotorX_Uniform(direction,distance_mm,speed_mm_s);
}

void moveMotorY(bool direction,db distance_mm,db speed_mm_s) {
  if(distance_mm >= LEAD_SCREW_PITCH) moveMotorY_Accel(direction,distance_mm,speed_mm_s);
  else moveMotorY_Uniform(direction,distance_mm,speed_mm_s);
}

void out(db x,db y) {
  static int cnt = 0;
  if(++cnt%1==0) {
    Serial1.print(x);
    Serial1.print(",");
    Serial1.print(y);
    Serial1.println("");
  }
}

db del=0.2;

namespace Line{

  struct Point {
      db x, y;
      Point(db x_ = 0, db y_ = 0) : x(x_), y(y_) {}
  };

  void lineInterpolation_DDA(Point start, Point end, db speed_mm_s) {
    if(start.x == end.x) {
      cur_x=start.x,cur_y=start.y;
      if(start.y < end.y) moveMotorY(1,end.y-start.y,speed_mm_s);
      else moveMotorY(0,start.y-end.y,speed_mm_s);
      return;
    }
    if(start.y == end.y) {
      cur_x=start.x,cur_y=start.y;
      if(start.x < end.x) moveMotorX(1,end.x-start.x,speed_mm_s);
      else moveMotorX(0,start.x-end.x,speed_mm_s);
      return;
    }
    Point current(0,0);
    cur_x=cur_y=0;
    int x_=end.x-start.x,y_=end.y-start.y;
    db F=0;
    int num=0;
    int total_steps = (fabs(end.x - start.x) + fabs(end.y - start.y))*(1.0 / del)+del/2;
    // Serial1.println(total_steps);
    int step_count = 0;
    int f=0;
    if(x_>0) {
      if(y_>0) f=1;
      else f=4;
    }
    else {
      if(y_>0) f=2;
      else f=3;
    }
    while(step_count < total_steps && !emergencyStop) {
      jc();
      if(emergencyStop) break;
      switch(f) {
        case 1:{
          if(F>=0) {
            F=F-y_*del;
            current.x+=del;
            moveMotorX(1,del,speed_mm_s);
          }
          else {
            F=F+x_*del;
            current.y+=del;
            moveMotorY(1,del,speed_mm_s);
          }
          break;
        }
        case 2:{
          if(F>=0) {
            F=F+x_*del;
            current.y+=del;
            moveMotorY(1,del,speed_mm_s);
          }
          else {
            F=F+y_*del;
            current.x-=del;
            moveMotorX(0,del,speed_mm_s);
          }
          break;
        }
        case 3:{
          if(F>=0) {
            F=F+y_*del;
            current.x-=del;
            moveMotorX(0,del,speed_mm_s);
          }
          else {
            F=F-x_*del;
            current.y-=del;
            moveMotorY(0,del,speed_mm_s);
          }
          break;
        }
        case 4:{
          if(F>=0) {
            F=F-x_*del;
            current.y-=del;
            moveMotorY(0,del,speed_mm_s);
          }
          else {
            F=F-y_*del;
            current.x+=del;
            moveMotorX(1,del,speed_mm_s);
          }
          break;
        }
      }
      step_count++;
      ++num;
      if(num%1==0) {
        out(current.x+start.x,current.y+start.y);
      }
      cur_x=current.x+start.x;
      cur_y=current.y+start.y;
    }
  }
}

namespace ArcInterpolation {
    struct Point {
        db x, y;
        Point(db x_ = 0, db y_ = 0) : x(x_), y(y_) {}
    };

    db pow(db x) {
      return x*x;
    }

    db dist(db x,db y,db xx,db yy) {
      db dx=x-xx,dy=y-yy;
      return sqrt(pow(dx)+pow(dy));
    }
    //单个象限圆弧插补
    void InterpolateArc(Point start, Point end,db speed,int dir,int qua,db ox,db oy) {
        // 计算半径（圆心为原点）
        db r1=dist(start.x,start.y,0,0);
        db r2=dist(end.x,end.y,0,0);
        if(fabs(r1-r2)>0.1) {
          Serial1.println("Two Points don't lie on the same circle");
          return;
        }

        Point current(start.x,start.y);
        
        db F = current.x * current.x + current.y * current.y - r1*r1;
        int total_steps = (fabs(end.x - start.x) + fabs(end.y - start.y))*(1.0 / del)+del/2;
        // Serial1.println(total_steps);

        int step_count = 0;
        int f=0;
        if(qua==1 && dir==0) f=1;
        if(qua==2 && dir==1) f=2;
        if(qua==3 && dir==0) f=3;
        if(qua==4 && dir==1) f=4;

        if(qua==1 && dir==1) f=5;
        if(qua==2 && dir==0) f=6;
        if(qua==3 && dir==1) f=7;
        if(qua==4 && dir==0) f=8;
        int num=0;
        while (step_count < total_steps && !emergencyStop) {
          jc();
          if(emergencyStop) break;
          if(f>=1 && f<=4) {
            if(F>=0) {
              if(f==1 || f==4) {
                F=F-2*current.x*del+pow(del),current.x-=del;
                moveMotorX(0,del,speed);
              }
              if(f==2 || f==3) {
                F=F+2*current.x*del+pow(del),current.x+=del;
                moveMotorX(1,del,speed);
              }
            }
            else if(F<0) {
              if(f==1 || f==2) {
                F=F+2*current.y*del+pow(del),current.y+=del;
                moveMotorY(1,del,speed);
              }
              if(f==3 || f==4) {
                F=F-2*current.y*del+pow(del),current.y-=del;
                moveMotorY(0,del,speed);
              }
            }
          }
          else {
            if(F>=0) {
              if(f==5 || f==6) {
                F=F-2*current.y*del+pow(del),current.y-=del;
                moveMotorY(0,del,speed);
              }
              if(f==7 || f==8) {
                F=F+2*current.y*del+pow(del),current.y+=del;
                moveMotorY(1,del,speed);
              }
            }
            else if(F<0) {
              if(f==5 || f==8) {
                F=F+2*current.x*del+pow(del),current.x+=del;
                moveMotorX(1,del,speed);
              }
              if(f==6 || f==7) {
                F=F-2*current.x*del+pow(del),current.x-=del;
                moveMotorX(0,del,speed);
              }
            }
          }
          step_count++;
          ++num;
          if(num%1==0) {
            out(current.x+ox,current.y+oy);
          }
        }
    }
}

String s;
int id=0,cnt=0;

void rdf(db &x) {
  db res=0,mul=0.1;
  int f=1;
  if(s[id]=='-') f=-1,id++;
  while(s[id]!='/' && s[id]!='E') {
    if(s[id]=='.') {
      mul=0.1;
      ++id;
      while(s[id]!='/' && s[id]!='E') {
        int c=s[id]-'0';
        res+=mul*c;
        mul*=0.1;
        ++id;
      }
    }
    else {
      int c=s[id]-'0';
      res=res*10+c;
      id++;
    }
  }
  x=res*f;
}

void rd(int &x) {
  int res=0,f=1;
  if(s[id]=='-') f=-1,id++;
  while(s[id]!='/' && s[id]!='E') {
    int c=s[id]-'0';
    res=res*10+c;
    ++id;
  }
  x=res*f;
}

int ck(db x,db y) {
  if(x>=0) {
    if(y>=0) return 1;
    else return 4;
  }
  else {
    if(y>=0) return 2;
    else return 3;
  }
}

db angle(db x1,db y1,db x2,db y2,int dir) {
  db w=atan2(y1,x1),w2=atan2(y2,x2);
  if(dir==1) {
    if(w<=w2) return w+2*pi-w2;
    else return w-w2;
  }
  else {
    if(w<=w2) return w2-w;
    else return w2+2*pi-w;
  }
}

int pd(db x,db y,db xx,db yy) {
  db midx=(x+xx)/2,midy=(y+yy)/2;
  return ck(midx,midy);
}

void setup() {
  // 初始化引脚
  pinMode(X_STEP_PIN, OUTPUT);
  pinMode(X_DIR_PIN, OUTPUT);
  pinMode(Y_STEP_PIN, OUTPUT);
  pinMode(Y_DIR_PIN, OUTPUT);
  Serial1.begin(9600);
  delay(2000);
}

void loop() {
  emergencyStop = 0;
  // Serial1.println("Ready");
  // delay(500);
  db sx,sy,tx,ty,speed,ox,oy;
  int opt,dir,mode,xifen;
  flag=0;

  if(Serial1.available() > 0 && (s = Serial1.readString()) != "STOP") {//防止连续按2次STO
    int len=s.length();
    id=0,cnt=0;
    while(++id<len) {
      ++cnt;
      if(s[id]=='/') {
        continue;
      }
      else if(s[id]=='E') break;
      if(cnt==1) {
        rd(opt);
      }
      else if(cnt==2) {
        rdf(sx);
      }
      else if(cnt==3) {
        rdf(sy);
      }
      else if(cnt==4) {
        rdf(tx);
      }
      else if(cnt==5) {
        rdf(ty);
      }
      else if(cnt==6) {
        rdf(ox);
      }
      else if(cnt==7) {
        rdf(oy);
      }
      else if(cnt==8) {
        rd(dir);
      }
      else if(cnt==9) {
        rdf(speed);
      }
      else if(cnt==10) {
        rd(xifen);
      }
      else {
        rdf(del);
      }
      PULSE_PER_REV=xifen;
      PULSE_PER_MM = PULSE_PER_REV / LEAD_SCREW_PITCH;
    }
    if(opt==1) {//1直线0圆弧
      if(!flag) out(sx,sy),flag=1;
      Line::Point start(sx,sy);
      Line::Point end(tx,ty);
      Line::lineInterpolation_DDA(start,end,speed);
    }
    else {
      if(!flag) out(sx,sy),flag=1;
      ArcInterpolation::Point start(sx,sy);
      ArcInterpolation::Point end(tx,ty);
      ArcInterpolation::Point S(sx-ox,sy-oy),T(tx-ox,ty-oy),now=S;
      db r=dist(S.x,S.y,0,0);
      db rr=dist(T.x,T.y,0,0);
      if(fabs(r-rr) > 0.1) {
        if(fabs(r-rr)>0.1) {
          Serial1.println("Two Points don't lie on the same circle");
          goto ff;
        }
      }
      int ranget=ck(T.x,T.y);
      while(1) {
        ArcInterpolation::Point tmp(0,0);
        if(now.x==T.x && now.y==T.y) {
          break;
        }
        db w=angle(now.x,now.y,T.x,T.y,dir);
        if(now.x==0 || now.y==0) {//坐标轴上
          if(now.x==0) {
            if(now.y>=0) {//y轴正半轴
              if(dir==1) {
                if(w<=pi/2) {
                  ArcInterpolation::InterpolateArc(now,T,speed,dir,1,ox,oy);
                  now=T;
                }
                else {
                  tmp={r,0};
                  ArcInterpolation::InterpolateArc(now,tmp,speed,dir,1,ox,oy);
                  now=tmp;
                }
              }
              else {
                if(w<=pi/2) {
                  ArcInterpolation::InterpolateArc(now,T,speed,dir,2,ox,oy);
                  now=T;
                }
                else {
                  tmp={-r,0};
                  ArcInterpolation::InterpolateArc(now,tmp,speed,dir,2,ox,oy);
                  now=tmp;
                }
              }
            }
            else {//y轴负半轴
              if(dir==1) {
                if(w<=pi/2) {
                  ArcInterpolation::InterpolateArc(now,T,speed,dir,3,ox,oy);
                  now=T;
                }
                else {
                  tmp={-r,0};
                  ArcInterpolation::InterpolateArc(now,tmp,speed,dir,3,ox,oy);
                  now=tmp;
                }
              }
              else {
                if(w<=pi/2) {
                  ArcInterpolation::InterpolateArc(now,T,speed,dir,4,ox,oy);
                  now=T;
                }
                else {
                  tmp={r,0};
                  ArcInterpolation::InterpolateArc(now,tmp,speed,dir,4,ox,oy);
                  now=tmp;
                }
              }
            }
          }
          else {
            if(now.x>=0) {//x轴正半轴
              if(dir==1) {
                if(w<=pi/2) {
                  ArcInterpolation::InterpolateArc(now,T,speed,dir,4,ox,oy);
                  now=T;
                }
                else {
                  tmp={0,-r};
                  ArcInterpolation::InterpolateArc(now,tmp,speed,dir,4,ox,oy);
                  now=tmp;
                }
              }
              else {
                if(w<=pi/2) {
                  ArcInterpolation::InterpolateArc(now,T,speed,dir,1,ox,oy);
                  now=T;
                }
                else {
                  tmp={0,r};
                  ArcInterpolation::InterpolateArc(now,tmp,speed,dir,1,ox,oy);
                  now=tmp;
                }
              }
            }
            else {//x轴负半轴
              if(dir==1) {
                if(w<=pi/2) {
                  ArcInterpolation::InterpolateArc(now,T,speed,dir,2,ox,oy);
                  now=T;
                }
                else {
                  tmp={0,r};
                  ArcInterpolation::InterpolateArc(now,tmp,speed,dir,2,ox,oy);
                  now=tmp;
                }
              }
              else {
                if(w<=pi/2) {
                  ArcInterpolation::InterpolateArc(now,T,speed,dir,3,ox,oy);
                  now=T;
                }
                else {
                  tmp={0,-r};
                  ArcInterpolation::InterpolateArc(now,tmp,speed,dir,3,ox,oy);
                  now=tmp;
                }
              }              
            }
          }
        }
        else {
          int range=ck(now.x,now.y);
          db w2;
          switch(range) {
            case 1:{
              if(dir==1) {
                tmp={r,0};
                w2=angle(now.x,now.y,r,0,dir);
                if(w2<=w) ArcInterpolation::InterpolateArc(now,tmp,speed,dir,1,ox,oy),now=tmp;
                else ArcInterpolation::InterpolateArc(now,T,speed,dir,1,ox,oy),now=T;
              }
              else {
                tmp={0,r};
                w2=angle(now.x,now.y,0,r,dir);
                if(w2<=w) ArcInterpolation::InterpolateArc(now,tmp,speed,dir,1,ox,oy),now=tmp;
                else ArcInterpolation::InterpolateArc(now,T,speed,dir,1,ox,oy),now=T;
              }
              break;
            }
            case 2:{
              if(dir==1) {
                tmp={0,r};
                w2=angle(now.x,now.y,0,r,dir);
                if(w2<=w) ArcInterpolation::InterpolateArc(now,tmp,speed,dir,2,ox,oy),now=tmp;
                else ArcInterpolation::InterpolateArc(now,T,speed,dir,2,ox,oy),now=T;
              }
              else {
                tmp={-r,0};
                w2=angle(now.x,now.y,-r,0,dir);
                if(w2<=w) ArcInterpolation::InterpolateArc(now,tmp,speed,dir,2,ox,oy),now=tmp;
                else ArcInterpolation::InterpolateArc(now,T,speed,dir,2,ox,oy),now=T;
              }
              break;
            }
            case 3:{
              if(dir==1) {
                tmp={-r,0};
                w2=angle(now.x,now.y,-r,0,dir);
                if(w2<=w) ArcInterpolation::InterpolateArc(now,tmp,speed,dir,3,ox,oy),now=tmp;
                else ArcInterpolation::InterpolateArc(now,T,speed,dir,3,ox,oy),now=T;
              }
              else {
                tmp={0,-r};
                w2=angle(now.x,now.y,0,-r,dir);
                if(w2<=w) ArcInterpolation::InterpolateArc(now,tmp,speed,dir,3,ox,oy),now=tmp;
                else ArcInterpolation::InterpolateArc(now,T,speed,dir,3,ox,oy),now=T;
              }
              break;
            }
            case 4:{
              if(dir==1) {
                tmp={0,-r};
                w2=angle(now.x,now.y,0,-r,dir);
                if(w2<=w) ArcInterpolation::InterpolateArc(now,tmp,speed,dir,4,ox,oy),now=tmp;
                else ArcInterpolation::InterpolateArc(now,T,speed,dir,4,ox,oy),now=T;
              }
              else {
                tmp={r,0};
                w2=angle(now.x,now.y,r,0,dir);
                if(w2<=w) ArcInterpolation::InterpolateArc(now,tmp,speed,dir,4,ox,oy),now=tmp;
                else ArcInterpolation::InterpolateArc(now,T,speed,dir,4,ox,oy),now=T;
              }
            }
          }
        }
      }
    }
    ff:;
  }
}