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

package servo

import (
	"fmt"

	"git.jettsang.com/drivers/lobot/iface"
	"git.jettsang.com/drivers/lobot/utils"
)

const (

	// Unit conversions
	maxPos          uint16  = 1000
	maxAngle        float64 = 240
	positionToAngle float64 = maxAngle / float64(maxPos) // 0.24
	angleToPosition float64 = 1 / positionToAngle        // 4.16
)

type Servo struct {
	Protocol     iface.Protocol
	ID           int
	instructions InstructionMap
}

// New returns a new Servo.
func New(proto iface.Protocol, instructions InstructionMap, ID int) *Servo {
	return &Servo{
		Protocol:     proto,
		ID:           ID,
		instructions: instructions,
	}
}

func (s *Servo) sendInstruction(i InstName, values []uint16) error {
	r, ok := s.instructions[i]
	if !ok {
		return fmt.Errorf("can't send a unsupported instruction: %v", i)
	}

	if r.Access == RO {
		return fmt.Errorf("can't sned a read-only instruction.")
	}

	var params []byte
	switch r.Length {
	case 1:
		if len(values) != 1 {
			return fmt.Errorf("invalid values length inputs.")
		}
		params = []byte{utils.Low(values[0])}
	case 4:
		if len(values) != 2 {
			return fmt.Errorf("invalid values length inputs.")
		}
		params = []byte{
			utils.Low(values[0]),
			utils.High(values[0]),
			utils.Low(values[1]),
			utils.High(values[1]),
		}
	}

	return s.Protocol.WriteData(s.ID, r.InstByte, params)
}

func (s *Servo) SendInstruction(i InstName, values []uint16) error {
	return s.sendInstruction(i, values)
}

func (s *Servo) getData(i InstName) ([]byte, error) {
	r, ok := s.instructions[i]
	if !ok {
		return nil, fmt.Errorf("can't send a unsupported instruction: %v", i)
	}

	if r.Access != RO {
		return nil, fmt.Errorf("can't sned a read-only instruction.")
	}

	return s.Protocol.ReadData(s.ID, r.InstByte)
}