/*
    r2d2 - interact with Sphero R2-D2
    Copyright (C) 2020 Ian Cowburn

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <errno.h>

#include <unistd.h>
#include <sys/socket.h>
#include <bluetooth/bluetooth.h>
#include <bluetooth/hci.h>
#include <bluetooth/hci_lib.h>
#include <bluetooth/l2cap.h>

typedef struct
{
    const char	*name;
    int		command[20];
} Command;

static Command plain_commands[] =
{
    {"laugh", {0x0A,0x18,0x00,0x1F,0x00,0x32,0x00,0x00,0x00,0x00,0x00,-1}},
    {"yes", {0x0A,0x17,0x05,0x41,0x00,0x0F,-1}},
    {"no", {0x0A,0x17,0x05,0x3F,0x00,0x10,-1}},
    {"alarm", {0x0A,0x17,0x05,0x17,0x00,0x07,-1}},
    {"angry", {0x0A,0x17,0x05,0x18,0x00,0x08,-1}},
    {"annoyed", {0x0A,0x17,0x05,0x19,0x00,0x09,-1}},
    {"ionblast", {0x0A,0x17,0x05,0x1A,0x00,0x0E,-1}},
    {"sad", {0x0A,0x17,0x05,0x1C,0x00,0x11,-1}},
    {"scared", {0x0A,0x17,0x05,0x1D,0x00,0x13,-1}},
    {"chatty", {0x0A,0x17,0x05,0x17,0x00,0x0A,-1}},
    {"confident", {0x0A,0x17,0x05,0x18,0x00,0x12,-1}},
    {"excited", {0x0A,0x17,0x05,0x19,0x00,0x0C,-1}},
    {"happy", {0x0A,0x17,0x05,0x1A,0x00,0x0D,-1}},
    {"laugh", {0x0A,0x17,0x05,0x1B,0x00,0x0F,-1}},
    {"surprise", {0x0A,0x17,0x05,0x1C,0x00,0x18,-1}},
    {"tripod", {0x0A,0x17,0x0D,0x1D,0x01,-1}},
    {"bipod", {0x0A,0x17,0x0D,0x1C,0x02,-1}},
    {NULL, {0}}
};

static unsigned char force_command[] =
{
    0x75,0x73,0x65,0x74,0x68,0x65,0x66,0x6F,0x72,0x63,0x65,0x2E,
    0x2E,0x2E,0x62,0x61,0x6E,0x64
};

static Command wake_command =
{
    "wakeup",
    {
    	0x0A,0x13,0x0D,0x00,-1
    }
};

static void Fatal(const char *call)
{
    perror(call);
    exit(EXIT_FAILURE);
}

static unsigned char Checksum(const char *p, int len)
{
    unsigned char c;
    int f;

    c = 0;

    for(f = 0; f < len; f++)
    {
    	c += p[f];
    }

    c = ~c;

    return c;
}

static int Write(int sock, unsigned char *buff, int len)
{
    int wr = 0;
    int tot = 0;

    while(tot < len)
    {
    	wr = write(sock, buff + tot, len - tot);

	if (wr < 1)
	{
	    return -1;
	}

	tot += wr;
    }

    return len;
}

static void CharWriteHandle(int sock, int handle,
			    const unsigned char *cmd, int len)
{
    unsigned char buff[1024];
    unsigned short *us;

    memcpy(buff + 3, cmd, len);
    len += 3;

    buff[0] = 0x12;

    us = (unsigned short *)(buff + 1);
    *us = htobs(handle);

    if (Write(sock, buff, len) == -1)
    {
    	Fatal("write");
    }
}

static void SendCommand(int sock, int handle, const Command *cmd)
{
    unsigned char buff[1024];
    unsigned short *us;
    int len;
    int f;

    for(f = 0; cmd->command[f] != -1; f++)
    {
    	buff[f] = (unsigned char)cmd->command[f];
    }

    len = f;

    buff[len] = Checksum(buff, len);
    len++;

    memmove(buff + 1, buff, len++);
    buff[0] = 0x8du;
    buff[++len] = 0xd8u;

    CharWriteHandle(sock, handle, buff, len);
}

int main(int argc, char *argv[])
{
    char address[32] = {0};
    struct sockaddr_l2 src_addr = {0};
    struct sockaddr_l2 dest_addr = {0};
    int base;
    int f;
    int sock;

    /* Get address
    */
    if (getenv("R2D2"))
    {
    	snprintf(address, sizeof address, "%s", getenv("R2D2"));
    }

    if (argc > 2 && strcmp(argv[1], "-a") == 0)
    {
    	snprintf(address, sizeof address, "%s", argv[2]);
	base = 3;
    }
    else
    {
    	base = 1;
    }

    /* Check address and arguments supplied
    */
    if (!address[0])
    {
    	fprintf(stderr, "%s: pass address or set R2D2 variable\n", argv[0]);
	return EXIT_FAILURE;
    }

    if (argc <= base)
    {
	fprintf(stderr, "%s: usage %s [-a address] command [..command]\n",
			    argv[0], argv[0]);
    	return EXIT_FAILURE;
    }

    /* Open connection to R2
    */
    sock = socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);

    if (sock == -1)
    {
    	Fatal("socket");
    }

    src_addr.l2_family = AF_BLUETOOTH;
    src_addr.l2_cid = htobs(4);
    bacpy(&src_addr.l2_bdaddr, BDADDR_ANY);
    src_addr.l2_bdaddr_type = BDADDR_LE_RANDOM;

    if (bind(sock, (void *)&src_addr, sizeof(src_addr)) == -1)
    {
    	Fatal("bind");
    }

    dest_addr.l2_family = AF_BLUETOOTH;
    dest_addr.l2_cid = htobs(4);
    str2ba(address, &dest_addr.l2_bdaddr);
    dest_addr.l2_bdaddr_type = BDADDR_LE_RANDOM;

    if (connect(sock, (void *)&dest_addr, sizeof(dest_addr)) == -1)
    {
    	Fatal("connect");
    }

    CharWriteHandle(sock, 0x15, force_command, sizeof force_command);
    SendCommand(sock, 0x1c,  &wake_command);

    for(f = base; f < argc; f++)
    {
	int n;

    	Command *cmd = NULL;

	for(n = 0; plain_commands[n].name && !cmd; n++)
	{
	    if (strcmp(argv[f], plain_commands[n].name) == 0)
	    {
	    	cmd = plain_commands + n;
	    }
	}

	if (cmd == NULL)
	{
	    fprintf(stderr, "%s: unknown command %s\n", argv[0], argv[f]);
	}
	else
	{
	    SendCommand(sock, 0x1c,  cmd);
	}
    }

    /* All done
    */
    if (close(sock) == -1)
    {
    	Fatal("close");
    }

    return EXIT_SUCCESS;
}