//------Raspberry Pi Pico W 同時２軸円弧補間実験ソフト------------------
#include <Stepper.h>

Stepper stepper_x(2048, 0, 2, 1, 3);  //X軸モータ設定(1回転2048ステップ)
Stepper stepper_y(2048, 4, 6, 5, 7);  //Y軸モータ設定(1回転2048ステップ)

long x_cur_pos = 0;    //X軸現在位置(符号付き機械座標のパルス位置)
long y_cur_pos = 0;    //Y軸現在位置(符号付き機械座標のパルス位置)

void setup() {
  Serial.begin(9600);       //USBシリアル通信初期化

  pinMode(15, INPUT);       //GP15を入力端子に設定(スイッチ接続)

  pinMode(0, OUTPUT);       //X軸ステッピングモータ(青線)
  pinMode(1, OUTPUT);       //x軸ステッピングモータ(桃線)
  pinMode(2, OUTPUT);       //x軸ステッピングモータ(黄線)
  pinMode(3, OUTPUT);       //x軸ステッピングモータ(橙線)
  pinMode(4, OUTPUT);       //Y軸ステッピングモータ(青線)
  pinMode(5, OUTPUT);       //Y軸ステッピングモータ(桃線)
  pinMode(6, OUTPUT);       //Y軸ステッピングモータ(黄線)
  pinMode(7, OUTPUT);       //Y軸ステッピングモータ(橙線)

  //ステッピングモータの初期設定
  stepper_x.setSpeed(500);  //X軸ステッピングモータの回転速度500rpm
  stepper_y.setSpeed(500);  //Y軸ステッピングモータの回転速度500rpm
}

void loop() {
  while (true){
    //スイッチ読込(LOW判断)
    if (digitalRead(15) == LOW){
      break;
    }
  }

  long x_pos = -400;        //X軸終点位置(パルス)
  long y_pos = 400;         //Y軸終点位置(パルス)
  long x_cent_leng = 0;     //X軸始点から中心までの距離(パルス)
  long y_cent_leng = 400;   //Y軸始点から中心までの距離(パルス)
  long vel = 250;           //送り速度(パルス/sec)
  bool flag_cw = false;     //CCW送り

  //円弧補間実施
  CircularInterporation(x_pos, y_pos, x_cent_leng, y_cent_leng, vel, flag_cw);

  x_pos = 0;                //X軸終点位置(パルス)
  y_pos = 0;                //Y軸終点位置(パルス)
  x_cent_leng = 0;          //X軸始点から中心までの距離(パルス)
  y_cent_leng = -400;       //Y軸始点から中心までの距離(パルス)
  vel = 250;                //送り速度(パルス/sec)
  flag_cw = false;          //CCW送り

  //円弧補間実施
  CircularInterporation(x_pos, y_pos, x_cent_leng, y_cent_leng, vel, flag_cw);

  x_pos = 400;              //X軸終点位置(パルス)
  y_pos = -400;             //Y軸終点位置(パルス)
  x_cent_leng = 400;        //X軸始点から中心までの距離(パルス)
  y_cent_leng = 0;          //Y軸始点から中心までの距離(パルス)
  vel = 250;                //送り速度(パルス/sec)
  flag_cw = true;           //CW送り

  //円弧補間実施
  CircularInterporation(x_pos, y_pos, x_cent_leng, y_cent_leng, vel, flag_cw);

  x_pos = 0;                //X軸終点位置(パルス)
  y_pos = 0;                //Y軸終点位置(パルス)
  x_cent_leng = -400;       //X軸始点から中心までの距離(パルス)
  y_cent_leng = 0;          //Y軸始点から中心までの距離(パルス)
  vel = 250;                //送り速度(パルス/sec)
  flag_cw = true;           //CW送り

  //円弧補間実施
  CircularInterporation(x_pos, y_pos, x_cent_leng, y_cent_leng, vel, flag_cw);
}

//-----------円弧補間実施処理--------------------------
//引数
//x_pos:  円弧補間終点X軸位置(パルス)
//y_pos:  円弧補間終点Y軸位置(パルス)
//x_dis_cent: 始点から中心までのX軸距離(パルス)
//y_dis_cent: 始点から中心までのY軸距離(パルス)
//vel:        送り速度(パルス/sec)
//cw_flag:    true: CW方向移動、 false: CCW方向移動
//----------------------------------------------------
void CircularInterporation(long x_pos, long y_pos, long x_dis_cent, long y_dis_cent, long vel, bool cw_flag){
  long x_cent = x_cur_pos + x_dis_cent;     //中心のX軸座標計算
  long y_cent = y_cur_pos + y_dis_cent;     //中心のY軸座標計算
  
  long x_sta_pos_0_0 = x_cur_pos - x_cent;  //中心(0,0)とした時のX軸始点設定
  long y_sta_pos_0_0 = y_cur_pos - y_cent;  //中心(0,0)とした時のY軸始点設定
  long x_pos_0_0 = x_sta_pos_0_0;           //中心(0,0)とした時のX軸現在位置初期設定
  long y_pos_0_0 = y_sta_pos_0_0;           //中心(0,0)とした時のY軸現在位置初期設定
  long x_end_pos_0_0 = x_pos - x_cent;      //中心(0,0)とした時のX軸の終点位置設定     
  long y_end_pos_0_0 = y_pos - y_cent;      //中心(0,0)とした時のY軸の終点位置設定
 
  //円の直径の二乗計算
  long radius_squar = x_sta_pos_0_0 * x_sta_pos_0_0 + y_sta_pos_0_0 * y_sta_pos_0_0;

  long sleepTime = 1000000 / vel;  //次のパルス出力までのスリープ時間
 
  //始点の象限検出
  int cur_quadrant = quadrantJudge(x_sta_pos_0_0, y_sta_pos_0_0, cw_flag);

  //終点の象限検出
  int end_quadrant = quadrantJudge(x_end_pos_0_0, y_end_pos_0_0, cw_flag);


  //円弧補間パルス発生処理
  while(true){
    //判別式判断処理
    long JD = x_pos_0_0 * x_pos_0_0 + y_pos_0_0 * y_pos_0_0 - radius_squar; //判別式
    if (JD >= 0){               //判別式>=0？
      if (cw_flag == false){    //CCW方向移動？
        switch (cur_quadrant){  //象限判断
          case 1:               //第１象限
            x_1_pulse_mov(-1);  //X軸負方向に１パルス移動
            x_pos_0_0--;        //中心を(0,0)とした時のX軸現在位置マイナス
            x_cur_pos--;        //現在位置マイナス
            break;
          case 2:               //第２象限
            y_1_pulse_mov(-1);  //Y軸負方向に１パルス移動
            y_pos_0_0--;        //中心を(0,0)とした時のy軸現在位置マイナス
            y_cur_pos--;        //現在位置マイナス
            break;
          case 3:               //第３象限
            x_1_pulse_mov(+1);  //X軸正方向に１パルス移動
            x_pos_0_0++;        //中心を(0,0)とした時のX軸現在位置プラス
            x_cur_pos++;        //現在位置プラス
            break;
          case 4:               //第４象限
            y_1_pulse_mov(+1);  //Y軸正方向に１パルス移動
            y_pos_0_0++;        //中心を(0,0)とした時のy軸現在位置プラス
            y_cur_pos++;        //現在位置プラス
            break;
          default:
            break;
        }
      }
      else{                     //CW方向移動
        switch (cur_quadrant){  //象限判断
          case 1:               //第１象限
            y_1_pulse_mov(-1);  //Y軸負方向に１パルス移動
            y_pos_0_0--;        //中心を(0,0)とした時のy軸現在位置マイナス
            y_cur_pos--;        //現在位置マイナス
            break;
          case 2:               //第２象限
            x_1_pulse_mov(+1);  //X軸正方向に１パルス移動
            x_pos_0_0++;        //中心を(0,0)とした時のX軸現在位置プラス
            x_cur_pos++;        //現在位置プラス
            break;
          case 3:               //第３象限
            y_1_pulse_mov(+1);  //Y軸正方向に１パルス移動
            y_pos_0_0++;        //中心を(0,0)とした時のy軸現在位置プラス
            y_cur_pos++;        //現在位置プラス
            break;
          case 4:               //第４象限
            x_1_pulse_mov(-1);  //X軸負方向に１パルス移動
            x_pos_0_0--;        //中心を(0,0)とした時のX軸現在位置マイナス
            x_cur_pos--;        //現在位置マイナス
            break;
          default:
            break;
        }
      }
    }
    else if (JD < 0){           //判別式<0
      if (cw_flag == false){    //CCW方向移動？
        switch (cur_quadrant){  //象限判断
          case 1:               //第１象限
            y_1_pulse_mov(+1);  //Y軸正方向に１パルス移動
            y_pos_0_0++;        //中心を(0,0)とした時のy軸現在位置プラス
            y_cur_pos++;        //現在位置プラス
            break;
          case 2:               //第２象限
            x_1_pulse_mov(-1);  //X軸負方向に１パルス移動
            x_pos_0_0--;        //中心を(0,0)とした時のX軸現在位置マイナス
            x_cur_pos--;        //現在位置マイナス
            break;
          case 3:               //第３象限
            y_1_pulse_mov(-1);  //Y軸負方向に１パルス移動
            y_pos_0_0--;        //中心を(0,0)とした時のy軸現在位置マイナス
            y_cur_pos--;        //現在位置マイナス
            break;
          case 4:               //第４象限
            x_1_pulse_mov(+1);  //X軸正方向に１パルス移動
            x_pos_0_0++;        //中心を(0,0)とした時のX軸現在位置プラス
            x_cur_pos++;        //現在位置プラス
            break;
          default:
            break;
        }
      }
      else{                     //CW方向移動
        switch (cur_quadrant){  //象限判断
          case 1:               //第１象限
            x_1_pulse_mov(+1);  //X軸負方向に１パルス移動
            x_pos_0_0++;        //中心を(0,0)とした時のX軸現在位置プラス
            x_cur_pos++;        //現在位置プラス
            break;
          case 2:               //第２象限
            y_1_pulse_mov(+1);  //Y軸正方向に１パルス移動
            y_pos_0_0++;        //中心を(0,0)とした時のy軸現在位置プラス
            y_cur_pos++;        //現在位置プラス
            break;
          case 3:               //第３象限
            x_1_pulse_mov(-1);  //X軸正方向に１パルス移動
            x_pos_0_0--;        //中心を(0,0)とした時のX軸現在位置マイナス
            x_cur_pos--;        //現在位置マイナス
            break;
          case 4:               //第４象限
            y_1_pulse_mov(-1);  //Y軸負方向に１パルス移動
            y_pos_0_0--;        //中心を(0,0)とした時のy軸現在位置マイナス
            y_cur_pos--;        //現在位置マイナス
            break;
          default:
            break;
        }
      }
    }

    //移動後の象限判断
    cur_quadrant = quadrantJudge(x_pos_0_0, y_pos_0_0, cw_flag);
  
    delayMicroseconds(sleepTime);     //次のパルス出力待ち

    //現在位置をPCへシリアル通信
    String text_x = String(x_cur_pos);
    String text_y = String(y_cur_pos);
    String text = String("X:" + text_x +"," + "Y:" + text_y);
    Serial.println(text);           //現在位置をPCへ送信

    //円弧補間終了判断処理
    bool res = Positioning_end_judge(x_pos_0_0, y_pos_0_0, x_end_pos_0_0, y_end_pos_0_0, cur_quadrant, end_quadrant, cw_flag);
    if (res == true){ //終了？
      break;
    }
  }
}

//-----------象限判断検出処理---------------------
//引数
//x_pos:  中心を(0,0)とした時のX軸位置
//y_pos:  中心を(0,0)とした時のY軸位置
//flag_cw:  移動方向　true: CW,   false: CCW
//リターン値　象限(1～4)
//-----------------------------------------------
int quadrantJudge(long x_pos, long y_pos, bool flag_cw){
  int quadrant = 1;                     //象限

  if ((x_pos > 0) && (y_pos > 0)){      //第１象限？
    quadrant = 1;                       //第１象限
  }
  else if ((x_pos < 0) && (y_pos > 0)){ //第２象限？
    quadrant = 2;                       //第２象限
  }
  else if ((x_pos < 0) && (y_pos < 0)){ //第３象限？
    quadrant = 3;                       //第３象限
  }
  else if ((x_pos > 0) && (y_pos < 0)){ //第４象限？
    quadrant = 4;                       //第４象限
  }
  else if (x_pos == 0){                //X=0の境界線での象限判断
    if (y_pos > 0){
      if (flag_cw == true){ //CW方向移動？
        quadrant = 1;       //第１象限
      }
      else{                 //CCW方向移動
        quadrant = 2;       //第２象限
      }

    }
    else if (y_pos < 0){
      if (flag_cw == true){ //CW方向移動？
        quadrant = 3;       //第３象限
      }
      else{                 //CCW方向移動
        quadrant = 4;       //第４象限
      }
    }
  }
  else if (y_pos == 0){     //y=0の境界線での象限判断
    if (x_pos > 0){
      if (flag_cw == true){ //CW方向移動？
        quadrant = 4;       //第４象限
      }
      else{                 //CCW方向移動
        quadrant = 1;       //第１象限
      }

    }
    else if (x_pos < 0){
      if (flag_cw == true){ //CW方向移動？
        quadrant = 2;       //第２象限
      }
      else{                 //CCW方向移動
        quadrant = 3;       //第３象限
      }
    }
  }

  return quadrant;
}

//------------円弧補間終了判断処理---------------------
//引数
//x_pos:      中心を(0,0)とした場合のX軸現在位置
//y_pos:      中心を(0,0)とした場合のY軸現在位置
//x_e_pos:    中心を(0,0)とした場合のX軸終点位置
//y_e_pos:    中心を(0,0)とした場合のY軸終点位置
//quadrant:   現在位置の象限
//e_quadrant: 終了位置の象限
//flag_cw:    移動方向フラグ  true: CW,  false: CCW
//リターン値
//true: 円弧補間終了、　false:  円弧補間終了
//----------------------------------------------------
bool Positioning_end_judge(long x_pos, long y_pos, long x_e_pos, long y_e_pos, int quadrant, int e_quadrant, bool flag_cw){
  bool res = false;

  if (quadrant == e_quadrant){        //現在の象限=終了象限？
    switch (quadrant){                //象限判断
      case 1:                         //第１象限
        if (flag_cw == false){        //CCW方向移動？
          if ((x_pos <= x_e_pos) && (y_pos >= y_e_pos)){
            res = true;
          }
        }
        else{                         //CW方向移動
          if ((x_pos >= x_e_pos) && (y_pos <= y_e_pos)){
            res = true;
          }
        }
        break;
      case 2:                         //第２象限
        if (flag_cw == false){        //CCW方向移動？
          if ((x_pos <= x_e_pos) && (y_pos <= y_e_pos)){
            res = true;
          }
        }
        else{                         //CW方向移動
          if ((x_pos >= x_e_pos) && (y_pos >= y_e_pos)){
            res = true;
          }
        }
        break;
      case 3:                         //第３象限
        if (flag_cw == false){        //CCW方向移動？
          if ((x_pos >= x_e_pos) && (y_pos <= y_e_pos)){
            res = true;
          }
        }
        else{                         //CW方向移動
          if ((x_pos <= x_e_pos) && (y_pos >= y_e_pos)){
            res = true;
          }
        }
        break;
      case 4:                         //第４象限
        if (flag_cw == false){        //CCW方向移動？
          if ((x_pos >= x_pos) && (y_pos >= y_e_pos)){
            res = true;
          }
        }
        else{                         //CW方向移動
          if ((x_pos <= x_pos) && (y_pos <= y_e_pos)){
            res = true;
          }
        }
        break;
      default:
        break;
    }
  }
 
  return res;
}

//-----------X軸１パルス移動処理---------------------
//引数
//direction: 移動方向   1: プラス方向、 -1: －方向
//--------------------------------------------------
void x_1_pulse_mov(int direction){
    //X軸方向に１パルス移動
  if (direction == 1){        //+方向移動？
    stepper_x.step(+1);       //+方向１ステップ移動
  }
  else if (direction == -1){  //-方向移動？
    stepper_x.step(-1);       //-方向１ステップ移動
  }
}

//-----------Y軸１パルス移動処理----------------------
//引数
//direction: 移動方向   1: プラス方向、 -1: －方向
//---------------------------------------------------
void y_1_pulse_mov(int direction){
  //Y軸方向に１パルス移動
  if (direction == 1){        //+方向移動？
    stepper_y.step(+1);       //+方向１ステップ移動
  }
  else if (direction == -1){  //-方向移動？
    stepper_y.step(-1);       //-方向１ステップ移動
  }
}