package yaokong;

import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.util.ArrayList;
import java.util.List;

public class Control01 {
    
    /**
     * 构建路径坐标指令（指令类型0x01）的HEX格式字符串
     * 
     * @param coordinates 坐标字符串，格式："X1,Y1;X2,Y2;X3,Y3"（分号分隔，逗号分隔XY）
     * @return 路径坐标指令的HEX格式字符串
     */
    public static String buildPathCommandHex(String coordinates) {
        List<Point> points = parseCoordinates(coordinates);
        byte[] commandBytes = buildPathCommandBytes(points);
        return bytesToHex(commandBytes);
    }
    
    /**
     * 构建路径坐标指令的字节数组
     */
    public static byte[] buildPathCommandBytes(List<Point> points) {
        if (points == null || points.isEmpty() || points.size() > 255) {
            throw new IllegalArgumentException("路径点数量必须在1-255之间");
        }
        
        int pointCount = points.size();
        int dataLength = 1 + pointCount * 16; // 1字节点数 + 每个点16字节（两个double）
        
        ByteBuffer buffer = ByteBuffer.allocate(2 + 1 + 2 + 2 + dataLength + 2 + 1);
        buffer.order(ByteOrder.LITTLE_ENDIAN);
        
        // 帧头
        buffer.put(BluetoothProtocol.FRAME_HEADER);
        
        // 指令类型
        buffer.put((byte) 0x01);
        
        // 数据长度
        buffer.putShort((short) dataLength);
        
        // 序列号
        buffer.putShort((short) BluetoothProtocol.getNextSequence());
        
        // 路径点数量
        buffer.put((byte) pointCount);
        
        // 路径点数据
        for (Point point : points) {
            buffer.putDouble(point.x);
            buffer.putDouble(point.y);
        }
        
        // 计算CRC16
        byte[] dataForCRC = new byte[1 + 2 + 2 + dataLength];
        System.arraycopy(buffer.array(), 2, dataForCRC, 0, dataForCRC.length);
        int crc = CRC16.calculateCRC16(dataForCRC, 0, dataForCRC.length);
        buffer.putShort((short) crc);
        
        // 帧尾
        buffer.put((byte) 0x0D);
        
        return buffer.array();
    }
    
    /**
     * 解析坐标字符串
     */
    private static List<Point> parseCoordinates(String coordinates) {
        if (coordinates == null || coordinates.trim().isEmpty()) {
            throw new IllegalArgumentException("坐标字符串不能为空");
        }
        
        List<Point> points = new ArrayList<>();
        String[] pointStrings = coordinates.split(";");
        
        for (String pointStr : pointStrings) {
            String[] xy = pointStr.split(",");
            if (xy.length != 2) {
                throw new IllegalArgumentException("坐标格式错误，应为X,Y格式");
            }
            
            try {
                double x = Double.parseDouble(xy[0].trim());
                double y = Double.parseDouble(xy[1].trim());
                points.add(new Point(x, y));
            } catch (NumberFormatException e) {
                throw new IllegalArgumentException("坐标必须是数字: " + pointStr);
            }
        }
        
        return points;
    }
    
    public static class Point {
        public double x;
        public double y;
        
        public Point(double x, double y) {
            this.x = x;
            this.y = y;
        }
    }
    
    private static String bytesToHex(byte[] bytes) {
        StringBuilder hexString = new StringBuilder();
        for (int i = 0; i < bytes.length; i++) {
            String hex = Integer.toHexString(bytes[i] & 0xFF);
            if (hex.length() == 1) {
                hexString.append('0');
            }
            hexString.append(hex);
            if (i < bytes.length - 1) {
                hexString.append(' ');
            }
        }
        return hexString.toString().toUpperCase();
    }    
}