/* $Id: test1.c 53 2007-03-10 14:46:36Z bja $ 
 *
 * Copyright (C) 2007, Joel Andersson <bja@kth.se>
 *  Modified by chad phillips
 
 * 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 2 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, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
 */
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <time.h>
#include <unistd.h>;
#include "wiimote.h"
#include "wiimote_api.h"

int
main (int argc, char **argv)
{
  wiimote_t wiimote = WIIMOTE_INIT;

  if (argc < 2)
    {
      fprintf (stderr, "Usage: test1 BDADDR\n");
      exit (1);
    }

  /* Print help information. */

  printf ("test5 - Firebot based on libwiimote test application\n\n");
  printf ("Press buttons 1 and 2 on the wiimote now to connect.\n");

  /* Connect the wiimote specified on the command line. */

  if (wiimote_connect (&wiimote, argv[1]) < 0)
    {
      fprintf (stderr, "unable to open wiimote: %s\n", wiimote_get_error ());
      exit (1);
    }

  /* Activate the wiimote IR sensor and accelerometer.  It will take effect on the 
     next call to wiimote_update. */

  wiimote.led.one = 1;
  wiimote.mode.ir = 1;
  wiimote.mode.acc = 1;

  int lastturn = 0;
  while (wiimote_is_open (&wiimote))
    {
      /* The wiimote_update function is used to synchronize the wiimote
         object with the real wiimote. It should be called as often as
         possible in order to minimize latency. */
      if (wiimote_update (&wiimote) < 0)
	{
	  wiimote_disconnect (&wiimote);
	  break;
	}


      /* slow down the wiimote updates so the servo's have time to spin the wheels */
      usleep (150000);

      if (lastturn == 3)
	{
	  /* if moving forward, pause for a little bit so the fan has more time to run */
	  wiimote_write_byte (&wiimote, 0x04a40001, 4);
	  if (wiimote_update (&wiimote) < 0)
	    {
	      wiimote_disconnect (&wiimote);
	      break;
	    }
	  sleep (1);
	}

      int number = 0;

      /* See if IR sensor found the candle */
      if (wiimote.ir1.size < 15 || wiimote.ir2.size < 15
	  || wiimote.ir3.size < 15 || wiimote.ir4.size < 15)
	{
	  fprintf (stderr, "IR1 x=%04d y=%04d ss=%d\n",
		   wiimote.ir1.x, wiimote.ir1.y, wiimote.ir1.size);

	  fprintf (stderr, "IR2 x=%04d y=%04d ss=%d\n",
		   wiimote.ir2.x, wiimote.ir2.y, wiimote.ir2.size);

	  fprintf (stderr, "IR3 x=%04d y=%04d ss=%d\n",
		   wiimote.ir3.x, wiimote.ir3.y, wiimote.ir3.size);

	  fprintf (stderr, "IR4 x=%04d y=%04d ss=%d\n",
		   wiimote.ir4.x, wiimote.ir4.y, wiimote.ir4.size);

	  number = 3;
	  printf ("Go forward \n");
	}
      else
	{
	  if (lastturn == 1)
	    {
	      printf ("Go right \n");
	      number = 1;
	    }
	  else
	    {
	      printf ("Go left \n");
	    }
	  if (lastturn == 3)
	    {
	      /* if we just lost track of the candle, pause for a little bit so the fan has more time to run
                 to make sure the candle is completely out */
	      wiimote_write_byte (&wiimote, 0x04a40001, 4);
	      if (wiimote_update (&wiimote) < 0)
		{
		  wiimote_disconnect (&wiimote);
		  break;
		}
	      sleep (1);
	    }
	  number = 2;
	}
      lastturn = number;

      /* Send the command to the wiimote */
      wiimote_write_byte (&wiimote, 0x04a40001, number);
    }
  return 0;
}