// Raspberry Pi Pico (EarlePhilhower版) 用
// ライブラリやPWMを使わずにサーボをコントロールする
// サーボはGPIO数の26個まで使用できます
// ※マルチコアを使用するためEarlePhilhower版が必要

// サーボを接続したGPピン番号の配列 (26個まで対応)
int8_t SERVO_PIN[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9};
int SERVO_MIN = 544;    // 0°のパルス幅(μs)
int SERVO_MAX = 2400;   // 180°のパルス幅(μs)
int SERVO_FREQ = 20;    // サーボの周期(ms)

int16_t servo_deg[28];   // サーボ角度(0-180)

int8_t servo_count;     // サーボ数
int8_t servo_flag[28];  // パルス状態(0/1)
uint32_t servo_timer[28];
uint32_t servo_freq_timer;  // 20msのタイマー
int8_t servo_freq_flag;     // 1: 20msの周期中

void servo_setup() {
  servo_count = sizeof(SERVO_PIN) / sizeof(uint8_t);
  for (int i = 0; i < servo_count; i++) {
    pinMode(SERVO_PIN[i], OUTPUT);
    digitalWrite(SERVO_PIN[i], LOW);
    servo_deg[i] = 90;
    servo_flag[i] = 0;
    servo_timer[i] = 0;
  }
  servo_freq_timer = 0;
  servo_freq_flag = 0;
}

// 定期的に呼び出す(最低10μsec毎)
void servo_tick() {
  // 20ms周期
  uint32_t currentTime = micros();
  if (servo_freq_flag == 0 && currentTime > servo_freq_timer) {
    servo_freq_flag = 1;
    servo_freq_timer = currentTime + SERVO_FREQ * 1000; // 次の20ms後

    // パルス幅の時間を計算
    for (int i = 0; i < servo_count; i++) {
      servo_timer[i] = currentTime + SERVO_MIN + (SERVO_MAX - SERVO_MIN) * servo_deg[i] / 180;
    }

    // 全てのサーボをパルスHIGH
    for (int i = 0; i < servo_count; i++) {
      digitalWrite(SERVO_PIN[i], HIGH);
      servo_flag[i] = 1;
    }
  }

  //  パルス幅の時間が来たサーボをLOWにする
  if (servo_freq_flag == 1) {
    uint32_t currentTime = micros();
    for (int i = 0; i < servo_count; i++) {
      if (servo_flag[i] == 1 && currentTime > servo_timer[i]) {
        digitalWrite(SERVO_PIN[i], LOW);
        servo_flag[i] = 0;
      }
    }

    // すべてLOWになったか調べる
    int8_t complete_flag = 1;
    for (int i = 0; i < servo_count; i++) {
      if (servo_flag[i] == 1) {
        complete_flag = 0;
        break;
      }
    }
    if (complete_flag == 1) {
       // すべてLOWになった
       servo_freq_flag = 0;
    }
  }
}

void servo_write(int idx, int deg) {
  if (deg < 0) deg = 0;
  if (deg > 180) deg = 180;
  servo_deg[idx] = deg;
}

// -------------------------------
// CORE0
// -------------------------------
int dir = 1;
int deg = 90;

void setup() {
  Serial.begin(115200);
  delay(1000);  // 1秒待つ
}

void loop() {
  Serial.println(deg);

  deg += dir;
  if (deg < 0) {deg = 0; dir *= -1;}
  if (deg > 180) {deg = 180; dir *= -1;}
  servo_write(0, deg);
  servo_write(1, deg);
  servo_write(2, deg);
  servo_write(3, deg);
  servo_write(4, deg);
  servo_write(5, deg);
  servo_write(6, deg);
  servo_write(7, deg);
  servo_write(8, deg);
  servo_write(9, deg);
  delay(20);
}

// -------------------------------
// CORE1
// servo_tick()を10μs以下の周期で呼び出す必要があるため、CORE1を利用する
// -------------------------------

void setup1() {
  servo_setup();
}

void loop1() {
  servo_tick();
}