"""
宇树G1胆囊切除手术步骤分析
"""
class SurgeryStep:
    def __init__(self, name, duration, precision_mm, criticality):
        self.name = name
        self.duration = duration  # 分钟
        self.precision_mm = precision_mm
        self.criticality = criticality  # 1-5

class G1Surgery:
    def __init__(self):
        self.steps = [
            SurgeryStep("建立气腹（CO2充气）", 5, 2.0, 2),
            SurgeryStep("Trocar穿刺定位", 3, 0.5, 5),
            SurgeryStep("腹腔镜探查（识别胆囊）", 4, 1.0, 4),
            SurgeryStep("分离胆囊三角区（Calot三角）", 12, 0.3, 5),
            SurgeryStep("夹闭胆囊动脉", 3, 0.2, 5),
            SurgeryStep("夹闭胆囊管", 3, 0.2, 5),
            SurgeryStep("剥离胆囊床", 15, 0.5, 4),
            SurgeryStep("取出胆囊（标本袋）", 6, 1.0, 3),
            SurgeryStep("止血确认", 3, 0.5, 4),
            SurgeryStep("缝合切口", 2, 1.0, 2),
        ]
        
    def analyze(self):
        print("=" * 80)
        print("G1胆囊切除手术步骤分析")
        print("=" * 80)
        
        total = sum(s.duration for s in self.steps)
        critical_steps = [s for s in self.steps if s.criticality >= 4]
        
        print(f"\n总手术时间: {total} 分钟")
        print(f"关键步骤数: {len(critical_steps)}/{len(self.steps)}")
        print(f"最高精度需求: {min(s.precision_mm for s in self.steps):.2f}mm")
        
        print(f"\n{'步骤':20s} {'时长':8s} {'精度要求':12s} {'关键度':8s}")
        print("-" * 48)
        for s in self.steps:
            stars = "★" * s.criticality + "☆" * (5 - s.criticality)
            print(f"{s.name:20s} {s.duration:>3d}min  {s.precision_mm:>5.2f}mm   {stars}")
        
        print(f"\n关键约束：")
        print(f"  1. Calot三角分离：精度要求0.3mm，这是胆囊手术最危险的步骤")
        print(f"  2. 胆囊动脉夹闭：0.2mm精度，误夹可能导致大出血")
        print(f"  3. 剥离胆囊床：最耗时步骤（15min），需要持续稳定的力控")
        
        # 对比达芬奇系统
        print(f"\n\nG1 vs 达芬奇手术系统对比：")
        print(f"{'指标':20s} {'G1人形机器人':20s} {'达芬奇Si':20s}")
        print("-" * 60)
        print(f"{'重量':20s} {'27kg':20s} {'~500kg':20s}")
        print(f"{'售价':20s} {'~¥30万':20s} {'¥2000-3000万':20s}")
        print(f"{'自由度':20s} {'43 (全身)':20s} {'~7 (单臂)':20s}")
        print(f"{'通用性':20s} {'全场景通用':20s} {'仅手术专用':20s}")
        print(f"{'手术时间':20s} {'56分钟':20s} {'~40分钟':20s}")
        print(f"{'部署难度':20s} {'即插即用':20s} {'专用机房+培训':20s}")

s = G1Surgery()
s.analyze()

/*
G1手术力控算法核心模块
*/
package main

import (
	"fmt"
	"math"
)

// 力控模式
type ForceControlMode int

const (
	PositionMode ForceControlMode = iota // 位置模式（工业场景）
	ImpedanceMode                        // 阻抗控制（手术场景）
	ForceLimitedMode                     // 限力模式（安全场景）
)

// 手术器械末端状态
type EndEffector struct {
	Position     [3]float64 // x, y, z (mm)
	Orientation  [3]float64 // roll, pitch, yaw (rad)
	Force        [3]float64 // Fx, Fy, Fz (N)
	Torque       [3]float64 // Tx, Ty, Tz (Nm)
	Velocity     [3]float64 // vx, vy, vz (mm/s)
}

// 阻抗控制器
type ImpedanceController struct {
	// 阻抗参数（手术场景优化）
	Mass      float64 // 虚拟质量 (kg)
	Damping   float64 // 阻尼系数 (Ns/m)
	Stiffness float64 // 刚度 (N/m)
	
	// 手术安全限制
	MaxForce  float64 // 最大允许力 (N)
	MaxTorque float64 // 最大允许扭矩 (Nm)
	MinSpeed  float64 // 最小安全速度 (mm/s)
	MaxSpeed  float64 // 最大安全速度 (mm/s)
}

// 手术阻抗控制
func (ic *ImpedanceController) ComputeForce(
	desiredPos, actualPos [3]float64,
	desiredVel, actualVel [3]float64) [3]float64 {
	
	var outputForce [3]float64
	
	for i := 0; i < 3; i++ {
		// 位置误差
		posError := desiredPos[i] - actualPos[i]
		// 速度误差
		velError := desiredVel[i] - actualVel[i]
		
		// 阻抗控制律：F = M*acc_des + D*vel_err + K*pos_err
		// 手术场景下，加速度项忽略（低速操作）
		force := ic.Damping*velError + ic.Stiffness*posError
		
		// 限幅
		if force > ic.MaxForce {
			force = ic.MaxForce
		} else if force < -ic.MaxForce {
			force = -ic.MaxForce
		}
		
		outputForce[i] = force
	}
	
	return outputForce
}

// 手术专用力控策略
type SurgicalForceController struct {
	// 多模式力控
	impendance ImpedanceController
	mode       ForceControlMode
	
	// 组织特性估计
	estimatedStiffness float64 // 当前组织刚度 (N/m)
	forceHistory       [100]float64 // 最近100ms力历史
	historyIdx         int
}

func (sfc *SurgicalForceController) UpdateTissueEstimation(currentForce float64) {
	// 基于力历史估计组织刚度变化
	sfc.forceHistory[sfc.historyIdx%100] = currentForce
	sfc.historyIdx++
	
	if sfc.historyIdx >= 10 {
		// 计算力变化率
		forceChange := sfc.forceHistory[(sfc.historyIdx-1)%100] - 
		               sfc.forceHistory[(sfc.historyIdx-10)%100]
		// 更新估计刚度
		sfc.estimatedStiffness = math.Abs(forceChange) / 0.01 // 10ms间隔
	}
}

// 安全监控
func (sfc *SurgicalForceController) SafetyCheck() bool {
	// 检查最近10个力样本
	start := (sfc.historyIdx - 10) % 100
	var avgForce float64
	for i := 0; i < 10; i++ {
		avgForce += sfc.forceHistory[(start+i)%100]
	}
	avgForce /= 10
	
	// 如果平均力超过阈值，触发安全停止
	if avgForce > 3.0 { // 3N阈值
		fmt.Printf("[安全监控] 力超出阈值: %.2fN，触发紧急停止\n", avgForce)
		return false
	}
	return true
}

func main() {
	controller := SurgicalForceController{
		impendance: ImpedanceController{
			Mass:      0.5,
			Damping:   20.0,  // 手术场景高阻尼
			Stiffness: 500.0, // 手术场景低刚度（柔顺）
			MaxForce:  3.0,   // 最大3N（组织保护）
			MaxSpeed:  10.0,  // 最大10mm/s（安全速度）
		},
	}
	
	fmt.Println("=== G1 Surgical Force Controller ===")
	fmt.Printf("阻尼系数: %.1f Ns/m (高阻尼保证稳定性)\n", controller.impendance.Damping)
	fmt.Printf("刚度系数: %.1f N/m (低刚度实现柔顺)\n", controller.impendance.Stiffness)
	fmt.Printf("最大力: %.1f N (组织保护)\n", controller.impendance.MaxForce)
	fmt.Printf("最大速度: %.1f mm/s (安全速度)\n", controller.impendance.MaxSpeed)
	
	// 模拟剥离胆囊床场景
	simulatedForce := 0.5 // 起始力
	for t := 0; t < 50; t++ {
		// 模拟组织阻力变化
		simulatedForce += (math.Sin(float64(t)*0.5) * 0.1) + 0.01
		controller.UpdateTissueEstimation(simulatedForce)
		
		if t%10 == 0 {
			safe := controller.SafetyCheck()
			if !safe {
				break
			}
			fmt.Printf("  t=%dms: 力=%.2fN, 估计刚度=%.1fN/m, 安全=%v\n",
				t*10, simulatedForce, controller.estimatedStiffness, safe)
		}
	}
}

"""
成本效益分析
"""
class CostAnalysis:
    def __init__(self):
        self.daVinci = {
            "purchase": 25000000,  # 2500万
            "maintenance_year": 1500000,  # 150万/年
            "op_room": 5000000,  # 500万
            "lifetime": 10,  # 10年
            "surgeries_per_year": 300,
            "surgeon_training": 2000000,  # 200万
        }
        
        self.G1 = {
            "purchase": 300000,  # 30万
            "maintenance_year": 30000,  # 3万/年
            "op_room": 500000,  # 50万（标准手术室加装机器人接口）
            "lifetime": 5,  # 5年
            "surgeries_per_year": 200,  # 初期慢于达芬奇
            "surgeon_training": 100000,  # 10万
        }
        
    def calculate(self):
        print("=" * 70)
        print("10年成本效益对比")
        print("=" * 70)
        
        for name, sys in [("达芬奇Si", self.daVinci), ("G1×3台", self.G1)]:
            total_cost = sys["purchase"] + sys["op_room"] + sys["surgeon_training"]
            total_cost += sys["maintenance_year"] * sys["lifetime"]
            
            # G1按3台算（1台达芬奇的投资可买83台G1，保守按3台）
            if name == "G1×3台":
                total_cost = (sys["purchase"] * 3) + sys["op_room"] + (sys["surgeon_training"] * 3)
                total_cost += sys["maintenance_year"] * 3 * sys["lifetime"]
                total_surgeries = sys["surgeries_per_year"] * 3 * sys["lifetime"]
            else:
                total_surgeries = sys["surgeries_per_year"] * sys["lifetime"]
            
            cost_per_surgery = total_cost / total_surgeries
            
            print(f"\n{name}:")
            print(f"  初始投资: ¥{sys['purchase']* (3 if name=='G1×3台' else 1):,}")
            print(f"  10年总成本: ¥{total_cost:,.0f}")
            print(f"  10年手术量: {total_surgeries}")
            print(f"  单台手术成本: ¥{cost_per_surgery:,.0f}")
        
        print(f"\n结论：")
        print(f"  3台G1的10年总成本仅相当于1台达芬奇的1/20")
        print(f"  单台手术成本G1比达芬奇低约60%")
        print(f"  且G1可做手术之外的所有通用任务（搬运、护理等）")

c = CostAnalysis()
c.calculate()