package mpu6050

import (
	"fmt"

	"golang.org/x/exp/io/i2c"
)

const (
	mpuAddr = 0x68

	// MPU registers

	// Power management
	// powerMgmt1 = 0x6B
	// powerMgmt2 = 0x6C
	// Acceleration
	// accelXOut0 = 0x3B
	// accelYOut0 = 0x3D
	// accelZOut0 = 0x3F
	// Temperature
	tempOut0 = 0x41

// Gyro
// gyroXOut0 = 0x43
// gyroYOut0 = 0x45
// gyroZOut0 = 0x47
// Configs
// accelConfig = 0x1C
// gyroConfig  = 0x1B
// mpuConfig   = 0x1A
)

type device struct {
	device *i2c.Device
}

type Device interface {
	GetTemp() (float32, error)
	GetAccel() ([3]float32, error)
	GetGyro() ([3]float32, error)
}

func Open() (Device, error) {
	d, err := i2c.Open(&i2c.Devfs{Dev: "/dev/i2c-1"}, mpuAddr)
	if err != nil {
		return nil, fmt.Errorf("open i2c: %w", err)
	}
	return &device{
		device: d,
	}, nil
}

func (d *device) GetTemp() (float32, error) {
	rawTemp, err := d.readWord(tempOut0)
	if err != nil {
		return 0, err
	}
	return (float32(rawTemp) / 340.0) + 36.53, nil
}

func (d *device) GetAccel() ([3]float32, error) {
	return [3]float32{}, nil
}

func (d *device) GetGyro() ([3]float32, error) {
	return [3]float32{}, nil
}