package gps

import (
	"errors"
	"time"
)

// flight mode disables the GPS COCOM limits
var flight_mode_cmd = [...]byte{
	0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x06, 0x03, 0x00, 0x00, 0x00,
	0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, 0x00, 0xFA, 0x00, 0x64, 0x00,
	0x2C, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x16, 0xDC}

// Sets CFG-GNSS to disable everything other than GPS GNSS
// solution. Failure to do this means GPS power saving
// doesn't work. Not needed for MAX7, needed for MAX8's
var cfg_gnss_cmd = [...]byte{
	0xB5, 0x62, 0x06, 0x3E, 0x2C, 0x00, 0x00, 0x00,
	0x20, 0x05, 0x00, 0x08, 0x10, 0x00, 0x01, 0x00,
	0x01, 0x01, 0x01, 0x01, 0x03, 0x00, 0x00, 0x00,
	0x01, 0x01, 0x03, 0x08, 0x10, 0x00, 0x00, 0x00,
	0x01, 0x01, 0x05, 0x00, 0x03, 0x00, 0x00, 0x00,
	0x01, 0x01, 0x06, 0x08, 0x0E, 0x00, 0x00, 0x00,
	0x01, 0x01, 0xFC, 0x11}

func FlightMode(d Device) (err error) {
	err = sendCommand(d, flight_mode_cmd[:])
	return err
}

func SetCfgGNSS(d Device) (err error) {
	err = sendCommand(d, cfg_gnss_cmd[:])
	return err
}

func sendCommand(d Device, command []byte) (err error) {
	d.WriteBytes(command)
	start := time.Now()
	for time.Now().Sub(start) < 1000 {
		if d.readNextByte() == '\n' {
			if d.readNextByte() == 0xB5 {
				d.readNextByte()
				if d.readNextByte() == 0x05 {
					if d.readNextByte() == 0x01 {
						return
					}
				}
			}
		}
	}
	return errors.New("no ACK to GPS command")
}