#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 <unistd.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(Radiator *r)
{
  int i=BRAKE;
  printf (" Motor braking\n");
  return cp210x_set_gpio(r->cp210x,i);
}

static int
drive_motor (Radiator*r, 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 cp210x_set_gpio(r->cp210x,i);
}


static int
sensor_led_power (Radiator *r, int on)
{

unsigned int control;

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

return cp210x_set_dtr(r->cp210x,on);

}


static int count_steps (Radiator *r, int count)
{
  int counted = 0;
  struct timeval tv;
  char buf[128];
  int i;
  fd_set rfds;

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


  while (count)
    {
	i=cp210x_read(r->cp210x,buf,sizeof(buf),TIMEOUT);

	if (i==0) break;

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




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

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

void
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;
}
len+=2;

return len;
}


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


int
radiator_program (Radiator * r)
{
  uint8_t buf[256];
  struct cp210x_port_config config;
  memset (&config, 0, sizeof (config));
  uint16_t vid,pid;
  int len;


  cp210x_get_portconf(r->cp210x,&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;


  cp210x_set_portconf(r->cp210x,&config);

  memset (&config, 0, sizeof (config));
  cp210x_get_portconf(r->cp210x,&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);

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

vid=0x413c;

  cp210x_set_vid(r->cp210x,vid);

pid=0x9500;
  cp210x_set_pid(r->cp210x,pid);

  len=make_usb_descriptor(buf,"USB Radiator Valve");
  cp210x_set_product(r->cp210x, buf,len);

  len=make_usb_descriptor(buf,"000001");
  cp210x_set_serial (r->cp210x,buf,len);
  
  len=make_usb_descriptor(buf,"Global Panaceas");
  cp210x_set_mfg (r->cp210x,buf,len);

 cp210x_reset(r->cp210x);

  return 0;
}

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


  r->cp210x=cp210x_open();

  if (!r->cp210x) {
	free(r);
	return NULL;
  }


  if (!quiet)
  	radiator_calibrate (r);

  return r;
}


void
radiator_close (Radiator * r)
{
  cp210x_close(r->cp210x);
  free (r);
}