// ESP32-S3 Robot addon
//
// Blocks: move(left, right)  — set both motors -255..255
//         distance left      — sonar distance in cm (value)
//         distance right     — sonar distance in cm (value)
//
// ─── CONFIGURE YOUR ROBOT ───────────────────────────────────
// Edit the pin numbers below to match your wiring, then re-install
// the addon (Addons → Remove "esp32s3-robot-addon" → Install same file again).

var LEFT_MOTOR_PIN1 = 3;
var LEFT_MOTOR_PIN2 = 4;
var RIGHT_MOTOR_PIN1 = 5;
var RIGHT_MOTOR_PIN2 = 6;
var LEFT_SONAR_TRIG = 9;
var LEFT_SONAR_ECHO = 8;
var RIGHT_SONAR_TRIG = 11;
var RIGHT_SONAR_ECHO = 10;

// ─── Block definitions ───────────────────────────────────────

api.Blockly.Blocks['robot_move'] = {
  init() {
    this.appendValueInput('LEFT')
      .setCheck('Number')
      .appendField('move left');
    this.appendValueInput('RIGHT')
      .setCheck('Number')
      .appendField('right');
    this.setInputsInline(true);
    this.setPreviousStatement(true, null);
    this.setNextStatement(true, null);
    this.setColour(45);
    this.setTooltip('Set left and right motor speed (-255 to 255). DRV8833 on pins 3,4 and 5,6.');
  },
};

api.Blockly.Blocks['robot_distance_left'] = {
  init() {
    this.appendDummyInput()
      .appendField('distance left (cm)');
    this.setOutput(true, 'Number');
    this.setColour(65);
    this.setTooltip('Left sonar distance in cm (trig=9, echo=8)');
  },
};

api.Blockly.Blocks['robot_distance_right'] = {
  init() {
    this.appendDummyInput()
      .appendField('distance right (cm)');
    this.setOutput(true, 'Number');
    this.setColour(65);
    this.setTooltip('Right sonar distance in cm (trig=11, echo=10)');
  },
};

// --- Code generators ---

var gen = api.pythonGenerator;

// Generated Python pin constants (from config at top of this file).
var ROBOT_PINS = [
  '# Robot pin configuration',
  'LEFT_MOTOR_PIN1 = ' + LEFT_MOTOR_PIN1,
  'LEFT_MOTOR_PIN2 = ' + LEFT_MOTOR_PIN2,
  'RIGHT_MOTOR_PIN1 = ' + RIGHT_MOTOR_PIN1,
  'RIGHT_MOTOR_PIN2 = ' + RIGHT_MOTOR_PIN2,
  'LEFT_SONAR_TRIG = ' + LEFT_SONAR_TRIG,
  'LEFT_SONAR_ECHO = ' + LEFT_SONAR_ECHO,
  'RIGHT_SONAR_TRIG = ' + RIGHT_SONAR_TRIG,
  'RIGHT_SONAR_ECHO = ' + RIGHT_SONAR_ECHO,
].join('\n');

var ROBOT_MOTOR_DEF = [
  'from machine import Pin, PWM',
  '_robot_left1 = PWM(Pin(LEFT_MOTOR_PIN1), freq=20000, duty=0)',
  '_robot_left2 = PWM(Pin(LEFT_MOTOR_PIN2), freq=20000, duty=0)',
  '_robot_right1 = PWM(Pin(RIGHT_MOTOR_PIN1), freq=20000, duty=0)',
  '_robot_right2 = PWM(Pin(RIGHT_MOTOR_PIN2), freq=20000, duty=0)',
  'def _robot_set_motor(pwm1, pwm2, speed):',
  '    speed = max(-255, min(255, int(speed)))',
  '    duty = abs(speed) * 4',
  '    if speed > 0:',
  '        pwm1.duty(duty)',
  '        pwm2.duty(0)',
  '    elif speed < 0:',
  '        pwm1.duty(0)',
  '        pwm2.duty(duty)',
  '    else:',
  '        pwm1.duty(0)',
  '        pwm2.duty(0)',
].join('\n');

var ROBOT_SONAR_DEF = [
  'def _robot_sonar_cm(trig, echo):',
  '    from machine import Pin, time_pulse_us',
  '    import time',
  '    t = Pin(trig, Pin.OUT)',
  '    e = Pin(echo, Pin.IN)',
  '    t.off()',
  '    time.sleep_us(2)',
  '    t.on()',
  '    time.sleep_us(10)',
  '    t.off()',
  '    d = time_pulse_us(e, 1, 30000)',
  '    return round(d / 58.0, 1) if d >= 0 else -1',
].join('\n');

function addRobotDefs() {
  gen.definitions_['robot_pins'] = ROBOT_PINS;
  gen.definitions_['robot_motors'] = ROBOT_MOTOR_DEF;
  gen.definitions_['robot_sonar'] = ROBOT_SONAR_DEF;
}

gen.forBlock['robot_move'] = function (block) {
  if (api.getDeviceId() !== 'esp32s3') {
    return '# Robot move block is for ESP32-S3 robot (DRV8833 on 3,4,5,6)\n';
  }
  addRobotDefs();
  var left = gen.valueToCode(block, 'LEFT', gen.ORDER_NONE) || '0';
  var right = gen.valueToCode(block, 'RIGHT', gen.ORDER_NONE) || '0';
  return '_robot_set_motor(_robot_left1, _robot_left2, ' + left + ')\n' +
         '_robot_set_motor(_robot_right1, _robot_right2, ' + right + ')\n';
};

gen.forBlock['robot_distance_left'] = function (block) {
  if (api.getDeviceId() !== 'esp32s3') {
    return ['-1', gen.ORDER_ATOMIC];
  }
  addRobotDefs();
  return ['_robot_sonar_cm(LEFT_SONAR_TRIG, LEFT_SONAR_ECHO)', gen.ORDER_FUNCTION_CALL];
};

gen.forBlock['robot_distance_right'] = function (block) {
  if (api.getDeviceId() !== 'esp32s3') {
    return ['-1', gen.ORDER_ATOMIC];
  }
  addRobotDefs();
  return ['_robot_sonar_cm(RIGHT_SONAR_TRIG, RIGHT_SONAR_ECHO)', gen.ORDER_FUNCTION_CALL];
};

// --- Register toolbox category ---

api.registerCategories([
  {
    kind: 'category',
    name: 'ESP32-S3 Robot',
    colour: '45',
    contents: [
      {
        kind: 'block',
        type: 'robot_move',
        inputs: {
          LEFT: { shadow: { type: 'math_number', fields: { NUM: 0 } } },
          RIGHT: { shadow: { type: 'math_number', fields: { NUM: 0 } } },
        },
      },
      { kind: 'block', type: 'robot_distance_left' },
      { kind: 'block', type: 'robot_distance_right' },
    ],
  },
]);