package bot4cj.hd
import pthread4cj.*

/*  pca9685舵机控制版 - bot4cj
 *  TODO(20260515-qing): 由于对统一接口的需求，设计基于回调的虚拟化硬件
 *  TODO(20260516-qing): 获取pca9685的中文或者英文参考资料
 *  NOTE(20260618-qing): 该代码已经被Lua脚本所取代，留着备用说不定以后会删除。  */
public class PCA9685 {
	private static const ADDR_0: UInt8 = 0x40   /* PCA9685的i2c地址 */
	private let i2c_fd: Int32
	private let addr: UInt8
	public init(	i2c_fd: Int32,
			addr!: UInt8 = ADDR_0) {		/* PCA9685硬件地址 */
		this.i2c_fd = i2c_fd
		this.addr = addr
		/* -------- 初始化PCA9685 --------
		 * FIXME(20260517-qing): 在旧版机器人代码移除以后，移除这一段 */

	}

	/*  -------- 设置PWM频率为50Hz（舵机标准周期20ms0） --------
	 *  TODO(20260516-qing): 不将数值写死 */
	public func setup() {
		unsafe {
			I2cLock_lock(i2c_fd) 
			setI2cAddr(i2c_fd, addr);
			writeI2cReg(i2c_fd, 0x00, 0x20);   /* 开启AI（地址自增）和关闭SLEEP */
			sleep(5000 * Duration.nanosecond);	/* 等待振荡器启动 */
        					/* FIXME: 这一条是不是重复了 */
			var freq: Float32 = 50.0;
			var prescale_val: Float32 = 25000000.0 / (4096.0 * freq) - 1.0;    /* 25MHz主频 */
			var prescale: UInt8 = UInt8(prescale_val + 0.5);

			writeI2cReg(i2c_fd, 0x00, 0x10);       /* 进入SLEEP模式（允许改频率） */
			writeI2cReg(i2c_fd, 0xFE, prescale);   /* 写入预分频值 */
			writeI2cReg(i2c_fd, 0x00, 0x20);       /* 退出SLEEP */

			I2cLock_unlock(i2c_fd)
		}
		sleep(5000 * Duration.nanosecond);  /* 等待振荡器启动 */
	}


	/* -------- 设置指定通道的PWM信号（0-4095）--------  */
	public func setPWM(channel: UInt8, on: UInt16, off: UInt16) {
		var reg: UInt8 = 0x06 + 4 * channel;   /* LED0 寄存器起始地址 */
		unsafe {
			writeI2cReg(i2c_fd, reg,     UInt8(on & 0xFF));
			writeI2cReg(i2c_fd, reg + 1, UInt8(on >> 8));
			writeI2cReg(i2c_fd, reg + 2, UInt8(off & 0xFF));
			writeI2cReg(i2c_fd, reg + 3, UInt8(off >> 8));
		}
		return
	}

	/* -------- 角度转PWM值（0°~180° → 0.5ms~2.5ms脉宽） -------- */
	public func setAngle(channel: UInt8, angle: Float32) {
		const min: Float32 = 0.5;    /* 0°对应0.5ms */
		const max: Float32 = 2.5;    /* 180°对应2.5ms */
		let width: Float32 = min + (max - min) * (angle / 180.0);
		let off_val: UInt16 = UInt16(width * 4096.0 / 20.0);        /* 20ms 周期 */
		unsafe {
			I2cLock_lock(i2c_fd)
			setI2cAddr(i2c_fd, this.addr);
			this.setPWM(channel, 0, off_val); /* ON=0, OFF=计算值 */
			I2cLock_unlock(i2c_fd)
		}
	}
}