// +build !windows

/* SPDX-License-Identifier: MIT
 *
 * Copyright (C) 2020 jet tsang zeon-git. All Rights Reserved.
 */
package protocal

import (
	"bytes"
	"fmt"
	"io"
)

const (
	FrameHeader byte = 0x55
	// Send an instruction to all servos
	BroadcastIdent int = 0xFE // 254
)

type Proto struct {
	Network io.ReadWriter
}

func New(network io.ReadWriter) *Proto {
	return &Proto{
		Network: network,
	}
}

func (p *Proto) writeInstruction(ident int, instruction byte, params []byte) error {
	buf := new(bytes.Buffer)
	id := byte(ident & 0xFF)
	pLen := len(params) + 3

	// +------+------+----+---------------+-------------+--------+-----+--------+----------+
	// | 0x55 | 0x55 | ID |      LEN      |     INST    | Param1 | ... | ParamN |    CKS   |
	// +------+------+----+---------------+-------------+--------+-----+--------+----------+
	// |    Header   | ID | Packet Length | Instruction |       Parameter       | Checksum |
	// +-------------+----+---------------+-------------+-----------------------+----------+

	buf.Write([]byte{
		FrameHeader, // Header
		FrameHeader, // Header
		id,          // target ID
		byte(pLen),  // Length
		instruction, // instruction type (see const section)
	})

	// append n params
	buf.Write(params)

	// calculate checksum
	b := CheckSum(buf.Bytes()[2:]) // Checksum = ~ (ID + Length + INST + Prm1 + ... PrmN)
	buf.WriteByte(byte(b))

	// write to port
	_, err := buf.WriteTo(p.Network)
	if err != nil {
		return err
	}

	return nil
}

func (p *Proto) readPacket(expID int) ([]byte, error) {

	// +------+------+----+---------------+-------------+--------+-----+--------+----------+
	// | 0x55 | 0x55 | ID |      LEN      |     INST    | Param1 | ... | ParamN |    CKS   |
	// +------+------+----+---------------+-------------+--------+-----+--------+----------+
	// |    Header   | ID | Packet Length | Instruction |       Parameter       | Checksum |
	// +-------------+----+---------------+-------------+-----------------------+----------+
	// Read the first nine bytes (up to Error), which should always be present.

	buf := make([]byte, 5)
	n, err := p.Network.Read(buf)
	if err != nil {
		return nil, fmt.Errorf("reading packet header: %s", err)
	}
	if n != 9 {
		return nil, fmt.Errorf("reading packet header: expected %d bytes, got %d", 9, n)
	}

	if buf[0] != FrameHeader || buf[1] != FrameHeader {
		return nil, fmt.Errorf("bad status packet header: 0x%02X 0x%02X ", buf[0], buf[1])
	}

	actID := int(buf[2])
	plen := int(buf[4]) - 3

	// Now read the params, if there are any. We must do this before checking
	// for errors, to avoid leaving junk in the buffer.

	var pbuf []byte
	if plen > 0 {
		pbuf = make([]byte, plen)
		_, err = p.Network.Read(pbuf)
		if err != nil {
			return nil, fmt.Errorf("reading %d params: %s", plen, err)
		}
	}

	// Read the checksum, which is always two bytes.

	buf = make([]byte, 1)
	n, err = p.Network.Read(buf)
	if err != nil {
		return nil, fmt.Errorf("reading checksum: %s", err)
	}
	if n != 2 {
		return nil, fmt.Errorf("reading checksum: expected %d bytes, got %d", 2, n)
	}

	if actID != expID {
		return nil, fmt.Errorf("expected status packet for %v, but got %v", expID, actID)
	}

	return pbuf, nil
}

// ReadData reads a slice of n bytes from the control table of the given servo
// ID. Use the bytesToInt function to convert the output to something more
// useful.
func (p *Proto) ReadData(ident int, instruction byte) ([]byte, error) {

	err := p.writeInstruction(ident, instruction, nil)
	if err != nil {
		return []byte{}, err
	}

	buf, err := p.readPacket(ident)
	if err != nil {
		return buf, err
	}

	return buf, nil
}

func (p *Proto) WriteData(ident int, instruction byte, data []byte) error {
	return p.writeInstruction(ident, instruction, data)
}