goog.provide('Blockly.Blocks.user');
goog.require('Blockly.Blocks');
goog.require('Blockly.Types');

Blockly.Blocks.user.HUE = 315;

// ================================
// TB6612fng 馬達擴充板
// ================================

  // 共用注入：腳位/旗標/映射/motor_c/wrapper/pinMode
  function ywrobot_tb6612fng_injectCommon_() {
    // 腳位常數（依你板子絲印）
    Blockly.Arduino.definitions_['yw_tb6612_pins'] =
      'const uint8_t TB6612_DIR1 = 4; // D4\n' +
      'const uint8_t TB6612_EN1  = 5; // D5 (PWM)\n' +
      'const uint8_t TB6612_EN2  = 6; // D6 (PWM)\n' +
      'const uint8_t TB6612_DIR2 = 7; // D7\n';

    // 反向旗標（預設 false；就算沒放初始化積木也能編譯）
    Blockly.Arduino.definitions_['yw_tb6612_inv_flags'] =
      'bool tb6612_inv_m1 = false;\n' +
      'bool tb6612_inv_m2 = false;\n';

    // 車子左右輪對應的馬達編號（預設：左=M1、右=M2；初始化積木會覆蓋）
    Blockly.Arduino.definitions_['yw_car_map'] =
      'uint8_t car_left_motor = 1;\n' +
      'uint8_t car_right_motor = 2;\n';

    // pinMode（只需一次）
    Blockly.Arduino.setups_['yw_tb6612_pinmode'] =
      'pinMode(TB6612_DIR1, OUTPUT);\n' +
      'pinMode(TB6612_EN1, OUTPUT);\n' +
      'pinMode(TB6612_EN2, OUTPUT);\n' +
      'pinMode(TB6612_DIR2, OUTPUT);\n';

    // motor_c：修正 stop + speed constrain + 反向旗標
    Blockly.Arduino.definitions_['motor_c'] =
      'void motor_c(int motor_n, int direction_m, int speed_m) {\n' +
      '  // direction_m: 0=正轉, 1=反轉, 2=停止\n' +
      '  speed_m = constrain(speed_m, 0, 255);\n' +
      '\n' +
      '  // 反向設定（只在非停止時反相）\n' +
      '  if (direction_m != 2) {\n' +
      '    if (motor_n == 1 && tb6612_inv_m1) direction_m = 1 - direction_m;\n' +
      '    if (motor_n == 2 && tb6612_inv_m2) direction_m = 1 - direction_m;\n' +
      '  }\n' +
      '\n' +
      '  if (motor_n == 1) {\n' +
      '    if (direction_m == 2) { analogWrite(TB6612_EN1, 0); return; }\n' +
      '    digitalWrite(TB6612_DIR1, (direction_m == 0) ? LOW : HIGH);\n' +
      '    analogWrite(TB6612_EN1, speed_m);\n' +
      '  } else if (motor_n == 2) {\n' +
      '    if (direction_m == 2) { analogWrite(TB6612_EN2, 0); return; }\n' +
      '    digitalWrite(TB6612_DIR2, (direction_m == 0) ? LOW : HIGH);\n' +
      '    analogWrite(TB6612_EN2, speed_m);\n' +
      '  }\n' +
      '}\n';

    // wrapper：以後所有積木都建議用 motor_left/right，不再碰 M1/M2
    Blockly.Arduino.definitions_['yw_motor_wrappers'] =
      'void motor_left(int direction_m, int speed_m) {\n' +
      '  motor_c(car_left_motor, direction_m, speed_m);\n' +
      '}\n' +
      'void motor_right(int direction_m, int speed_m) {\n' +
      '  motor_c(car_right_motor, direction_m, speed_m);\n' +
      '}\n';
  }


  // ① 初始化積木 generator
  Blockly.Arduino['tb6612fng_init'] = function(block) {
    ywrobot_tb6612fng_injectCommon_();

    var leftMotor  = block.getFieldValue('LEFT_MOTOR') || '1';   // "1"/"2"
    var rightMotor = block.getFieldValue('RIGHT_MOTOR') || '2';  // "1"/"2"

    var invM1 = (block.getFieldValue('INV_M1') === 'TRUE');
    var invM2 = (block.getFieldValue('INV_M2') === 'TRUE');

    Blockly.Arduino.setups_['yw_car_apply_map'] =
      'car_left_motor = ' + leftMotor + ';\n' +
      'car_right_motor = ' + rightMotor + ';\n' +
      'tb6612_inv_m1 = ' + (invM1 ? 'true' : 'false') + ';\n' +
      'tb6612_inv_m2 = ' + (invM2 ? 'true' : 'false') + ';\n';

    return '';
  };


  // ② 雙輪控制積木 generator
  Blockly.Arduino['tb6612fng_dual_wheel'] = function(block) {
    ywrobot_tb6612fng_injectCommon_();

    var lDir = block.getFieldValue('L_DIR') || '0'; // "0","1","2"
    var rDir = block.getFieldValue('R_DIR') || '0'; // "0","1","2"

    var lSpeed = Blockly.Arduino.valueToCode(block, 'L_SPEED', Blockly.Arduino.ORDER_ATOMIC) || '0';
    var rSpeed = Blockly.Arduino.valueToCode(block, 'R_SPEED', Blockly.Arduino.ORDER_ATOMIC) || '0';

    var code = '';
    code += 'motor_left(' + lDir + ', ' + lSpeed + ');\n';
    code += 'motor_right(' + rDir + ', ' + rSpeed + ');\n';
    return code;
  };


// ================================
// VL5310x 雷射測距感測模組
// ================================

  Blockly.Arduino['block_init_vl53l0x'] = function(block) {
    var dropdown_vl53l0x_mode_accuracy = block.getFieldValue('vl53l0x_mode_accuracy') || 'eLow';
    var dropdown_vl53l0x_mode_measurement = block.getFieldValue('vl53l0x_mode_measurement') || 'eContinuous';

    Blockly.Arduino.addInclude('Wire','#include <Wire.h>');
    Blockly.Arduino.addInclude('DFRobot_VL53L0X','#include <DFRobot_VL53L0X.h>');
    Blockly.Arduino.addDeclaration('vl53l0x','DFRobot_VL53L0X vl53l0x;');
    Blockly.Arduino.addSetup('Wire','Wire.begin();',true);
    Blockly.Arduino.addSetup('vl53l0x.begin','vl53l0x.begin();',true);
    Blockly.Arduino.addSetup('vl53l0x.setMode','vl53l0x.setMode(' + dropdown_vl53l0x_mode_measurement + ',' + dropdown_vl53l0x_mode_accuracy + ');',true);
    Blockly.Arduino.addSetup('vl53l0x.start','vl53l0x.start();',true);

    return '';
  };

  Blockly.Arduino['block_vl53l0x'] = function(block) {
    Blockly.Arduino.addInclude('Wire','#include <Wire.h>');
    Blockly.Arduino.addInclude('DFRobot_VL53L0X','#include <DFRobot_VL53L0X.h>');

    Blockly.Arduino.addDeclaration('vl53l0x','DFRobot_VL53L0X vl53l0x;');

    Blockly.Arduino.addSetup('Wire','Wire.begin();',true);
    Blockly.Arduino.addSetup('vl53l0x.begin','vl53l0x.begin();',true);
    Blockly.Arduino.addSetup('vl53l0x.setMode','vl53l0x.setMode(eContinuous,eLow);',true);
    Blockly.Arduino.addSetup('vl53l0x.start','vl53l0x.start();',true);

    var code = 'vl53l0x.getDistance()';
    return [code, Blockly.Arduino.ORDER_NONE];
  };


// ================================
// YFROBOT QTR-5RC 循線模組
// ================================

/* Removed from the maintained toolbox: YFROBOT QTR-5RC generators are legacy-only.
  Blockly.Arduino['block_initqrt_5rc'] = function(block) {
    var dropdown_ir1 = block.getFieldValue('IR1');
    var dropdown_ir2 = block.getFieldValue('IR2');
    var dropdown_ir3 = block.getFieldValue('IR3');
    var dropdown_ir4 = block.getFieldValue('IR4');
    var dropdown_ir5 = block.getFieldValue('IR5');
    var dropdown_en = block.getFieldValue('EN');

    Blockly.Arduino.addInclude('QTRSensors','#include <QTRSensors.h>');
    Blockly.Arduino.addDeclaration('QTRSensors','QTRSensors qtr;');

    Blockly.Arduino.addSetup('setTypeRC','qtr.setTypeRC();',true);
    Blockly.Arduino.addSetup('setSensorPins','qtr.setSensorPins((const uint8_t[]){' + dropdown_ir1 + ',' + dropdown_ir2 + ',' + dropdown_ir3 + ',' + dropdown_ir4 + ',' + dropdown_ir5 + '},5);',true);
    Blockly.Arduino.addSetup('setEmitterPin','qtr.setEmitterPin(' + dropdown_en + ');',true);

    return '';
  };

  Blockly.Arduino['block_qrt_calibrate'] = function(block) {
    Blockly.Arduino.addSetup('calibrate','qtr.calibrate();',true);
    return '';
  };

  Blockly.Arduino['block_qrt_read_save'] = function(block) {
    Blockly.Arduino.addVariable('sensorValues','uint16_t sensorValues[5];',true);
    return 'qtr.readLineBlack(sensorValues);\n';
  };

  Blockly.Arduino['block_qrt_read'] = function(block) {
    var code = 'qtr.readLineBlack(sensorValues)';
    return [code, Blockly.Arduino.ORDER_NONE];
  };

  Blockly.Arduino['block_qrt_value'] = function(block) {
    var dropdown_ir_n_value = block.getFieldValue('ir_n_value');
    var code = 'sensorValues[' + dropdown_ir_n_value + ']';
    return [code, Blockly.Arduino.ORDER_NONE];
  };
*/


// ================================
// 八路循線模組
// ================================

// Keep the legacy Uno/Nano adapter on the same sensor profile contract as the
// ESP32 board generators.  The old Ardublockly loader cannot import the TS
// module directly, so this small plain-JavaScript mirror is intentionally kept
// beside the legacy generator.
var EIGHT_LINE_SENSOR_PROFILES = {
  YAHBO: { address: '0x12', register: '0x30', blackIsZero: 'true', reverseBits: 'true' },
  HINWONDER: { address: '0x5D', register: '5', blackIsZero: 'false', reverseBits: 'false' }
};

// 8路循線 I2C 共用
function ensureEightLineI2CCommon_() {
  Blockly.Arduino.addInclude('include_wire', '#include <Wire.h>');

  if (!Blockly.Arduino.definitions_['eight_line_vars_once']) {
    Blockly.Arduino.definitions_['eight_line_vars_once'] =
      'uint8_t eight_X1=0,eight_X2=0,eight_X3=0,eight_X4=0,eight_X5=0,eight_X6=0,eight_X7=0,eight_X8=0;\n' +
      'uint8_t eight_data = 0;\n' +
      'bool EIGHT_READ_OK = false;\n' +
      'int EIGHT_I2C_DELAY_MS = 10;\n' +
      'uint8_t EIGHT_I2C_ADDR = 0x12;\n' +
      'uint8_t EIGHT_STATE_REG = 0x30;\n' +
      'bool EIGHT_BLACK_IS_ZERO = true;\n' +
      'bool EIGHT_REVERSE_BITS = true;\n' +
      'int EIGHT_PD_SIGN = 1;\n' +
      'int EIGHT_PD_DIV = 1;\n' +
      'int EIGHT_WEIGHT_X1 = -7;\n' +
      'int EIGHT_WEIGHT_X2 = -5;\n' +
      'int EIGHT_WEIGHT_X3 = -3;\n' +
      'int EIGHT_WEIGHT_X4 = -1;\n' +
      'int EIGHT_WEIGHT_X5 = 1;\n' +
      'int EIGHT_WEIGHT_X6 = 3;\n' +
      'int EIGHT_WEIGHT_X7 = 5;\n' +
      'int EIGHT_WEIGHT_X8 = 7;\n';
  }

  Blockly.Arduino.setups_['eight_line_wire_begin_once'] = 'Wire.begin();\n';

  if (!Blockly.Arduino.definitions_['eight_line_read_func_once']) {
    Blockly.Arduino.definitions_['eight_line_read_func_once'] = ' ';

    Blockly.Arduino.addFunction(
      'eightLineRead',
`void eightLineRead() {
  EIGHT_READ_OK = false;
  Wire.beginTransmission(EIGHT_I2C_ADDR);
  Wire.write(EIGHT_STATE_REG);
  Wire.endTransmission();
  delay(EIGHT_I2C_DELAY_MS);

  Wire.requestFrom(EIGHT_I2C_ADDR, (uint8_t)1);
  if (Wire.available()) {
    eight_data = Wire.read();
    EIGHT_READ_OK = true;
  }
  if (!EIGHT_READ_OK) {
    eight_X1 = eight_X2 = eight_X3 = eight_X4 = 0;
    eight_X5 = eight_X6 = eight_X7 = eight_X8 = 0;
    return;
  }

  uint8_t b1, b2, b3, b4, b5, b6, b7, b8;

  if (EIGHT_REVERSE_BITS) {
    b1 = (eight_data >> 7) & 0x01;
    b2 = (eight_data >> 6) & 0x01;
    b3 = (eight_data >> 5) & 0x01;
    b4 = (eight_data >> 4) & 0x01;
    b5 = (eight_data >> 3) & 0x01;
    b6 = (eight_data >> 2) & 0x01;
    b7 = (eight_data >> 1) & 0x01;
    b8 = (eight_data >> 0) & 0x01;
  } else {
    b1 = (eight_data >> 0) & 0x01;
    b2 = (eight_data >> 1) & 0x01;
    b3 = (eight_data >> 2) & 0x01;
    b4 = (eight_data >> 3) & 0x01;
    b5 = (eight_data >> 4) & 0x01;
    b6 = (eight_data >> 5) & 0x01;
    b7 = (eight_data >> 6) & 0x01;
    b8 = (eight_data >> 7) & 0x01;
  }

  if (EIGHT_BLACK_IS_ZERO) {
    eight_X1 = !b1;
    eight_X2 = !b2;
    eight_X3 = !b3;
    eight_X4 = !b4;
    eight_X5 = !b5;
    eight_X6 = !b6;
    eight_X7 = !b7;
    eight_X8 = !b8;
  } else {
    eight_X1 = b1;
    eight_X2 = b2;
    eight_X3 = b3;
    eight_X4 = b4;
    eight_X5 = b5;
    eight_X6 = b6;
    eight_X7 = b7;
    eight_X8 = b8;
  }
}`
    );
  }

  if (!Blockly.Arduino.definitions_['eight_line_weight_helper_once']) {
    Blockly.Arduino.definitions_['eight_line_weight_helper_once'] = ' ';

    Blockly.Arduino.addFunction(
      'eightLineSensorWeight',
`int eightLineSensorWeight(uint8_t sensorNo) {
  switch (sensorNo) {
    case 1: return EIGHT_WEIGHT_X1;
    case 2: return EIGHT_WEIGHT_X2;
    case 3: return EIGHT_WEIGHT_X3;
    case 4: return EIGHT_WEIGHT_X4;
    case 5: return EIGHT_WEIGHT_X5;
    case 6: return EIGHT_WEIGHT_X6;
    case 7: return EIGHT_WEIGHT_X7;
    case 8: return EIGHT_WEIGHT_X8;
    default: return 0;
  }
}`
    );

    Blockly.Arduino.addFunction(
      'eightLineWeightedError',
`float eightLineWeightedError() {
  int sum = 0;
  int cnt = 0;

  if (eight_X1) { sum += eightLineSensorWeight(1); cnt++; }
  if (eight_X2) { sum += eightLineSensorWeight(2); cnt++; }
  if (eight_X3) { sum += eightLineSensorWeight(3); cnt++; }
  if (eight_X4) { sum += eightLineSensorWeight(4); cnt++; }
  if (eight_X5) { sum += eightLineSensorWeight(5); cnt++; }
  if (eight_X6) { sum += eightLineSensorWeight(6); cnt++; }
  if (eight_X7) { sum += eightLineSensorWeight(7); cnt++; }
  if (eight_X8) { sum += eightLineSensorWeight(8); cnt++; }

  // 999 代表目前沒有任何感測器偵測到黑線，避免與中心值 0 混淆
  if (cnt == 0) return 999.0;

  float e = (float)sum / (float)cnt;
  e = e * (float)EIGHT_PD_SIGN;
  if (EIGHT_PD_DIV > 1) e = e / (float)EIGHT_PD_DIV;
  return e;
}`
    );

    Blockly.Arduino.addFunction(
      'eightLineRangeAtLeast',
`bool eightLineRangeAtLeast(uint8_t startNo, uint8_t endNo, uint8_t minCount, bool targetBlack) {
  startNo = constrain(startNo, 1, 8);
  endNo = constrain(endNo, 1, 8);
  if (startNo > endNo) {
    uint8_t tmp = startNo;
    startNo = endNo;
    endNo = tmp;
  }

  uint8_t matchCount = 0;
  for (uint8_t i = startNo; i <= endNo; i++) {
    uint8_t v = 0;
    switch (i) {
      case 1: v = eight_X1; break;
      case 2: v = eight_X2; break;
      case 3: v = eight_X3; break;
      case 4: v = eight_X4; break;
      case 5: v = eight_X5; break;
      case 6: v = eight_X6; break;
      case 7: v = eight_X7; break;
      case 8: v = eight_X8; break;
    }

    if (targetBlack) {
      if (v == 1) matchCount++;
    } else {
      if (v == 0) matchCount++;
    }
  }

  return matchCount >= minCount;
}`
    );
  }
 }

Blockly.Arduino['block_init_eight_line'] = function(block) {
  ensureEightLineI2CCommon_();

  var moduleType = block.getFieldValue('MODULE_TYPE') || 'YAHBO';
  var delayMs = parseInt(block.getFieldValue('I2C_DELAY_MS') || '10', 10);
  if (isNaN(delayMs)) delayMs = 10;

  var code = '';
  code += 'EIGHT_I2C_DELAY_MS = constrain(' + delayMs + ', 0, 50);\n';

  var profile = EIGHT_LINE_SENSOR_PROFILES[moduleType] || EIGHT_LINE_SENSOR_PROFILES.YAHBO;
  code += 'EIGHT_I2C_ADDR = ' + profile.address + ';\n';
  code += 'EIGHT_STATE_REG = ' + profile.register + ';\n';
  code += 'EIGHT_BLACK_IS_ZERO = ' + profile.blackIsZero + ';\n';
  code += 'EIGHT_REVERSE_BITS = ' + profile.reverseBits + ';\n';
  code += 'EIGHT_PD_SIGN = 1;\n';
  code += 'EIGHT_PD_DIV = 1;\n';

  return code;
};

Blockly.Arduino['block_line_pd_gain_setting'] = function(block) {
  ensurePdFunctions_();

  var kp = parseFloat(block.getFieldValue('KP') || '20');
  var kd = parseFloat(block.getFieldValue('KD') || '8');

  if (isNaN(kp)) kp = 20;
  if (isNaN(kd)) kd = 8;

  var code = '';
  code += 'KP_PD = ' + kp.toFixed(2) + ';\n';
  code += 'KD_PD = ' + kd.toFixed(2) + ';\n';
  return code;
};

Blockly.Arduino['block_line_pd_filter_setting'] = function(block) {
  ensurePdFunctions_();

  var filter = parseFloat(block.getFieldValue('FILTER') || '0.35');
  var maxCorrection = parseInt(block.getFieldValue('MAX_CORRECTION') || '120', 10);

  if (isNaN(filter)) filter = 0.35;
  if (isNaN(maxCorrection)) maxCorrection = 120;

  var code = '';
  code += 'PD_FILTER = constrain(' + filter.toFixed(2) + ', 0.0, 1.0);\n';
  code += 'PD_MAX_CORRECTION = constrain(' + maxCorrection + ', 0, 255);\n';
  return code;
};

Blockly.Arduino['block_line_pd_lost_setting'] = function(block) {
  ensurePdFunctions_();

  var lostSpeed = parseInt(block.getFieldValue('LOST_SPEED') || '90', 10);
  var lostLimit = parseInt(block.getFieldValue('LOST_LIMIT') || '4', 10);

  if (isNaN(lostSpeed)) lostSpeed = 90;
  if (isNaN(lostLimit)) lostLimit = 4;

  var code = '';
  code += 'PD_LOST_SPEED = constrain(' + lostSpeed + ', 0, 255);\n';
  code += 'PD_LOST_LIMIT = constrain(' + lostLimit + ', 1, 20);\n';
  return code;
};

Blockly.Arduino['block_read_eight_line_value'] = function(block) {
  ensureEightLineI2CCommon_();
  return 'eightLineRead();\n';
};

Blockly.Arduino['block_eight_value'] = function(block) {
  ensureEightLineI2CCommon_();
  var dropdown_value_name = block.getFieldValue('Value_NAME') || 'X1';
  if (!/^X[1-8]$/.test(dropdown_value_name)) dropdown_value_name = 'X1';
  var code = 'eight_' + dropdown_value_name;
  return [code, Blockly.Arduino.ORDER_ATOMIC];
};

Blockly.Arduino['block_eight_line_bin8'] = function(block) {
  ensureEightLineI2CCommon_();
  var code =
    'String(eight_X1)+String(eight_X2)+String(eight_X3)+String(eight_X4)+' +
    'String(eight_X5)+String(eight_X6)+String(eight_X7)+String(eight_X8)';
  return [code, Blockly.Arduino.ORDER_ATOMIC];
};

Blockly.Arduino['block_eight_line_black_count'] = function(block) {
  ensureEightLineI2CCommon_();
  var code =
    '(eight_X1 + eight_X2 + eight_X3 + eight_X4 + ' +
    ' eight_X5 + eight_X6 + eight_X7 + eight_X8)';
  return [code, Blockly.Arduino.ORDER_ATOMIC];
};


Blockly.Arduino['block_eight_line_range_count_condition'] = function(block) {
  ensureEightLineI2CCommon_();

  var startNo = parseInt(block.getFieldValue('START') || '1', 10);
  var endNo = parseInt(block.getFieldValue('END') || '8', 10);
  var minCount = parseInt(block.getFieldValue('MIN_COUNT') || '1', 10);
  var target = block.getFieldValue('TARGET') || 'BLACK';

  if (isNaN(startNo)) startNo = 1;
  if (isNaN(endNo)) endNo = 8;
  if (isNaN(minCount)) minCount = 1;

  startNo = Math.max(1, Math.min(8, startNo));
  endNo = Math.max(1, Math.min(8, endNo));
  minCount = Math.max(1, Math.min(8, minCount));

  var targetBlack = (target === 'BLACK') ? 'true' : 'false';
  var code = 'eightLineRangeAtLeast(' + startNo + ', ' + endNo + ', ' + minCount + ', ' + targetBlack + ')';
  return [code, Blockly.Arduino.ORDER_ATOMIC];
};

Blockly.Arduino['block_eight_line_sensor_weight'] = function(block) {
  ensureEightLineI2CCommon_();
  var sensorName = block.getFieldValue('SENSOR_NAME') || 'X1';
  if (!/^X[1-8]$/.test(sensorName)) sensorName = 'X1';
  var sensorNo = parseInt(sensorName.replace('X', ''), 10);
  if (isNaN(sensorNo)) sensorNo = 1;
  var code = 'eightLineSensorWeight(' + sensorNo + ')';
  return [code, Blockly.Arduino.ORDER_ATOMIC];
};

Blockly.Arduino['block_eight_line_weight_error'] = function(block) {
  ensureEightLineI2CCommon_();
  var code = 'eightLineWeightedError()';
  return [code, Blockly.Arduino.ORDER_ATOMIC];
};

Blockly.Arduino['block_line_pd_weight_setting'] = function(block) {
  ensureEightLineI2CCommon_();

  function readWeight_(name, fallback) {
    var v = parseInt(block.getFieldValue(name), 10);
    if (isNaN(v)) v = fallback;
    return Math.max(-50, Math.min(50, v));
  }

  var w1 = readWeight_('W_X1', -7);
  var w2 = readWeight_('W_X2', -5);
  var w3 = readWeight_('W_X3', -3);
  var w4 = readWeight_('W_X4', -1);
  var w5 = readWeight_('W_X5', 1);
  var w6 = readWeight_('W_X6', 3);
  var w7 = readWeight_('W_X7', 5);
  var w8 = readWeight_('W_X8', 7);

  var code = '';
  code += 'EIGHT_WEIGHT_X1 = ' + w1 + ';\n';
  code += 'EIGHT_WEIGHT_X2 = ' + w2 + ';\n';
  code += 'EIGHT_WEIGHT_X3 = ' + w3 + ';\n';
  code += 'EIGHT_WEIGHT_X4 = ' + w4 + ';\n';
  code += 'EIGHT_WEIGHT_X5 = ' + w5 + ';\n';
  code += 'EIGHT_WEIGHT_X6 = ' + w6 + ';\n';
  code += 'EIGHT_WEIGHT_X7 = ' + w7 + ';\n';
  code += 'EIGHT_WEIGHT_X8 = ' + w8 + ';\n';
  return code;
};



// PD 動態速度模式設定（v21）
Blockly.Arduino['block_line_pd_dynamic_speed_setting'] = function(block) {
  ensurePdFunctions_();

  var enable = (block.getFieldValue('DYN_ENABLE') || '1') === '1';
  var fastSpeed = parseInt(block.getFieldValue('FAST_SPEED') || '210', 10);
  var slowSpeed = parseInt(block.getFieldValue('SLOW_SPEED') || '140', 10);
  var centerErr = parseFloat(block.getFieldValue('CENTER_ERR') || '1.0');
  var curveErr = parseFloat(block.getFieldValue('CURVE_ERR') || '4.0');

  if (isNaN(fastSpeed)) fastSpeed = 210;
  if (isNaN(slowSpeed)) slowSpeed = 140;
  if (isNaN(centerErr)) centerErr = 1.0;
  if (isNaN(curveErr)) curveErr = 4.0;

  var code = '';
  code += 'PD_DYNAMIC_ENABLE = ' + (enable ? 'true' : 'false') + ';\n';
  code += 'PD_DYN_FAST_SPEED = constrain(' + fastSpeed + ', 0, 255);\n';
  code += 'PD_DYN_SLOW_SPEED = constrain(' + slowSpeed + ', 0, 255);\n';
  code += 'PD_DYN_CENTER_ERR = ' + centerErr.toFixed(2) + ';\n';
  code += 'PD_DYN_CURVE_ERR = ' + curveErr.toFixed(2) + ';\n';
  return code;
};

// ================================
// 傳統循線（Straight）
// ================================
Blockly.Arduino['block_line_straight_init_gain'] = function(block) {
  ensureEightLineI2CCommon_();
  ywrobot_tb6612fng_injectCommon_();

  var lPwmField = block.getFieldValue('L_PWM_IN');
  var rPwmField = block.getFieldValue('R_PWM_IN');
  var lPwmExpr = (lPwmField !== null && lPwmField !== undefined && String(lPwmField).trim() !== '') ? lPwmField : (Blockly.Arduino.valueToCode(block, 'L_PWM_IN', Blockly.Arduino.ORDER_ATOMIC) || '150');
  var rPwmExpr = (rPwmField !== null && rPwmField !== undefined && String(rPwmField).trim() !== '') ? rPwmField : (Blockly.Arduino.valueToCode(block, 'R_PWM_IN', Blockly.Arduino.ORDER_ATOMIC) || '150');


  function gain_(name, fallback) {
    var value = Number(block.getFieldValue(name));
    return (isFinite(value) ? Math.max(0, Math.min(1, value)) : fallback).toFixed(2);
  }
  var G0 = gain_('GAIN_0', 1.00);
  var G1 = gain_('GAIN_1', 0.85);
  var G2 = gain_('GAIN_2', 0.75);
  var G3 = gain_('GAIN_3', 0.65);
  var G4 = gain_('GAIN_4', 0.55);
  var G5 = gain_('GAIN_5', 0.45);

  Blockly.Arduino.addVariable('line_pos_str_straight', 'String blackPos_straight = "";', true);

  Blockly.Arduino.addVariable(
    'line_straight_params_st',
    'int LF_PWM_ST=150;\n' +
    'int RF_PWM_ST=150;\n' +
    'float GAIN_ST0=1.00;\n' +
    'float GAIN_ST1=0.85;\n' +
    'float GAIN_ST2=0.75;\n' +
    'float GAIN_ST3=0.65;\n' +
    'float GAIN_ST4=0.55;\n' +
    'float GAIN_ST5=0.45;\n' +
    'int LINE_STRAIGHT_LAST_DIR=0; // -1=最後偏左，1=最後偏右\n',
    true
  );

  Blockly.Arduino.addFunction(
    'lineStraightReadAndBuildPos',
`void lineStraightReadAndBuildPos() {
  eightLineRead();
  blackPos_straight =
    String(eight_X1)+String(eight_X2)+String(eight_X3)+String(eight_X4)+
    String(eight_X5)+String(eight_X6)+String(eight_X7)+String(eight_X8);
}`
  );

  Blockly.Arduino.addFunction(
    'lineStraightApplyTableAndDrive',
`void lineStraightApplyTableAndDrive() {
  // 使用加權誤差取代有限的字串查表，讓多顆同時壓在線上、路口與不同
  // 感測器組合也能得到連續且可預期的轉向。
  if (!EIGHT_READ_OK) {
    motor_left(2, 0);
    motor_right(2, 0);
    return;
  }

  int sum = 0;
  int count = 0;
  if (eight_X1) { sum += EIGHT_WEIGHT_X1; count++; }
  if (eight_X2) { sum += EIGHT_WEIGHT_X2; count++; }
  if (eight_X3) { sum += EIGHT_WEIGHT_X3; count++; }
  if (eight_X4) { sum += EIGHT_WEIGHT_X4; count++; }
  if (eight_X5) { sum += EIGHT_WEIGHT_X5; count++; }
  if (eight_X6) { sum += EIGHT_WEIGHT_X6; count++; }
  if (eight_X7) { sum += EIGHT_WEIGHT_X7; count++; }
  if (eight_X8) { sum += EIGHT_WEIGHT_X8; count++; }

  if (count == 0) {
    // 全白代表暫時丟線：依最後偏移方向原地搜尋，未知方向則低速直行。
    int search = constrain((int)(min(LF_PWM_ST, RF_PWM_ST) * 0.45), 40, 120);
    if (LINE_STRAIGHT_LAST_DIR < 0) {
      motor_left(1, search);
      motor_right(0, search);
    } else if (LINE_STRAIGHT_LAST_DIR > 0) {
      motor_left(0, search);
      motor_right(1, search);
    } else {
      motor_left(0, search);
      motor_right(0, search);
    }
    return;
  }

  float error = ((float)sum / (float)count) * (float)EIGHT_PD_SIGN;
  if (EIGHT_PD_DIV > 1) error = error / (float)EIGHT_PD_DIV;
  if (error < -0.2) LINE_STRAIGHT_LAST_DIR = -1;
  else if (error > 0.2) LINE_STRAIGHT_LAST_DIR = 1;
  int level = constrain((int)(abs(error) + 0.5), 0, 5);
  float lMul = GAIN_ST0;
  float rMul = GAIN_ST0;
  float gain = GAIN_ST0;
  if (level == 1) gain = GAIN_ST1;
  else if (level == 2) gain = GAIN_ST2;
  else if (level == 3) gain = GAIN_ST3;
  else if (level == 4) gain = GAIN_ST4;
  else if (level >= 5) gain = GAIN_ST5;
  if (error < -0.2) lMul = gain;
  else if (error > 0.2) rMul = gain;

  int lSpd = constrain((int)(LF_PWM_ST * lMul), 0, 255);
  int rSpd = constrain((int)(RF_PWM_ST * rMul), 0, 255);

  motor_left(0, lSpd);
  motor_right(0, rSpd);
}`
  );

  var code = '';
  code += 'LF_PWM_ST = constrain((int)(' + lPwmExpr + '), 0, 255);\n';
  code += 'RF_PWM_ST = constrain((int)(' + rPwmExpr + '), 0, 255);\n';
  code += 'GAIN_ST0 = ' + G0 + ';\n';
  code += 'GAIN_ST1 = ' + G1 + ';\n';
  code += 'GAIN_ST2 = ' + G2 + ';\n';
  code += 'GAIN_ST3 = ' + G3 + ';\n';
  code += 'GAIN_ST4 = ' + G4 + ';\n';
  code += 'GAIN_ST5 = ' + G5 + ';\n';
  code += 'LINE_STRAIGHT_LAST_DIR = 0;\n';

  return code;
};

Blockly.Arduino['block_line_straight_forward_gain'] = function(block) {
  ensureEightLineI2CCommon_();
  ywrobot_tb6612fng_injectCommon_();

  var code =
    'lineStraightReadAndBuildPos();\n' +
    'lineStraightApplyTableAndDrive();\n';
  return code;
};


// ================================
// PD 循線（加權 + float誤差 + 濾波 + 可開關丟線尋黑線）
// ================================
function ensurePdFunctions_() {
  ensureEightLineI2CCommon_();
  ywrobot_tb6612fng_injectCommon_();

  // 預設 PD 變數：即使使用者只放「PD前進」或「重設PD誤差」也能編譯
  Blockly.Arduino.addVariable(
    'pd_params_pd',
    'int L_BASE_PD = 180;\n' +
    'int R_BASE_PD = 180;\n' +
    'float KP_PD = 20;\n' +
    'float KD_PD = 8;\n' +
    'float PD_FILTER = 0.35;\n' +
    'int PD_MAX_CORRECTION = 120;\n' +
    'int PD_LOST_SPEED = 90;\n' +
    'int PD_LOST_LIMIT = 4;\n' +
    'bool PD_LOST_ENABLE = true; // true=丟線後自動尋黑線, false=丟線後停止\n' +
    'int PD_LOST_COUNT = 0;\n' +
    'int PD_BLACK_COUNT = 0;\n' +
    'int PD_LAST_DIR = 0; // -1=最後線在左側, 1=最後線在右側, 0=未知或接近中心\n' +
    'bool PD_WAS_LOST = false; // 曾進入丟線搜尋，找回線時用來避免 D 項爆衝\n' +
    'bool PD_DYNAMIC_ENABLE = false; // true=依誤差大小自動調整基礎速度\n' +
    'int PD_DYN_FAST_SPEED = 210; // 直線或接近中心時的速度\n' +
    'int PD_DYN_SLOW_SPEED = 140; // 彎道或偏移較大時的速度\n' +
    'float PD_DYN_CENTER_ERR = 1.0; // 小於等於此誤差視為中心，可加速\n' +
    'float PD_DYN_CURVE_ERR = 4.0; // 大於等於此誤差視為彎道，需降速\n' +
    'int PD_LAST_LEFT_SPEED = 0; // 上一次送給左輪的有號速度，正=前進、負=後退\n' +
    'int PD_LAST_RIGHT_SPEED = 0; // 上一次送給右輪的有號速度，正=前進、負=後退\n' +
    'float err_new_pd = 0;\n' +
    'float err_prev_pd = 0;\n' +
    'String blackPos_pd = "";\n',
    true
  );

  if (!Blockly.Arduino.definitions_['pd_funcs_guard']) {
    Blockly.Arduino.definitions_['pd_funcs_guard'] = ' ';

    Blockly.Arduino.addFunction(
      'pdReadAndBuildPos',
`void pdReadAndBuildPos() {
  eightLineRead();
  blackPos_pd =
    String(eight_X1)+String(eight_X2)+String(eight_X3)+String(eight_X4)+
    String(eight_X5)+String(eight_X6)+String(eight_X7)+String(eight_X8);
}`
    );

    Blockly.Arduino.addFunction(
      'pdPosToError',
`void pdPosToError() {
  int sum = 0;
  int cnt = 0;

  if (!EIGHT_READ_OK) {
    PD_BLACK_COUNT = 0;
    return;
  }

  if (eight_X1) { sum += eightLineSensorWeight(1); cnt++; }
  if (eight_X2) { sum += eightLineSensorWeight(2); cnt++; }
  if (eight_X3) { sum += eightLineSensorWeight(3); cnt++; }
  if (eight_X4) { sum += eightLineSensorWeight(4); cnt++; }
  if (eight_X5) { sum += eightLineSensorWeight(5); cnt++; }
  if (eight_X6) { sum += eightLineSensorWeight(6); cnt++; }
  if (eight_X7) { sum += eightLineSensorWeight(7); cnt++; }
  if (eight_X8) { sum += eightLineSensorWeight(8); cnt++; }

  PD_BLACK_COUNT = cnt;

  if (cnt > 0) {
    PD_LOST_COUNT = 0;

    // 使用 float 計算加權平均，避免整數除法造成誤差不連續
    float rawError = (float)sum / (float)cnt;
    rawError = rawError * (float)EIGHT_PD_SIGN;
    if (EIGHT_PD_DIV > 1) rawError = rawError / (float)EIGHT_PD_DIV;

    // 記錄最後一次黑線偏向；丟線時依此決定搜尋方向
    if (rawError < -0.2) PD_LAST_DIR = -1;
    else if (rawError > 0.2) PD_LAST_DIR = 1;

    // 濾波：PD_FILTER 越大，越相信新誤差；越小，越平滑但反應較慢
    PD_FILTER = constrain(PD_FILTER, 0.0, 1.0);
    err_new_pd = err_new_pd * (1.0 - PD_FILTER) + rawError * PD_FILTER;

    // 若剛從丟線搜尋回到黑線上，先同步前一次誤差，避免 D 項瞬間過大
    if (PD_WAS_LOST) {
      err_prev_pd = err_new_pd;
      PD_WAS_LOST = false;
    }
  }
}`
    );

    Blockly.Arduino.addFunction(
      'pdDriveSigned',
`void pdDriveSigned(int leftSpeed, int rightSpeed) {
  leftSpeed  = constrain(leftSpeed,  -255, 255);
  rightSpeed = constrain(rightSpeed, -255, 255);

  if (leftSpeed == 0) motor_left(2, 0);
  else if (leftSpeed > 0) motor_left(0, leftSpeed);
  else motor_left(1, -leftSpeed);

  if (rightSpeed == 0) motor_right(2, 0);
  else if (rightSpeed > 0) motor_right(0, rightSpeed);
  else motor_right(1, -rightSpeed);
}`
    );

    Blockly.Arduino.addFunction(
      'pdStepDrive',
`void pdStepDrive() {
  // v20 防誤觸：短暫全白先視為感測瞬間空窗，不立刻旋轉找線。
  // 只有連續全白達到 PD_LOST_LIMIT 後，才正式進入丟線處理。
  if (PD_BLACK_COUNT == 0) {
    PD_LOST_COUNT++;

    if (PD_LOST_COUNT < PD_LOST_LIMIT) {
      // 尚未正式丟線：沿用上一輪馬達輸出，避免正常循線時因瞬間全白而突然晃動。
      int holdLeft = PD_LAST_LEFT_SPEED;
      int holdRight = PD_LAST_RIGHT_SPEED;

      // 若一開始就全白、沒有上一輪輸出，則用較保守的直行速度嘗試通過感測空窗。
      if (holdLeft == 0 && holdRight == 0) {
        holdLeft = constrain((int)(L_BASE_PD * 0.70), 0, 255);
        holdRight = constrain((int)(R_BASE_PD * 0.70), 0, 255);
      }

      pdDriveSigned(holdLeft, holdRight);
      return;
    }

    int lostSpeed = constrain(PD_LOST_SPEED, 0, 255);
    PD_WAS_LOST = true;

    if (!PD_LOST_ENABLE) {
      // 關閉丟線尋黑線：確認丟線後直接停止，方便教學觀察或交給其他邏輯處理
      PD_LAST_LEFT_SPEED = 0;
      PD_LAST_RIGHT_SPEED = 0;
      motor_left(2, 0);
      motor_right(2, 0);
      return;
    }

    if (PD_LAST_DIR < 0) {
      // 最後一次線在左側：向左原地搜尋
      PD_LAST_LEFT_SPEED = -lostSpeed;
      PD_LAST_RIGHT_SPEED = lostSpeed;
      pdDriveSigned(PD_LAST_LEFT_SPEED, PD_LAST_RIGHT_SPEED);
    } else if (PD_LAST_DIR > 0) {
      // 最後一次線在右側：向右原地搜尋
      PD_LAST_LEFT_SPEED = lostSpeed;
      PD_LAST_RIGHT_SPEED = -lostSpeed;
      pdDriveSigned(PD_LAST_LEFT_SPEED, PD_LAST_RIGHT_SPEED);
    } else {
      // 尚未知道線在哪一側時，採預設右旋搜尋
      PD_LAST_LEFT_SPEED = lostSpeed;
      PD_LAST_RIGHT_SPEED = -lostSpeed;
      pdDriveSigned(PD_LAST_LEFT_SPEED, PD_LAST_RIGHT_SPEED);
    }
    return;
  }

  float pd = KP_PD * err_new_pd + KD_PD * (err_new_pd - err_prev_pd);
  err_prev_pd = err_new_pd;

  float maxCorr = (float)constrain(PD_MAX_CORRECTION, 0, 255);
  if (pd > maxCorr) pd = maxCorr;
  if (pd < -maxCorr) pd = -maxCorr;

  int leftBase = L_BASE_PD;
  int rightBase = R_BASE_PD;

  // v21 動態速度：誤差小代表接近中心，可提高基礎速度；誤差大代表彎道或偏移，降低基礎速度。
  // 若左右基礎速度不同，會保留左右速度差作為馬達補償。
  if (PD_DYNAMIC_ENABLE) {
    int fastSpeed = constrain(PD_DYN_FAST_SPEED, 0, 255);
    int slowSpeed = constrain(PD_DYN_SLOW_SPEED, 0, 255);

    float centerErr = PD_DYN_CENTER_ERR;
    float curveErr = PD_DYN_CURVE_ERR;
    if (centerErr < 0) centerErr = -centerErr;
    if (curveErr < 0) curveErr = -curveErr;
    if (curveErr <= centerErr) curveErr = centerErr + 0.1;

    float absErr = err_new_pd;
    if (absErr < 0) absErr = -absErr;

    float ratio = 0.0;
    if (absErr <= centerErr) {
      ratio = 0.0; // 使用直線速度
    } else if (absErr >= curveErr) {
      ratio = 1.0; // 使用彎道速度
    } else {
      ratio = (absErr - centerErr) / (curveErr - centerErr); // 平滑過渡
    }

    int dynBase = (int)((float)fastSpeed + ((float)slowSpeed - (float)fastSpeed) * ratio);
    dynBase = constrain(dynBase, 0, 255);

    int baseAvg = (L_BASE_PD + R_BASE_PD) / 2;
    leftBase = constrain(dynBase + (L_BASE_PD - baseAvg), 0, 255);
    rightBase = constrain(dynBase + (R_BASE_PD - baseAvg), 0, 255);
  }

  int leftSpeed  = leftBase + (int)pd;
  int rightSpeed = rightBase - (int)pd;

  leftSpeed  = constrain(leftSpeed,  -255, 255);
  rightSpeed = constrain(rightSpeed, -255, 255);

  // 記錄正常 PD 輸出，供短暫全白時平滑沿用
  PD_LAST_LEFT_SPEED = leftSpeed;
  PD_LAST_RIGHT_SPEED = rightSpeed;

  pdDriveSigned(leftSpeed, rightSpeed);
}`
    );
  }
}

Blockly.Arduino['block_line_pd_init'] = function(block) {
  ensurePdFunctions_();

  var lBaseField = block.getFieldValue('L_BASE');
  var rBaseField = block.getFieldValue('R_BASE');
  var lBaseExpr = (lBaseField !== null && lBaseField !== undefined && String(lBaseField).trim() !== '') ? lBaseField : (Blockly.Arduino.valueToCode(block, 'L_BASE', Blockly.Arduino.ORDER_ATOMIC) || '180');
  var rBaseExpr = (rBaseField !== null && rBaseField !== undefined && String(rBaseField).trim() !== '') ? rBaseField : (Blockly.Arduino.valueToCode(block, 'R_BASE', Blockly.Arduino.ORDER_ATOMIC) || '180');

  var kp = parseFloat(block.getFieldValue('KP') || '20');
  var kd = parseFloat(block.getFieldValue('KD') || '8');
  var filter = parseFloat(block.getFieldValue('FILTER') || '0.35');
  var maxCorrection = parseInt(block.getFieldValue('MAX_CORRECTION') || '120', 10);
  var lostEnable = (block.getFieldValue('LOST_ENABLE') || '1') === '1';
  var lostSpeed = parseInt(block.getFieldValue('LOST_SPEED') || '90', 10);
  var lostLimit = parseInt(block.getFieldValue('LOST_LIMIT') || '4', 10);

  if (isNaN(kp)) kp = 20;
  if (isNaN(kd)) kd = 8;
  if (isNaN(filter)) filter = 0.35;
  if (isNaN(maxCorrection)) maxCorrection = 120;
  if (isNaN(lostSpeed)) lostSpeed = 90;
  if (isNaN(lostLimit)) lostLimit = 4;

  var code = '';
  code += 'L_BASE_PD = constrain((int)(' + lBaseExpr + '), 0, 255);\n';
  code += 'R_BASE_PD = constrain((int)(' + rBaseExpr + '), 0, 255);\n';
  code += 'KP_PD = ' + kp.toFixed(2) + ';\n';
  code += 'KD_PD = ' + kd.toFixed(2) + ';\n';
  code += 'PD_FILTER = constrain(' + filter.toFixed(2) + ', 0.0, 1.0);\n';
  code += 'PD_MAX_CORRECTION = constrain(' + maxCorrection + ', 0, 255);\n';
  code += 'PD_LOST_ENABLE = ' + (lostEnable ? 'true' : 'false') + ';\n';
  code += 'PD_LOST_SPEED = constrain(' + lostSpeed + ', 0, 255);\n';
  code += 'PD_LOST_LIMIT = constrain(' + lostLimit + ', 1, 20);\n';
  code += 'err_new_pd = 0;\n';
  code += 'err_prev_pd = 0;\n';
  code += 'PD_LOST_COUNT = 0;\n';
  code += 'PD_BLACK_COUNT = 0;\n';
  code += 'PD_LAST_DIR = 0;\n';
  code += 'PD_WAS_LOST = false;\n';
  code += 'PD_LAST_LEFT_SPEED = 0;\n';
  code += 'PD_LAST_RIGHT_SPEED = 0;\n';

  return code;
};

goog.provide('Blockly.Arduino.user');
goog.require('Blockly.Arduino');

// ================================
// 重設PD誤差
// ================================
Blockly.Arduino['pd_reset_error'] = function(block) {
  ensurePdFunctions_();
  var code = ''
    + 'err_new_pd = 0;\n'
    + 'err_prev_pd = 0;\n'
    + 'PD_LOST_COUNT = 0;\n'
    + 'PD_BLACK_COUNT = 0;\n'
    + 'PD_LAST_DIR = 0;\n'
    + 'PD_WAS_LOST = false;\n'
    + 'PD_LAST_LEFT_SPEED = 0;\n'
    + 'PD_LAST_RIGHT_SPEED = 0;\n';
  return code;
};

Blockly.Arduino['block_line_pd_forward'] = function(block) {
  ensurePdFunctions_();
  var code =
    'pdReadAndBuildPos();\n' +
    'pdPosToError();\n' +
    'pdStepDrive();\n';
  return code;
};





// ================================
// ICM42688 陀螺儀角度感測器
// ================================
function ensureICM42688Common_() {
  Blockly.Arduino.addInclude('include_wire', '#include <Wire.h>');

  if (!Blockly.Arduino.definitions_['icm42688_vars_once']) {
    Blockly.Arduino.definitions_['icm42688_vars_once'] =
      'uint8_t ICM42688_I2C_ADDR = 0x68;\n' +
      'const uint8_t ICM42688_REG_DEVICE_CONFIG = 0x11;\n' +
      'const uint8_t ICM42688_REG_PWR_MGMT0     = 0x4E;\n' +
      'const uint8_t ICM42688_REG_GYRO_CONFIG0  = 0x4F;\n' +
      'const uint8_t ICM42688_REG_ACCEL_CONFIG0 = 0x50;\n' +
      'const uint8_t ICM42688_REG_WHO_AM_I      = 0x75;\n' +
      'const uint8_t ICM42688_REG_BANK_SEL      = 0x76;\n' +
      'const uint8_t ICM42688_REG_GYRO_X1       = 0x25;\n' +
      'const float ICM42688_GYRO_SCALE = 1000.0 / 32768.0; // Gyro 設為 ±1000 dps\n' +
      'const float ICM42688_GYRO_DEADBAND = 0.30;\n' +
      'float icm42688_gyro_z_offset = 0.0;\n' +
      'float icm42688_angle_z = 0.0;\n' +
      'int16_t icm42688_gx_raw = 0;\n' +
      'int16_t icm42688_gy_raw = 0;\n' +
      'int16_t icm42688_gz_raw = 0;\n' +
      'unsigned long icm42688_last_micros = 0;\n';
  }

  Blockly.Arduino.setups_['icm42688_wire_begin_once'] = 'Wire.begin();\nWire.setClock(100000);\n';

  if (!Blockly.Arduino.definitions_['icm42688_funcs_guard']) {
    Blockly.Arduino.definitions_['icm42688_funcs_guard'] = ' ';

    Blockly.Arduino.addFunction(
      'icm42688WriteReg',
`void icm42688WriteReg(uint8_t reg, uint8_t value) {
  Wire.beginTransmission((uint8_t)ICM42688_I2C_ADDR);
  Wire.write(reg);
  Wire.write(value);
  Wire.endTransmission();
}`
    );

    Blockly.Arduino.addFunction(
      'icm42688ReadReg',
`uint8_t icm42688ReadReg(uint8_t reg) {
  Wire.beginTransmission((uint8_t)ICM42688_I2C_ADDR);
  Wire.write(reg);
  Wire.endTransmission(false);

  Wire.requestFrom((uint8_t)ICM42688_I2C_ADDR, (uint8_t)1);
  if (Wire.available()) return Wire.read();
  return 0xFF;
}`
    );

    Blockly.Arduino.addFunction(
      'icm42688ReadRegs',
`void icm42688ReadRegs(uint8_t reg, uint8_t *buf, uint8_t len) {
  Wire.beginTransmission((uint8_t)ICM42688_I2C_ADDR);
  Wire.write(reg);
  Wire.endTransmission(false);

  Wire.requestFrom((uint8_t)ICM42688_I2C_ADDR, (uint8_t)len);
  for (uint8_t i = 0; i < len; i++) {
    if (Wire.available()) buf[i] = Wire.read();
    else buf[i] = 0;
  }
}`
    );

    Blockly.Arduino.addFunction(
      'icm42688ToInt16',
`int16_t icm42688ToInt16(uint8_t highByte, uint8_t lowByte) {
  return (int16_t)((highByte << 8) | lowByte);
}`
    );

    Blockly.Arduino.addFunction(
      'icm42688ReadGyro',
`void icm42688ReadGyro() {
  uint8_t buf[6];
  icm42688ReadRegs(ICM42688_REG_GYRO_X1, buf, 6);
  icm42688_gx_raw = icm42688ToInt16(buf[0], buf[1]);
  icm42688_gy_raw = icm42688ToInt16(buf[2], buf[3]);
  icm42688_gz_raw = icm42688ToInt16(buf[4], buf[5]);
}`
    );

    Blockly.Arduino.addFunction(
      'icm42688Init',
`void icm42688Init(uint8_t addr) {
  ICM42688_I2C_ADDR = addr;

  Wire.begin();
  Wire.setClock(100000);

  // 切回 Bank 0
  icm42688WriteReg(ICM42688_REG_BANK_SEL, 0x00);
  delay(10);

  // 軟體重置
  icm42688WriteReg(ICM42688_REG_DEVICE_CONFIG, 0x01);
  delay(100);

  // 重置後再切回 Bank 0
  icm42688WriteReg(ICM42688_REG_BANK_SEL, 0x00);
  delay(10);

  // Accel：±4g, 100Hz
  icm42688WriteReg(ICM42688_REG_ACCEL_CONFIG0, 0x48);
  delay(10);

  // Gyro：±1000 dps, 100Hz
  icm42688WriteReg(ICM42688_REG_GYRO_CONFIG0, 0x28);
  delay(10);

  // 啟用 Gyro + Accel Low Noise Mode
  icm42688WriteReg(ICM42688_REG_PWR_MGMT0, 0x0F);
  delay(100);

  icm42688_angle_z = 0.0;
  icm42688_last_micros = micros();
}`
    );

    Blockly.Arduino.addFunction(
      'icm42688CalibrateGyroZ',
`void icm42688CalibrateGyroZ(int samples, int intervalMs) {
  if (samples < 1) samples = 1;
  if (intervalMs < 1) intervalMs = 1;

  float sum = 0.0;
  delay(300);

  for (int i = 0; i < samples; i++) {
    icm42688ReadGyro();
    float gz = icm42688_gz_raw * ICM42688_GYRO_SCALE;
    sum += gz;
    delay(intervalMs);
  }

  icm42688_gyro_z_offset = sum / samples;
  icm42688_angle_z = 0.0;
  icm42688_last_micros = micros();
}`
    );

    Blockly.Arduino.addFunction(
      'icm42688ResetAngleZ',
`void icm42688ResetAngleZ() {
  icm42688_angle_z = 0.0;
  icm42688_last_micros = micros();
}`
    );

    Blockly.Arduino.addFunction(
      'icm42688UpdateAngleZ',
`void icm42688UpdateAngleZ() {
  icm42688ReadGyro();

  unsigned long nowMicros = micros();
  float dt = (nowMicros - icm42688_last_micros) / 1000000.0;
  icm42688_last_micros = nowMicros;

  float gz = icm42688_gz_raw * ICM42688_GYRO_SCALE;
  float gzCorrected = gz - icm42688_gyro_z_offset;

  if (gzCorrected < ICM42688_GYRO_DEADBAND && gzCorrected > -ICM42688_GYRO_DEADBAND) {
    gzCorrected = 0.0;
  }

  icm42688_angle_z += gzCorrected * dt;
}`
    );

    Blockly.Arduino.addFunction(
      'icm42688GetAngleZ',
`float icm42688GetAngleZ() {
  return icm42688_angle_z;
}`
    );

    Blockly.Arduino.addFunction(
      'icm42688GetAngleZAuto',
`float icm42688GetAngleZAuto() {
  icm42688UpdateAngleZ();
  return icm42688_angle_z;
}`
    );

    Blockly.Arduino.addFunction(
      'icm42688AngleZReachedAuto',
`bool icm42688AngleZReachedAuto(float targetDeg, float toleranceDeg) {
  icm42688UpdateAngleZ();

  if (targetDeg < 0) targetDeg = -targetDeg;
  if (toleranceDeg < 0) toleranceDeg = -toleranceDeg;

  float a = icm42688_angle_z;
  if (a < 0) a = -a;

  return a >= (targetDeg - toleranceDeg);
}`
    );
  }
}

Blockly.Arduino['icm42688_init'] = function(block) {
  ensureICM42688Common_();
  var addr = block.getFieldValue('ADDR') || '0x68';
  return 'icm42688Init(' + addr + ');\n';
};

Blockly.Arduino['icm42688_calibrate_gyro_z'] = function(block) {
  ensureICM42688Common_();
  var samples = parseInt(block.getFieldValue('SAMPLES') || '300', 10);
  var interval = parseInt(block.getFieldValue('INTERVAL') || '10', 10);
  if (isNaN(samples)) samples = 300;
  if (isNaN(interval)) interval = 10;
  samples = Math.max(1, Math.min(2000, samples));
  interval = Math.max(1, Math.min(100, interval));
  return 'icm42688CalibrateGyroZ(' + samples + ', ' + interval + ');\n';
};

Blockly.Arduino['icm42688_reset_angle_z'] = function(block) {
  ensureICM42688Common_();
  return 'icm42688ResetAngleZ();\n';
};

Blockly.Arduino['icm42688_update_angle_z'] = function(block) {
  ensureICM42688Common_();
  return 'icm42688UpdateAngleZ();\n';
};

Blockly.Arduino['icm42688_get_angle_z'] = function(block) {
  ensureICM42688Common_();
  return ['icm42688GetAngleZAuto()', Blockly.Arduino.ORDER_ATOMIC];
};

Blockly.Arduino['icm42688_angle_z_reached'] = function(block) {
  ensureICM42688Common_();
  var target = parseFloat(block.getFieldValue('TARGET') || '90');
  var tolerance = parseFloat(block.getFieldValue('TOLERANCE') || '3');
  if (isNaN(target)) target = 90;
  if (isNaN(tolerance)) tolerance = 3;
  if (target < 0) target = -target;
  tolerance = Math.max(0, Math.min(45, tolerance));
  var code = 'icm42688AngleZReachedAuto(' + target.toFixed(2) + ', ' + tolerance.toFixed(2) + ')';
  return [code, Blockly.Arduino.ORDER_ATOMIC];
};

// ================================
// TCS34725 顏色感測模組
// ================================

'use strict';

Blockly.Arduino = Blockly.Arduino || {};

// ------------------------------------------------------------
// Common ensure helpers
// ------------------------------------------------------------
function ensureTcsIncludesAndGlobals_() {
  Blockly.Arduino.addInclude('include_wire', '#include <Wire.h>');
  Blockly.Arduino.addInclude('include_tcs', '#include "Adafruit_TCS34725.h"');

  // default tcs obj (if user never drops init block)
  if (!Blockly.Arduino.definitions_['define_tcs_obj']) {
    Blockly.Arduino.definitions_['define_tcs_obj'] =
      'Adafruit_TCS34725 tcs = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_50MS, TCS34725_GAIN_4X);\n';
  }

  // centroid storage (max 10)
  if (!Blockly.Arduino.definitions_['define_centroid_globals']) {
    Blockly.Arduino.definitions_['define_centroid_globals'] =
      '#define CENTROID_MAX 10\n' +
      'uint16_t g_centR1k[CENTROID_MAX];\n' +
      'uint16_t g_centG1k[CENTROID_MAX];\n' +
      'uint16_t g_centB1k[CENTROID_MAX];\n' +
      'uint8_t  g_centUsed[CENTROID_MAX];\n' +
      'uint32_t g_dist_th = 2000; // 預設平方距離門檻（可用積木改）\n';
  }
}

function ensureWaitSerialY_() {
  if (!Blockly.Arduino.definitions_['define_wait_y']) {
    Blockly.Arduino.definitions_['define_wait_y'] =
      'void wait_serial_y(){\n' +
      '  while (Serial.available() > 0) Serial.read();\n' +
      '  Serial.println("輸入 y 後開始...");\n' +
      '  while (true) {\n' +
      '    if (Serial.available() > 0) {\n' +
      '      char ch = (char)Serial.read();\n' +
      '      if (ch == \'y\' || ch == \'Y\') break;\n' +
      '    }\n' +
      '  }\n' +
      '}\n';
  }
}

function ensureMeasureAvg_() {
  if (!Blockly.Arduino.definitions_['define_measure_avg']) {
    Blockly.Arduino.definitions_['define_measure_avg'] =
      'void tcs_measure_avg(uint16_t *outR, uint16_t *outG, uint16_t *outB, uint16_t *outC,\n' +
      '                     uint8_t n, uint16_t dtMs, bool ledOn){\n' +
      '  if (n == 0) n = 1;\n' +
      '  uint32_t rs=0, gs=0, bs=0, cs=0;\n' +
      '  uint16_t r,g,b,c;\n' +
      '  if (ledOn) tcs.setInterrupt(false);\n' +
      '  delay(60);\n' +
      '  for (uint8_t i=0;i<n;i++){\n' +
      '    tcs.getRawData(&r,&g,&b,&c);\n' +
      '    rs+=r; gs+=g; bs+=b; cs+=c;\n' +
      '    delay(dtMs);\n' +
      '  }\n' +
      '  if (ledOn) tcs.setInterrupt(true);\n' +
      '  *outR = (uint16_t)(rs/n);\n' +
      '  *outG = (uint16_t)(gs/n);\n' +
      '  *outB = (uint16_t)(bs/n);\n' +
      '  *outC = (uint16_t)(cs/n);\n' +
      '}\n';
  }
}

function ensureCompute1k_() {
  if (!Blockly.Arduino.definitions_['define_compute_1k']) {
    Blockly.Arduino.definitions_['define_compute_1k'] =
      'void tcs_compute_1k(uint16_t rAvg, uint16_t gAvg, uint16_t bAvg, uint16_t cAvg,\n' +
      '                    uint16_t *R1k, uint16_t *G1k, uint16_t *B1k){\n' +
      '  if (cAvg == 0) { *R1k=0; *G1k=0; *B1k=0; return; }\n' +
      '  *R1k = (uint32_t)rAvg * 1000 / cAvg;\n' +
      '  *G1k = (uint32_t)gAvg * 1000 / cAvg;\n' +
      '  *B1k = (uint32_t)bAvg * 1000 / cAvg;\n' +
      '}\n';
  }
}

function ensureCentroidHelpers_() {
  if (!Blockly.Arduino.definitions_['define_centroid_set']) {
    Blockly.Arduino.definitions_['define_centroid_set'] =
      'void centroid_set(uint8_t id1, uint16_t R1k, uint16_t G1k, uint16_t B1k){\n' +
      '  if (id1 < 1 || id1 > CENTROID_MAX) return;\n' +
      '  uint8_t i = id1 - 1;\n' +
      '  g_centR1k[i] = R1k;\n' +
      '  g_centG1k[i] = G1k;\n' +
      '  g_centB1k[i] = B1k;\n' +
      '  g_centUsed[i] = 1;\n' +
      '}\n';
  }

  if (!Blockly.Arduino.definitions_['define_centroid_classify']) {
    Blockly.Arduino.definitions_['define_centroid_classify'] =
      'uint8_t centroid_classify(uint16_t R1k, uint16_t G1k, uint16_t B1k, uint32_t *bestDistOut){\n' +
      '  uint8_t bestId = 0;\n' +
      '  uint32_t bestD = 0xFFFFFFFFUL;\n' +
      '  for (uint8_t i=0;i<CENTROID_MAX;i++){\n' +
      '    if (!g_centUsed[i]) continue;\n' +
      '    int32_t dR = (int32_t)R1k - (int32_t)g_centR1k[i];\n' +
      '    int32_t dG = (int32_t)G1k - (int32_t)g_centG1k[i];\n' +
      '    int32_t dB = (int32_t)B1k - (int32_t)g_centB1k[i];\n' +
      '    uint32_t d = (uint32_t)(dR*dR) + (uint32_t)(dG*dG) + (uint32_t)(dB*dB);\n' +
      '    if (d < bestD) { bestD = d; bestId = (uint8_t)(i+1); }\n' +
      '  }\n' +
      '  if (bestDistOut) *bestDistOut = bestD;\n' +
      '  if (bestId == 0) return 0;        // 未設定任何中心點\n' +
      '  if (bestD > g_dist_th) return 0;  // 超出距離門檻\n' +
      '  return bestId;\n' +
      '}\n';
  }
}

function ensureMeasureAndClassifyFn_() {
  if (!Blockly.Arduino.definitions_['define_measure_and_classify']) {
    Blockly.Arduino.definitions_['define_measure_and_classify'] =
      'uint8_t measure_and_classify(uint8_t n, uint16_t dtMs, bool ledOn, bool waitY, bool printFeat, bool printCode){\n' +
      '  if (waitY) wait_serial_y();\n' +
      '  uint16_t rAvg,gAvg,bAvg,cAvg;\n' +
      '  tcs_measure_avg(&rAvg,&gAvg,&bAvg,&cAvg,n,dtMs,ledOn);\n' +
      '  uint16_t R1k,G1k,B1k;\n' +
      '  tcs_compute_1k(rAvg,gAvg,bAvg,cAvg,&R1k,&G1k,&B1k);\n' +
      '  uint32_t bestD=0;\n' +
      '  uint8_t code = centroid_classify(R1k,G1k,B1k,&bestD);\n' +
      '  if (printFeat){\n' +
      '    Serial.print("avgR="); Serial.print(rAvg);\n' +
      '    Serial.print(",avgG="); Serial.print(gAvg);\n' +
      '    Serial.print(",avgB="); Serial.print(bAvg);\n' +
      '    Serial.print(",avgC="); Serial.print(cAvg);\n' +
      '    Serial.print(",R1k="); Serial.print(R1k);\n' +
      '    Serial.print(",G1k="); Serial.print(G1k);\n' +
      '    Serial.print(",B1k="); Serial.print(B1k);\n' +
      '    Serial.print(",bestD="); Serial.println(bestD);\n' +
      '  }\n' +
      '  if (printCode){\n' +
      '    Serial.print("code="); Serial.println(code);\n' +
      '  }\n' +
      '  return code;\n' +
      '}\n';
  }
}

// ------------------------------------------------------------
// Block generators
// ------------------------------------------------------------

// 1) 初始化（statement，不檢測）
Blockly.Arduino['tcs34725_init'] = function(block) {
  // includes
  Blockly.Arduino.addInclude('include_wire', '#include <Wire.h>');
  Blockly.Arduino.addInclude('include_tcs', '#include "Adafruit_TCS34725.h"');

  // fields (保底預設)
  var it   = block.getFieldValue('IT')   || 'TCS34725_INTEGRATIONTIME_50MS';
  var gain = block.getFieldValue('GAIN') || 'TCS34725_GAIN_4X';
  var baud = Number(block.getFieldValue('BAUD')) || 9600;

  // define tcs object with chosen IT/Gain
  Blockly.Arduino.definitions_['define_tcs_obj'] =
    'Adafruit_TCS34725 tcs = Adafruit_TCS34725(' + it + ', ' + gain + ');\n';

  // ensure centroid globals exist
  ensureTcsIncludesAndGlobals_();

  // setup serial (only once)
  Blockly.Arduino.setups_['setup_serial_begin'] =
    'Serial.begin(' + baud + ');\n' +
    'delay(200);\n';

  // no-check begin
  return 'tcs.begin();\n';
};

// 2) 量測並輸出特徵（CSV）
Blockly.Arduino['tcs34725_measure_print_features'] = function(block) {
  ensureTcsIncludesAndGlobals_();
  ensureWaitSerialY_();
  ensureMeasureAvg_();
  ensureCompute1k_();

  var label = block.getFieldValue('LABEL') || 'item';
  label = label.replace(/\\/g, '\\\\').replace(/"/g, '\\"');

  // 讀內嵌數字欄位
  var n     = Number(block.getFieldValue('N'));
  var dt    = Number(block.getFieldValue('DT'));
  if (!isFinite(n)) n = 10;
  if (!isFinite(dt)) dt = 20;
  n = Math.max(1, Math.min(255, Math.floor(n)));
  dt = Math.max(0, Math.min(65535, Math.floor(dt)));
  var waitY = block.getFieldValue('WAIT_Y') || '0'; // "1"/"0"

  var code = '';
  code += '{\n';
  code += '  if (' + waitY + ') wait_serial_y();\n';
  code += '  uint16_t rAvg,gAvg,bAvg,cAvg;\n';
  code += '  tcs_measure_avg(&rAvg,&gAvg,&bAvg,&cAvg,(uint8_t)(' + n + '),(uint16_t)(' + dt + '), true);\n';
  code += '  uint16_t R1k,G1k,B1k;\n';
  code += '  tcs_compute_1k(rAvg,gAvg,bAvg,cAvg,&R1k,&G1k,&B1k);\n';
  code += '  Serial.print("' + label + '"); Serial.print(",");\n';
  code += '  Serial.print(rAvg); Serial.print(",");\n';
  code += '  Serial.print(gAvg); Serial.print(",");\n';
  code += '  Serial.print(bAvg); Serial.print(",");\n';
  code += '  Serial.print(cAvg); Serial.print(",");\n';
  code += '  Serial.print(R1k); Serial.print(",");\n';
  code += '  Serial.print(G1k); Serial.print(",");\n';
  code += '  Serial.println(B1k);\n';
  code += '}\n';
  return code;
};

// 3) 設定中心點
Blockly.Arduino['centroid_set_target'] = function(block) {
  ensureTcsIncludesAndGlobals_();
  ensureCentroidHelpers_();

  var id = Number(block.getFieldValue('ID')) || 1;
  var r1k = Blockly.Arduino.valueToCode(block, 'R1K', Blockly.Arduino.ORDER_NONE) || '0';
  var g1k = Blockly.Arduino.valueToCode(block, 'G1K', Blockly.Arduino.ORDER_NONE) || '0';
  var b1k = Blockly.Arduino.valueToCode(block, 'B1K', Blockly.Arduino.ORDER_NONE) || '0';
  var ps  = block.getFieldValue('PRINT_SET') || '0'; // "1"/"0"

  var code = '';
  code += 'centroid_set(' + id + ', (uint16_t)(' + r1k + '), (uint16_t)(' + g1k + '), (uint16_t)(' + b1k + '));\n';
  code += 'if (' + ps + ') {\n';
  code += '  Serial.print("set id="); Serial.print(' + id + ');\n';
  code += '  Serial.print(" R1k="); Serial.print((uint16_t)(' + r1k + '));\n';
  code += '  Serial.print(" G1k="); Serial.print((uint16_t)(' + g1k + '));\n';
  code += '  Serial.print(" B1k="); Serial.println((uint16_t)(' + b1k + '));\n';
  code += '}\n';
  return code;
};

// 4) 設定距離門檻
Blockly.Arduino['centroid_set_dist_threshold'] = function(block) {
  ensureTcsIncludesAndGlobals_();

  var th = Blockly.Arduino.valueToCode(block, 'TH', Blockly.Arduino.ORDER_NONE) || '2000';
  var pt = block.getFieldValue('PRINT_TH') || '0'; // "1"/"0"

  var code = '';
  code += 'g_dist_th = (uint32_t)(' + th + ');\n';
  code += 'if (' + pt + ') { Serial.print("dist_th="); Serial.println(g_dist_th); }\n';
  return code;
};

// 5) 量測並辨識（回傳代碼）
Blockly.Arduino['centroid_classify_measure_code'] = function(block) {
  ensureTcsIncludesAndGlobals_();
  ensureWaitSerialY_();
  ensureMeasureAvg_();
  ensureCompute1k_();
  ensureCentroidHelpers_();
  ensureMeasureAndClassifyFn_();

  var n      = Blockly.Arduino.valueToCode(block, 'N',  Blockly.Arduino.ORDER_NONE) || '7';
  var dt     = Blockly.Arduino.valueToCode(block, 'DT', Blockly.Arduino.ORDER_NONE) || '20';
  var waitY  = block.getFieldValue('WAIT_Y') || '0';      // "1"/"0"
  var pFeat  = block.getFieldValue('PRINT_FEAT') || '0';  // "1"/"0"
  var pCode  = block.getFieldValue('PRINT_CODE') || '0';  // "1"/"0"

  // ledOn 固定 true
  var code = 'measure_and_classify((uint8_t)(' + n + '), (uint16_t)(' + dt + '), true, (' + waitY + '), (' + pFeat + '), (' + pCode + '))';
  return [code, Blockly.Arduino.ORDER_ATOMIC];
};

/* Removed from the maintained toolbox; use the normal expression blocks.
Blockly.Arduino['paren_expr'] = function(block) {
  var expr = Blockly.Arduino.valueToCode(block, 'EXPR', Blockly.Arduino.ORDER_NONE) || '0';
  return ['(' + expr + ')', Blockly.Arduino.ORDER_ATOMIC];
};
*/

Blockly.Arduino['servo270_attach'] = function (block) {
    var pinKey = block.getFieldValue('SERVO_PIN');
    var servoName = block.getFieldValue('SERVO_NAME');
    var servoId = Blockly.Arduino.variableDB_.getName(
        servoName,
        Blockly.Variables.NAME_TYPE);

    Blockly.Arduino.reservePin(block, pinKey, Blockly.Arduino.pinTypes.SERVO, 'Servo270 Write');
    Blockly.Arduino.addInclude('Servo_inc', '#include <Servo.h>');
    Blockly.Arduino.addVariable(servoName, 'Servo ' + servoId + ';', true);

    var code = servoId + '.attach(' + pinKey + ');\n';
    return code;
};

Blockly.Arduino['servo270_read'] = function (block) {
    var servoName = block.getFieldValue('SERVO_NAME');
    var servoId = Blockly.Arduino.variableDB_.getName(
        servoName,
        Blockly.Variables.NAME_TYPE);

    Blockly.Arduino.addInclude('Servo_inc', '#include <Servo.h>');
    Blockly.Arduino.addVariable(servoName, 'Servo ' + servoId + ';', true);

    var code = servoId + '.read()';
    return [code, Blockly.Arduino.ORDER_NONE];
};

Blockly.Arduino['servo270_write'] = function (block) {
    var servoName = block.getFieldValue('SERVO_NAME');
    var servoAngle = Blockly.Arduino.valueToCode(
        block, 'SERVO_ANGLE', Blockly.Arduino.ORDER_ATOMIC) || '0';
    var servoId = Blockly.Arduino.variableDB_.getName(
        servoName,
        Blockly.Variables.NAME_TYPE);

    Blockly.Arduino.addInclude('Servo_inc', '#include <Servo.h>');
    Blockly.Arduino.addVariable(servoName, 'Servo ' + servoId + ';', true);

    var code = servoId + '.write(map(constrain(' + servoAngle + ', 0, 270), 0, 270, 0, 180));\n';
    return code;
};

Blockly.Arduino['servo270_detach'] = function (block) {
    var servoName = block.getFieldValue('SERVO_NAME');
    var servoId = Blockly.Arduino.variableDB_.getName(
        servoName,
        Blockly.Variables.NAME_TYPE);

    Blockly.Arduino.addInclude('Servo_inc', '#include <Servo.h>');
    Blockly.Arduino.addVariable(servoName, 'Servo ' + servoId + ';', true);

    var code = servoId + '.detach();\n';
    return code;
};
