package modbustcpclient

import (
	"encoding/binary"
	"fmt"
	"io"
	"net"
	"time"
)

type Mbclient struct {
	transactionCounter uint16
	conn               net.Conn
	header             [7]byte
	unit               uint8
}

func New(address string, unit uint8) (*Mbclient, error) {
	var err error

	c := new(Mbclient)
	c.conn, err = net.Dial("tcp", address)
	if err != nil {
		return nil, err
	}
	c.unit = unit
	return c, nil
}

func (m *Mbclient) ReadRegisters(first uint16, numRegs uint16) ([]uint16, error) {
	const requestLength = 12
	m.transactionCounter++
	req := make([]byte, requestLength)
	var responseHeader mbapHeader
	var mbpayload mbPDU

	binary.BigEndian.PutUint16(req[0:2], m.transactionCounter)
	binary.BigEndian.PutUint16(req[2:4], 0)
	binary.BigEndian.PutUint16(req[4:6], 6) // Length
	req[6] = m.unit
	req[7] = 3 //FunctionCode
	binary.BigEndian.PutUint16(req[8:10], first-1)
	binary.BigEndian.PutUint16(req[10:12], numRegs)
	m.conn.SetDeadline(time.Now().Add(10 * time.Second))
	byteswritten, err := m.conn.Write(req)
	if err != nil {
		return nil, err
	}
	if byteswritten != requestLength {
		return nil, fmt.Errorf("Failed to send request")
	}
	m.conn.SetDeadline(time.Now().Add(10 * time.Second))
	_, err = io.ReadFull(m.conn, m.header[:])
	responseHeader.unMarshal(m.header)
	expectedDataLength := responseHeader.length - 1

	response := make([]byte, expectedDataLength)
	_, err = m.conn.Read(response)

	err = mbpayload.unMarshal(response)
	if mbpayload.functionCode != 3 {
		return nil, fmt.Errorf("modbus exception %v", mbpayload.functionCode&0x7F)
	}
	if err != nil {
		return nil, err
	}

	return mbpayload.registers, nil
}

type mbapHeader struct {
	transactionID      uint16
	protocolIdentifier uint16
	length             uint16
	unitIdentifier     byte
}

// func (m *mbapHeader) marshalBinary() ([]byte, error) {

// }

func (m *mbapHeader) unMarshal(data [7]byte) error {
	m.transactionID = binary.BigEndian.Uint16(data[0:2])
	m.protocolIdentifier = binary.BigEndian.Uint16(data[2:4])
	m.length = binary.BigEndian.Uint16(data[4:6])
	m.unitIdentifier = data[6]

	return nil
}

type mbPDU struct {
	functionCode uint8
	length       uint8
	registers    []uint16
}

func (d *mbPDU) unMarshal(data []byte) error {
	d.functionCode = data[0]
	d.length = data[1]
	if d.length+2 != uint8(len(data)) {
		return fmt.Errorf("Lenght mismatch in modbus payload")
	}
	d.registers = make([]uint16, d.length/2)
	var n uint8
	for n < d.length/2 {
		d.registers[n] = binary.BigEndian.Uint16(data[n*2+2 : n*2+4])
		n++
	}
	return nil
}