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

#include "radiator.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 BRAKE		(RIGHT_LOW | LEFT_LOW)
#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

static int brake_motor(int fd)
{
  int i=BRAKE;
  printf (" Motor braking\n");
  return ioctl (fd, CPIOC_GPIOSET, &i);
}

static 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);
}

static 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);
}


#if 0
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;
}
#else
count_steps (int fd, int count)
{
  int counted = 0;
  struct timeval tv;
  char buf[128];
  int i;
  fd_set rfds;

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

FD_ZERO(&rfds);

tcflush(fd,TCIFLUSH);


  while (count)
    {
	tv.tv_sec=0;
	tv.tv_usec=TIMEOUT;
	FD_SET(fd,&rfds);

	if (!select(fd+1,&rfds,NULL,NULL,&tv)) break;

	i=read(fd,buf,sizeof(buf));
	if (i<0) continue;

        counted+=i;
	if (count>0) {
		count-=i;
		if (count<0) count=0;
	}

         printf (".");
         fflush (stdout);
    }

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




static int
drive_motor_count (Radiator * r, int dir, int steps)
{
  int s1, s2, ret;

  sensor_led_power (r->fd, 1);
  drive_motor (r->fd, dir);
  s1 = count_steps (r->fd, steps);
  if (s1 != steps) {
    printf (" Hit end stop\n");
    drive_motor (r->fd, 0);
  } else  {
    brake_motor(r->fd);
  }
  s2 = count_steps (r->fd, -1);
  sensor_led_power (r->fd, 0);
  drive_motor (r->fd, 0);

  /*did we hit an end stop? */
  if (s1 != steps)
    ret = s1 - s2;
  else
    ret = s1 + s2;

  printf (" %d steps under power (of %d), %d steps coasting -> %d steps\n",
          s1, steps, s2, ret);

  r->pos += ret * dir;

  /*Recalibrate our notion of ends */

  if (s1 != steps)
    {
      if (dir == -1)
        {
          r->pos = 0;
        }
      else
        {
          r->max = r->pos;
        }
    }

  return ret;
}




int
radiator_set_pos (Radiator * r, int wanted)
{
  int guard, delta;

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

  if (wanted == 0)
    {
      drive_motor_count (r, -1, -1);
      return 0;
    }
  else if (r->max && (wanted >= r->max))
    {
      drive_motor_count (r, 1, -1);
      return 0;
    }


  guard = r->overshoot * 3;

  if ((delta > -guard) && (delta < guard))
    {
      printf ("Too close\n");

      drive_motor_count (r, (r->pos > r->half) ? -1 : 1, RUN_IN);

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


  if (delta < 0)
    {
      drive_motor_count (r, -1, (-delta) - r->overshoot);
    }
  else if (delta > 0)
    {
      drive_motor_count (r, 1, delta - r->overshoot);
    }

  printf ("Wanted %d, got %d\n", wanted, r->pos);

  return 0;
}

int
radiator_calibrate (Radiator * r)
{
  radiator_set_pos (r, 0);

  drive_motor_count (r, 1, RUN_IN);

  r->overshoot = r->pos - RUN_IN;
}


static int utf16(uint8_t *dst,uint8_t *src)
{
int len=0;


while (*src) {
	*(dst++)=*(src++);
	len+=2;
	*(dst++)=0;
}

return len;
}


static void make_usb_descriptor(uint8_t *dst,uint8_t *src)
{
int len;
len=utf16(dst+2,src);
dst[0]=len+2;
dst[1]=0x3;
}




int
radiator_program (Radiator * r)
{
  struct cp210x_port_config config;
  uint8_t buf[256];


#if 0
  memset (&config, 0, sizeof (config));
  ioctl (r->fd, CPIOC_PORTCONFGET, &config);


  printf
    ("config was reset=(%x,%x,%x), suspend=(%x,%x,%x), enhanced_fxn=%x\n",
     config.reset.mode, config.reset.low_power, config.reset.latch,
     config.suspend.mode, config.suspend.low_power, config.suspend.latch,
     config.enhanced_fxn);


  memset (&config, 0, sizeof (config));

  config.reset.mode = ~CP_INPUT_PINS;
  config.reset.low_power = 0;
  config.reset.latch = CP_INPUT_PINS | CP_PIN_GPIO_1 | CP_PIN_GPIO_2;
  config.suspend.mode = ~CP_INPUT_PINS;
  config.suspend.low_power = 0;
  config.suspend.latch = CP_INPUT_PINS | CP_PIN_GPIO_1 | CP_PIN_GPIO_2;
  config.enhanced_fxn = CP_EFXN_ENABLE_WPU;

  ioctl (r->fd, CPIOC_PORTCONFSET, &config);

  memset (&config, 0, sizeof (config));
  ioctl (r->fd, CPIOC_PORTCONFGET, &config);


  printf
    ("config is  reset=(%x,%x,%x), suspend=(%x,%x,%x), enhanced_fxn=%x\n",
     config.reset.mode, config.reset.low_power, config.reset.latch,
     config.suspend.mode, config.suspend.low_power, config.suspend.latch,
     config.enhanced_fxn);

#endif
  memset(buf,0,sizeof(buf));

{
int i=0x413c;
  ioctl (r->fd, CPIOC_SETVID, &i);
}

{
int i=0x9500;
  ioctl (r->fd, CPIOC_SETPID, &i);
}

  make_usb_descriptor(buf,"USB Radiator Valve");
  ioctl (r->fd, CPIOC_SETPRODUCT, buf);

  make_usb_descriptor(buf,"000001");
  ioctl (r->fd, CPIOC_SETSERIAL, buf);
  
#if 0
  make_usb_descriptor(buf,"Global Panaceas");
  ioctl (r->fd, CPIOC_SETMFG, buf);
#endif


  ioctl (r->fd, CPIOC_DEVICERESET, 0);




  return 0;
}


static void tty_setup(int fd)
{
struct termios tios;

tcgetattr(fd, &tios);

cfmakeraw( &tios);

  tios.c_iflag = PARMRK | INPCK;
  tios.c_oflag = NL0 | CR0 | TAB0 | BS0 | VT0 | FF0;
  tios.c_lflag = 0;
  tios.c_cflag = CS8 | CREAD | CLOCAL;
  tios.c_cc[VMIN]=1;
  tios.c_cc[VTIME]=0;

  cfsetispeed (&tios, B115200);
  cfsetospeed (&tios, B115200);

  tcsetattr (fd, TCSANOW, &tios);
}



static void
set_nonblocking (int fd)
{
  long arg = 0;
  arg = fcntl (fd, F_GETFL, arg);
  arg |= O_NONBLOCK;
  fcntl (fd, F_SETFL, arg);
}

static void
set_blocking (int fd)
{
  long arg = 0;
  arg = fcntl (fd, F_GETFL, arg);
  arg &= ~O_NONBLOCK;
  fcntl (fd, F_SETFL, arg);
}


Radiator *
radiator_open (char *s, int quiet)
{
  Radiator *r = malloc (sizeof (Radiator));

  r->fd = open (s, O_RDWR |O_NOCTTY);

  if (r->fd < 0)
    {
      free (r);
      return NULL;
    }

  tty_setup(r->fd);
  set_nonblocking(r->fd);

  r->half = 5 * RUN_IN;
  r->overshoot = 0;
  r->max = 0;
  r->pos = 0;

  if (!quiet)
  	radiator_calibrate (r);
}

void
radiator_close (Radiator * r)
{
  close (r->fd);
  free (r);
}