package protocol

import (
	"encoding/binary"
	"errors"
	"fmt"
)

const (
	Magic0  byte = 'G'
	Magic1  byte = 'S'
	Version byte = 1

	TypeRCChannels byte = 1

	Channels = 16
	Header   = 10
	CRCSize  = 2
	RCSize   = Header + Channels*2 + CRCSize
)

var (
	ErrFrameLength = errors.New("groundstation: invalid frame length")
	ErrMagic       = errors.New("groundstation: invalid frame magic")
	ErrVersion     = errors.New("groundstation: unsupported frame version")
	ErrCRC         = errors.New("groundstation: invalid frame crc")
	ErrType        = errors.New("groundstation: unexpected frame type")
)

type RCFrame struct {
	Sequence  uint16
	Timestamp uint32
	Channels  [Channels]int16
}

func EncodeRC(frame RCFrame) []byte {
	buf := make([]byte, RCSize)
	buf[0], buf[1], buf[2], buf[3] = Magic0, Magic1, Version, TypeRCChannels
	binary.LittleEndian.PutUint16(buf[4:6], frame.Sequence)
	binary.LittleEndian.PutUint32(buf[6:10], frame.Timestamp)
	for i, value := range frame.Channels {
		binary.LittleEndian.PutUint16(buf[Header+i*2:Header+i*2+2], uint16(value))
	}
	binary.LittleEndian.PutUint16(buf[RCSize-CRCSize:], CRC16CCITT(buf[:RCSize-CRCSize]))
	return buf
}

func DecodeRC(buf []byte) (RCFrame, error) {
	var frame RCFrame
	if len(buf) != RCSize {
		return frame, ErrFrameLength
	}
	if buf[0] != Magic0 || buf[1] != Magic1 {
		return frame, ErrMagic
	}
	if buf[2] != Version {
		return frame, ErrVersion
	}
	if buf[3] != TypeRCChannels {
		return frame, ErrType
	}
	if got, want := binary.LittleEndian.Uint16(buf[RCSize-CRCSize:]), CRC16CCITT(buf[:RCSize-CRCSize]); got != want {
		return frame, fmt.Errorf("%w: got %04x want %04x", ErrCRC, got, want)
	}
	frame.Sequence = binary.LittleEndian.Uint16(buf[4:6])
	frame.Timestamp = binary.LittleEndian.Uint32(buf[6:10])
	for i := range frame.Channels {
		frame.Channels[i] = int16(binary.LittleEndian.Uint16(buf[Header+i*2 : Header+i*2+2]))
	}
	return frame, nil
}

func CRC16CCITT(data []byte) uint16 {
	crc := uint16(0xffff)
	for _, b := range data {
		crc ^= uint16(b) << 8
		for bit := 0; bit < 8; bit++ {
			if crc&0x8000 != 0 {
				crc = (crc << 1) ^ 0x1021
			} else {
				crc <<= 1
			}
		}
	}
	return crc
}