const { AgenticNode } = require('agentic-robotics');

async function main() {
  // Create a robot node
  const robot = new AgenticNode('my-first-robot');
  console.log('🤖 Robot initialized!');

  // Create sensor publisher
  const sensorPub = await robot.createPublisher('/sensors/temperature');

  // Create command subscriber
  const commandSub = await robot.createSubscriber('/commands');

  // Listen for commands
  await commandSub.subscribe((message) => {
    const cmd = JSON.parse(message);
    console.log('📥 Received command:', cmd);

    if (cmd.action === 'read_sensor') {
      // Simulate sensor reading
      const reading = {
        value: 20 + Math.random() * 10,
        unit: 'celsius',
        timestamp: Date.now()
      };

      sensorPub.publish(JSON.stringify(reading));
      console.log('🌡️  Published sensor reading:', reading);
    }
  });

  console.log('✅ Robot ready! Listening for commands on /commands');
  console.log('💡 Tip: Use the MCP server to control with AI!');
}

main().catch(console.error);

const { AgenticNode } = require('agentic-robotics');

class DeliveryRobot {
  constructor(name) {
    this.node = new AgenticNode(name);
    this.currentPosition = { x: 0, y: 0, z: 0 };
    this.deliveries = [];
  }

  async initialize() {
    // Create control publisher (velocity commands)
    this.controlPub = await this.node.createPublisher('/cmd_vel');

    // Create position publisher (for tracking)
    this.posePub = await this.node.createPublisher('/robot/pose');

    // Subscribe to lidar data
    this.lidarSub = await this.node.createSubscriber('/sensors/lidar');
    await this.lidarSub.subscribe(this.handleLidarData.bind(this));

    // Subscribe to delivery commands
    this.deliverySub = await this.node.createSubscriber('/commands/deliver');
    await this.deliverySub.subscribe(this.handleDelivery.bind(this));

    console.log('✅ Delivery robot initialized');
  }

  handleLidarData(message) {
    const lidar = JSON.parse(message);
    // Check for obstacles
    const minDistance = Math.min(...lidar.ranges);

    if (minDistance < 0.5) {
      console.log('⚠️  Obstacle detected! Stopping...');
      this.stop();
    }
  }

  async handleDelivery(message) {
    const delivery = JSON.parse(message);
    console.log(`📦 New delivery: ${delivery.item} to ${delivery.location}`);

    this.deliveries.push(delivery);
    if (this.deliveries.length === 1) {
      await this.executeNextDelivery();
    }
  }

  async executeNextDelivery() {
    if (this.deliveries.length === 0) {
      console.log('✅ All deliveries complete!');
      return;
    }

    const delivery = this.deliveries[0];
    console.log(`🚀 Navigating to: ${delivery.location}`);

    // Simple navigation (move toward goal)
    const target = delivery.coordinates;
    await this.navigateTo(target);

    console.log(`✅ Delivered: ${delivery.item}`);
    this.deliveries.shift();
    await this.executeNextDelivery();
  }

  async navigateTo(target) {
    while (!this.isAtTarget(target)) {
      // Calculate direction to target
      const dx = target.x - this.currentPosition.x;
      const dy = target.y - this.currentPosition.y;
      const distance = Math.sqrt(dx * dx + dy * dy);

      if (distance < 0.1) break; // Close enough

      // Move toward target
      const speed = Math.min(0.5, distance);
      await this.controlPub.publish(JSON.stringify({
        linear: { x: speed, y: 0, z: 0 },
        angular: { x: 0, y: 0, z: Math.atan2(dy, dx) }
      }));

      // Update position (in real robot, would come from sensors)
      this.currentPosition.x += dx * 0.1;
      this.currentPosition.y += dy * 0.1;

      // Publish current pose
      await this.posePub.publish(JSON.stringify(this.currentPosition));

      await new Promise(resolve => setTimeout(resolve, 100));
    }
  }

  isAtTarget(target) {
    const dx = target.x - this.currentPosition.x;
    const dy = target.y - this.currentPosition.y;
    return Math.sqrt(dx * dx + dy * dy) < 0.1;
  }

  async stop() {
    await this.controlPub.publish(JSON.stringify({
      linear: { x: 0, y: 0, z: 0 },
      angular: { x: 0, y: 0, z: 0 }
    }));
  }
}

// Run the robot
async function main() {
  const robot = new DeliveryRobot('delivery-bot-01');
  await robot.initialize();

  // Simulate delivery request
  const deliveryPub = await robot.node.createPublisher('/commands/deliver');
  await deliveryPub.publish(JSON.stringify({
    item: 'Package #42',
    location: 'Office 201',
    coordinates: { x: 10.0, y: 5.0, z: 0 }
  }));
}

main().catch(console.error);

const { AgenticNode } = require('agentic-robotics');

class WarehouseCoordinator {
  constructor() {
    this.node = new AgenticNode('warehouse-coordinator');
    this.robots = new Map(); // Track robot status
    this.pendingTasks = [];
  }

  async initialize() {
    // Subscribe to robot status updates
    this.statusSub = await this.node.createSubscriber('/robots/+/status');
    await this.statusSub.subscribe(this.handleRobotStatus.bind(this));

    // Create task assignment publisher
    this.taskPub = await this.node.createPublisher('/tasks/assignments');

    // Subscribe to new orders
    this.orderSub = await this.node.createSubscriber('/warehouse/orders');
    await this.orderSub.subscribe(this.handleNewOrder.bind(this));

    console.log('✅ Warehouse coordinator ready');
  }

  handleRobotStatus(message) {
    const status = JSON.parse(message);
    this.robots.set(status.robotId, status);

    console.log(`🤖 Robot ${status.robotId}: ${status.state}`);

    // If robot became idle, assign next task
    if (status.state === 'idle' && this.pendingTasks.length > 0) {
      this.assignTask(status.robotId);
    }
  }

  handleNewOrder(message) {
    const order = JSON.parse(message);
    console.log(`📦 New order: ${order.orderId}`);

    // Break order into tasks (pick items, pack, deliver)
    const tasks = this.planTasks(order);
    this.pendingTasks.push(...tasks);

    // Assign to available robots
    this.assignPendingTasks();
  }

  planTasks(order) {
    // Create pick tasks for each item
    return order.items.map(item => ({
      type: 'pick',
      orderId: order.orderId,
      item: item,
      location: this.findItemLocation(item),
      priority: order.priority || 0
    }));
  }

  assignPendingTasks() {
    for (const [robotId, status] of this.robots) {
      if (status.state === 'idle' && this.pendingTasks.length > 0) {
        this.assignTask(robotId);
      }
    }
  }

  async assignTask(robotId) {
    if (this.pendingTasks.length === 0) return;

    // Sort by priority
    this.pendingTasks.sort((a, b) => b.priority - a.priority);

    const task = this.pendingTasks.shift();

    console.log(`📋 Assigning task to robot ${robotId}:`, task);

    await this.taskPub.publish(JSON.stringify({
      robotId: robotId,
      task: task,
      timestamp: Date.now()
    }));
  }

  findItemLocation(item) {
    // Simplified: in real system, query warehouse DB
    return {
      aisle: Math.floor(Math.random() * 10) + 1,
      shelf: Math.floor(Math.random() * 5) + 1,
      bin: Math.floor(Math.random() * 20) + 1
    };
  }
}

class WarehouseRobot {
  constructor(robotId) {
    this.robotId = robotId;
    this.node = new AgenticNode(`robot-${robotId}`);
    this.state = 'idle';
    this.currentTask = null;
  }

  async initialize() {
    // Subscribe to task assignments
    this.taskSub = await this.node.createSubscriber('/tasks/assignments');
    await this.taskSub.subscribe(this.handleTaskAssignment.bind(this));

    // Create status publisher
    this.statusPub = await this.node.createPublisher(`/robots/${this.robotId}/status`);

    // Report status every second
    setInterval(() => this.reportStatus(), 1000);

    console.log(`🤖 Robot ${this.robotId} initialized`);
    this.reportStatus();
  }

  async handleTaskAssignment(message) {
    const assignment = JSON.parse(message);

    // Ignore if not for this robot
    if (assignment.robotId !== this.robotId) return;

    this.currentTask = assignment.task;
    this.state = 'working';

    console.log(`📋 Robot ${this.robotId} received task:`, this.currentTask.type);

    // Execute task
    await this.executeTask(this.currentTask);

    this.currentTask = null;
    this.state = 'idle';
    console.log(`✅ Robot ${this.robotId} completed task`);
  }

  async executeTask(task) {
    // Simulate task execution
    const duration = 2000 + Math.random() * 3000;
    await new Promise(resolve => setTimeout(resolve, duration));
  }

  async reportStatus() {
    await this.statusPub.publish(JSON.stringify({
      robotId: this.robotId,
      state: this.state,
      currentTask: this.currentTask?.type || null,
      battery: 0.7 + Math.random() * 0.3,
      position: {
        x: Math.random() * 100,
        y: Math.random() * 50
      },
      timestamp: Date.now()
    }));
  }
}

// Run the warehouse system
async function main() {
  // Create coordinator
  const coordinator = new WarehouseCoordinator();
  await coordinator.initialize();

  // Create 5 robots
  const robots = [];
  for (let i = 1; i <= 5; i++) {
    const robot = new WarehouseRobot(i);
    await robot.initialize();
    robots.push(robot);
  }

  // Simulate orders
  const orderPub = await coordinator.node.createPublisher('/warehouse/orders');

  setInterval(async () => {
    await orderPub.publish(JSON.stringify({
      orderId: `ORD-${Date.now()}`,
      items: ['Widget A', 'Widget B', 'Widget C'],
      priority: Math.floor(Math.random() * 3),
      timestamp: Date.now()
    }));
  }, 5000);

  console.log('🏭 Warehouse system running!');
}

main().catch(console.error);

// This tutorial uses the MCP server to enable AI control

// 1. Install and configure MCP server (see Quick Start above)

// 2. Create a robot that responds to AI commands
const { AgenticNode } = require('agentic-robotics');

class VoiceControlledRobot {
  constructor() {
    this.node = new AgenticNode('voice-robot');
    this.executingCommand = false;
  }

  async initialize() {
    // Subscribe to AI commands from MCP server
    this.commandSub = await this.node.createSubscriber('/ai/commands');
    await this.commandSub.subscribe(this.handleAICommand.bind(this));

    // Create result publisher
    this.resultPub = await this.node.createPublisher('/ai/results');

    console.log('🎤 Voice-controlled robot ready!');
    console.log('💡 Say things like:');
    console.log('   - "Move forward 2 meters"');
    console.log('   - "Turn left 90 degrees"');
    console.log('   - "Go to the kitchen"');
    console.log('   - "Find the nearest charging station"');
  }

  async handleAICommand(message) {
    if (this.executingCommand) {
      console.log('⏳ Still executing previous command...');
      return;
    }

    this.executingCommand = true;
    const command = JSON.parse(message);

    console.log(`🗣️  Received: "${command.natural_language}"`);
    console.log(`🤖 Interpreted as: ${command.action}`, command.parameters);

    try {
      const result = await this.execute(command);

      await this.resultPub.publish(JSON.stringify({
        command: command.natural_language,
        success: true,
        result: result,
        timestamp: Date.now()
      }));

      console.log('✅ Command completed:', result);
    } catch (error) {
      console.error('❌ Command failed:', error.message);

      await this.resultPub.publish(JSON.stringify({
        command: command.natural_language,
        success: false,
        error: error.message,
        timestamp: Date.now()
      }));
    } finally {
      this.executingCommand = false;
    }
  }

  async execute(command) {
    // Execute based on action type
    switch (command.action) {
      case 'move':
        return await this.move(command.parameters);
      case 'turn':
        return await this.turn(command.parameters);
      case 'navigate':
        return await this.navigate(command.parameters);
      case 'scan':
        return await this.scan(command.parameters);
      default:
        throw new Error(`Unknown action: ${command.action}`);
    }
  }

  async move(params) {
    const distance = params.distance || 1.0;
    console.log(`🚶 Moving ${distance}m...`);
    await new Promise(resolve => setTimeout(resolve, distance * 1000));
    return `Moved ${distance} meters`;
  }

  async turn(params) {
    const degrees = params.degrees || 90;
    console.log(`🔄 Turning ${degrees}°...`);
    await new Promise(resolve => setTimeout(resolve, 500));
    return `Turned ${degrees} degrees`;
  }

  async navigate(params) {
    const destination = params.destination;
    console.log(`🗺️  Navigating to ${destination}...`);
    await new Promise(resolve => setTimeout(resolve, 3000));
    return `Arrived at ${destination}`;
  }

  async scan(params) {
    console.log(`👁️  Scanning environment...`);
    await new Promise(resolve => setTimeout(resolve, 1000));
    return {
      objects_detected: ['chair', 'table', 'person'],
      confidence: 0.95
    };
  }
}

async function main() {
  const robot = new VoiceControlledRobot();
  await robot.initialize();

  // Robot is now listening for commands from AI via MCP server
  // Use Claude Desktop to send natural language commands!
}

main().catch(console.error);

// Coordinate robotic arms for assembly line
const assemblyLine = new AgenticNode('assembly-line');

// Each station reports completion
await stationSub.subscribe(async (msg) => {
  const { station, product } = JSON.parse(msg);

  // Use AI to detect defects
  const inspection = await aiInspect(product);

  if (inspection.quality < 0.95) {
    await rejectPub.publish(JSON.stringify({ product, reason: inspection.issues }));
  } else {
    await nextStationPub.publish(JSON.stringify({ product, nextStation: station + 1 }));
  }
});

// Hospital delivery robot with prioritization
class HospitalDeliveryBot {
  async handleEmergencyRequest(request) {
    // Store experience for learning
    await this.memory.storeEpisode({
      context: 'emergency_delivery',
      priority: 'urgent',
      path_taken: this.currentPath,
      obstacles_encountered: this.obstacles,
      time_to_delivery: this.completionTime
    });

    // AI learns optimal emergency routes over time
    const optimalRoute = await this.aiPlanner.getBestRoute({
      from: this.currentLocation,
      to: request.destination,
      priority: 'emergency',
      learned_preferences: true
    });

    await this.navigate(optimalRoute);
  }
}

// Autonomous farming robot with AI decision making
class FarmingRobot {
  async inspectCrop(location) {
    // Capture image
    const image = await this.camera.capture();

    // AI analyzes crop health
    const analysis = await this.aiVision.analyzeCrop(image);

    if (analysis.needs_water) {
      await this.waterCrop(location, analysis.water_amount);
    }

    if (analysis.pest_detected) {
      await this.alertFarmer({
        location: location,
        pest_type: analysis.pest_type,
        severity: analysis.severity,
        image: image
      });
    }

    // Store for yield prediction
    await this.memory.storeCropData({
      location: location,
      health_score: analysis.health_score,
      growth_stage: analysis.growth_stage,
      timestamp: Date.now()
    });
  }
}

// Smart home security robot
class SecurityRobot {
  async patrol() {
    const route = await this.planPatrolRoute();

    for (const checkpoint of route) {
      await this.navigateTo(checkpoint);

      // AI-powered anomaly detection
      const scan = await this.scanArea();
      const anomalies = await this.aiDetector.detectAnomalies(scan);

      if (anomalies.length > 0) {
        // Record event
        await this.recordEvent({
          type: 'anomaly_detected',
          location: checkpoint,
          details: anomalies,
          video: await this.camera.record(30) // 30 sec clip
        });

        // Alert homeowner
        await this.sendAlert({
          severity: this.assessThreat(anomalies),
          message: `Unusual activity detected at ${checkpoint.name}`,
          livestream_url: this.streamURL
        });
      }
    }
  }
}

// Construction site inspection drone
class InspectionDrone {
  async inspectStructure(building) {
    const flightPlan = await this.planInspectionFlight(building);

    for (const waypoint of flightPlan) {
      await this.flyTo(waypoint);

      // Capture high-res images
      const images = await this.camera.captureMultiple(5);

      // AI structural analysis
      const analysis = await this.aiInspector.analyzeStructure(images);

      if (analysis.defects.length > 0) {
        await this.report.addDefects({
          location: waypoint,
          defects: analysis.defects,
          severity: analysis.severity,
          images: images,
          recommendations: analysis.repair_recommendations
        });
      }

      // Update 3D model
      await this.model.updateWithImages(images, waypoint);
    }

    // Generate comprehensive report
    return await this.report.generate();
  }
}