#include <stdio.h>
#include <fcntl.h>
#include <stdlib.h>
#include <stdint.h>
#include <errno.h>
#include <string.h>
#include <time.h>
#include <sys/time.h>
#include <sys/ioctl.h>
#include <linux/cp210x.h>

/*
 * bit 0: 1	left  leg NPN base
 * bit 1: 2	left  leg PNP base
 * bit 2: 4	right leg PNP base
 * bit 3: 8	right leg NPN base
 */

#define LEFT_NPN_OFF	0
#define LEFT_NPN_ON	1
#define LEFT_PNP_OFF	2
#define LEFT_PNP_ON	0

#define LEFT_LOW	(LEFT_NPN_ON | LEFT_PNP_OFF)
#define LEFT_HI		(LEFT_NPN_OFF | LEFT_PNP_ON)
#define LEFT_OFF	(LEFT_NPN_OFF | LEFT_PNP_OFF)

#define RIGHT_NPN_OFF	0
#define RIGHT_NPN_ON	8
#define RIGHT_PNP_OFF	4
#define RIGHT_PNP_ON	0


#define RIGHT_LOW	(RIGHT_NPN_ON | RIGHT_PNP_OFF)
#define RIGHT_HI	(RIGHT_NPN_OFF | RIGHT_PNP_ON)
#define RIGHT_OFF	(RIGHT_NPN_OFF | RIGHT_PNP_OFF)

#define FORWARDS	(RIGHT_LOW | LEFT_HI)
#define BACKWARDS	(RIGHT_HI | LEFT_LOW)
#define OFF		(RIGHT_OFF| LEFT_OFF)

#define TIMEOUT 	750000         /*75ms */
#define RUN_IN 		50
#define RELEASE_TENSION	10

int
drive_motor (int fd, int dir)
{
  int i;

  if (dir > 0)
    {
      printf (" Motor forwards\n");
      i = FORWARDS;
    }
  else if (dir < 0)
    {
      printf (" Motor backwards\n");
      i = BACKWARDS;
    }
  else
    {
      printf (" Motor off\n");
      i = OFF;
    }


  return ioctl (fd, CPIOC_GPIOSET, &i);
}

int
sensor_led_power (int fd, int on)
{
  int i = TIOCM_DTR;

  printf (" Rotation sensor power %s\n", on ? "on" : "off");

  return ioctl (fd, on ? TIOCMBIS : TIOCMBIC, &i);
}


static int
count_steps (int fd, int count)
{
  struct timeval tv, tv_changed, tv_diff;
  int cts, old_cts = -1;
  int counted = 0;

  printf (" Counting %d steps", count);

  gettimeofday (&tv_changed, NULL);

  while (count)
    {
      ioctl (fd, TIOCMGET, &cts);
      cts &= TIOCM_CTS;

      if (cts != old_cts)
        {
          if (cts)
            {
              counted++;
              count--;
              printf (".");
              fflush (stdout);
            }
          gettimeofday (&tv_changed, NULL);
          old_cts = cts;
        }
      gettimeofday (&tv, NULL);
      timersub (&tv, &tv_changed, &tv_diff);

      if (tv_diff.tv_sec || (tv_diff.tv_usec > TIMEOUT))
        break;

    }

  printf ("%d steps\n", counted);
  return counted;
}




static int
drive_motor_home (int fd)
{
  int ret;
  sensor_led_power (fd, 1);
  drive_motor (fd, -1);
  ret = count_steps (fd, -1);
  drive_motor (fd, 0);
  ret -= count_steps (fd, -1);
  sensor_led_power (fd, 0);
  return ret;
}

static int
drive_motor_count (int fd, int dir, int steps)
{
  int ret;
  sensor_led_power (fd, 1);
  drive_motor (fd, dir);
  ret = count_steps (fd, steps);
  drive_motor (fd, 0);
  ret += count_steps (fd, -1);
  sensor_led_power (fd, 0);
  return ret;
}

int
main (int argc, char *argv[])
{
  int overshoot, guard, pos, wanted, delta;

  int fd = open (argv[1], O_RDWR);

  if (fd < 0)
    return -1;


  pos = drive_motor_home (fd);

  printf ("Motor was %d steps from home\n", pos);


  if (argc == 2)
    {
      pos = drive_motor_count (fd, 1, RELEASE_TENSION);

      printf ("Motor now %d steps from home\n", pos);
      return 0;
    }

  pos = drive_motor_count (fd, 1, RUN_IN);

  overshoot = pos - RUN_IN;     /*overshot */

  wanted = atoi (argv[2]);

  delta = wanted - pos;
  printf ("Now at %d want %d, delta %d\n", pos, wanted, delta);

  guard = overshoot * 3;

  if ((delta > -guard) && (delta < guard))
    {
      printf ("Too close\n");
      pos += drive_motor_count (fd, 1, RUN_IN);
      delta = wanted - pos;
      printf ("Now at %d want %d, delta %d\n", pos, wanted, delta);
    }


  if (delta < 0)
    {
      pos -= drive_motor_count (fd, -1, (-delta) - overshoot);
    }
  else if (delta > 0)
    {
      pos += drive_motor_count (fd, 1, delta - overshoot);
    }

  printf ("Wanted %d steps, got %d steps\n", wanted, pos);

  return 0;
}