#include <stdlib.h>
#include <stdio.h>
#include <termios.h>
#include <unistd.h>
#include <stdint.h>
#include <string.h>
#include <errno.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>

static void usage(void);
static int ConfigureUart(uint32_t baudrate);
int SendMessage(uint8_t *data, uint16_t size);
int ParseMessage(uint8_t *data, uint8_t size);

int fd_tty;

static char tty[50] = "/dev/ttyUSB0";
static char command[50] = "VERSION";
static char manuf[50] = "";
int initial_wait = 0;
int expected_bytes = 0;
int manuf_entered = 0;
int isWPC = 0;
int isStern = 0;
int isCapcomA = 0;
int isCapcomB = 0;
int command_read = 0;
int version = 0;

int main ( int argc, char **argv ) 
{
  int status = 0;
  uint8_t buffer[4096];
  uint8_t msg_bytes = 0;
  ssize_t bytes_read;
  int i;
  int total_bytes = 0;
  int found_sync = -1;
  char message[20];
  int profile;
  int bright;
  int value;
  int delay;
  int lamp;
  int col;
  int row;
  int retry;

    // parse arguments
    if (argc > 1)
    {
        for (argc--, argv++; argc > 0; argc--, argv++)
        {
            if (!strcmp("-d", *argv))
            {
                argc--; argv++;
                if(argc)
                {
                    strcpy(tty, *argv);
                }
                else
                {
                    usage();
                    return -1;
                }
            }
            else if (!strcmp("-w", *argv))
            {
                argc--; argv++;
                if(argc)
                {
                    initial_wait = atoi(*argv);
                }
                else
                {
                    usage();
                    return -1;
                }
            }
            else if (!strcmp("-c", *argv))
            {
                argc--; argv++;
                if(argc)
                {
                    strcpy(command, *argv);
                }
                else
                {
                    usage();
                    return -1;
                }
            }
            else if (!strcmp("-m", *argv))
            {
                argc--; argv++;
                if(argc)
                {
                    strcpy(manuf, *argv);
                    manuf_entered = 1;
                }
                else
                {
                    usage();
                    return -1;
                }
            }
            else
            {
                usage();
                return -1;
            }
        }
    }

    fd_tty = open(tty, O_RDWR);
    if (fd_tty < 0)
    {
        printf("ERROR: unable to open %s\n", tty);
        perror(tty);
        exit(-1);
    }
    else
    {
        //printf("Using %s for LED OCD interface\n", tty);
        status = ConfigureUart(9600);
    }

    if(!manuf_entered)
    {
    //query firmware version to make sure we know what we're talking to
    SendMessage("<V>", 3);

  for(i=10; i>0; i--)
  {
    bytes_read = read(fd_tty, &buffer[total_bytes], 32);
    if(bytes_read > 0)
    {
      //if(bytes_read >= 4) printf("Bytes: %i\n",bytes_read);
      total_bytes+=bytes_read;
      i=2;
      //if(total_bytes >= 5) i=0;
    }
  }
  if(total_bytes < 5)
  {
    printf("ERROR: Unable to read firmware version from LED OCD %s\n", buffer);
    exit(-1);
  }
  if((buffer[0] != '{') || (buffer[4] != '}'))
  {
    printf("ERROR: Unable to read firmware version from LED OCD  %s\n", buffer);
    exit(-1);
  }
  if(buffer[1] == 'G')
  {
    printf("ERROR: This appers to be a GI OCD.  Use different config software.\n");
    exit(-1);
  }
  version = (buffer[2] - '0') * 10 + (buffer[3] - '0');
  }

  switch(version)
  {
  case 14:
  case 21:
  case 23:
  case 24:
  case 28:
  case 29:
  case 30:
  case 31:
  case 35:
  case 36:
    //Williams, DataEast, Spooky firmware detected
    isWPC = 1;
    break;

  case 15:
  case 25:
  case 32:
  case 33:
    //Stern firmware detected
    isStern = 1;
    break;

  case 22:
  case 26:
  case 27:
  case 34:
  	if(strcmp(manuf, "CAPCOMA") == 0)
  	{
  		isCapcomA = 1;
  	}
  	else if(strcmp(manuf, "CAPCOMB") == 0)
  	{
  		isCapcomB = 1;
  	}
  	else
  	{
  		printf("Capcom detected, but matrix not specified.  Defaulting to matrix A.\n");
  		printf("Use -m CAPCOMB to communicate with matrix B.\n");
  		isCapcomA = 1;
  	}
    break;

  default:
    if(strcmp(manuf, "STERN") == 0)
    {
      isStern = 1;
    }
    else if(strcmp(manuf, "WPC") == 0)
    {
      isWPC = 1;
    }
  	else if(strcmp(manuf, "CAPCOMA") == 0)
  	{
  		isCapcomA = 1;
  	}
  	else if(strcmp(manuf, "CAPCOMB") == 0)
  	{
  		isCapcomB = 1;
  	}
    else
    {
      printf("ERROR: Unknown firmware version %i\n", version);
      printf("You can use -m STERN or similar to get around this issue\n");
      exit(-2);
    }
    break;
  }
  total_bytes = 0;

for(retry=2; retry >= 0; retry--)
{
  if(strcmp(command, "VERSION") == 0)
  {
    SendMessage("<V>", 3);
    initial_wait = 10;
  }
  else if(strncmp(command, "READSETTINGS", 4) == 0)
  {
    command_read = 1;
    SendMessage("<Q>", 3);
    initial_wait = 10;
  }
  else if(strncmp(command, "SAVESETTINGS", 4) == 0)
  {
    SendMessage("<S>", 3);
    initial_wait = 0;
  }
  else if(strncmp(command, "NORMALOCD", 4) == 0)
  {
    sprintf(message, "<Tx>");
    message[2] = 0;
    SendMessage(message, 4);
    initial_wait = 0;
  }
  else if(strncmp(command, "PASSTHROUGH", 4) == 0)
  {
    sprintf(message, "<Tx>");
    message[2] = 2;
    SendMessage(message, 4);
    initial_wait = 0;
  }
  else if(strncmp(command, "MANUAL", 4) == 0)
  {
    sprintf(message, "<Tx>");
    message[2] = 1;
    SendMessage(message, 4);
    initial_wait = 0;
  }
  else if(strncmp(command, "SETPN", 5) == 0)
  {
    //SETPD=8,2
    if((command[5] == '=') && (command[7] == ','))
    {
      profile = atoi(&command[6]);
      if(profile >=1 && profile <=8)
      {
          //printf("Valid SETPN COMMAND %i, %s\n", profile, &command[8]);
          sprintf(message, "<PxNxxxxxxxx>");
          message[2] = profile-1;
          strncpy(&message[4], &command[8], 8);
          SendMessage(message, 13);
          initial_wait = 0;
      }
      else printf("Invalid SETPN command: profile must be (1-8)\n");
    }
    else printf("Invalid SETPN command: SETPN=8,LED_100%%\n");
  }
  else if(strncmp(command, "SETPB", 5) == 0)
  {
    //SETPB=8,2,14
    if((command[5] == '=') && (command[7] == ',') && (command[9] == ','))
    {
      profile = atoi(&command[6]);
      bright = atoi(&command[8]);
      value = atoi(&command[10]);
      if(profile >=1 && profile <=8)
      {
        if(bright >= 1 && bright <= 8)
        {
          if(value >= 0 && value <= 100)
          {
            //62 is the same as '>', so it causes parsing errors on the LED OCD firmware
            if(value == 62) value = 63;
            //printf("Valid SETPB COMMAND %i, %i, %i\n", profile, bright, value);
            sprintf(message, "<PxBxVx>");
            message[2] = profile-1;
            message[4] = bright;
            message[6] = value;
            SendMessage(message, 8);
            initial_wait = 0;
          }
          else printf("Invalid SETPB command: value must be (0-100)\n");
        }
        else printf("Invalid SETPB command: bright must be (1-8)\n");
      }
      else printf("Invalid SETPB command: profile must be (1-8)\n");
    }
    else printf("Invalid SETPB command: SETPB=3,5,79\n");
  }
  else if(strncmp(command, "SETPD", 5) == 0)
  {
    //SETPD=8,2
    if((command[5] == '=') && (command[7] == ','))
    {
      profile = atoi(&command[6]);
      delay = atoi(&command[8]);
      if(profile >=1 && profile <=8)
      {
        if(delay >= 0 && delay <= 9)
        {
          //printf("Valid SETPD COMMAND %i, %i\n", profile, delay);
          sprintf(message, "<PxDx>");
          message[2] = profile-1;
          message[4] = delay;
          SendMessage(message, 6);
          initial_wait = 0;
        }
        else printf("Invalid SETPD command: delay must be (0-9)\n");
      }
      else printf("Invalid SETPD command: profile must be (1-8)\n");
    }
    else printf("Invalid SETPD command: SETPB=3,5\n");
  }
  else if(strncmp(command, "SETLP", 5) == 0)
  {
    //SETLP=8,2
    int maxrow = (isStern ? 10 : 8);

    if((command[5] == '=') && ((command[7] == ',') || (command[8] == ',')))
    {
      lamp = atoi(&command[6]);
      if(command[7] == ',') profile = atoi(&command[8]); 
      else profile = atoi(&command[9]);
      if(profile >=1 && profile <=8)
      {
        if(lamp >= 1 && lamp <= 88)
        {
          if(isStern == 0)
          {
            col = (lamp / 10);
            row = (lamp % 10);
          }
          else if(isStern == 1)
          {
            col = ((lamp - 1) % 8) + 1;
            row = ((lamp - 1) / 8) + 1;
          }
	        if(row >= 1 && row <= maxrow)
	        {
		        if(col >= 1 && col <= 8)
		        {
		          //printf("Valid SETLP COMMAND %i, %i, %i, %i\n", lamp, col, row, profile);
		          sprintf(message, "<LxxPx>");
		          message[2] = col-1;
		          message[3] = row-1;
		          message[5] = profile-1;
		          SendMessage(message, 7);
		          initial_wait = 0;
		        }
            else printf("Invalid SETLP command: col must be (1-8)\n");
	        }
          else printf("Invalid SETLP command: row must be (1-%i)\n", maxrow);
        }
        else printf("Invalid SETLP command: lamp must be (1-88)\n");
      }
      else printf("Invalid SETLP command: profile must be (1-8)\n");
    }
    else printf("Invalid SETLP command: SETLP=75,5\n");
  }
  else if(strncmp(command, "SETLB", 5) == 0)
  {
    //SETLB=8,2
    int maxrow = (isStern ? 10 : 8);

    if((command[5] == '=') && ((command[7] == ',') || (command[8] == ',')))
    {
      lamp = atoi(&command[6]);
      if(command[7] == ',') bright = atoi(&command[8]);
      else bright = atoi(&command[9]);
      if(bright >=0 && bright <=8)
      {
        if(lamp >= 1 && lamp <= 88)
        {
          if(isStern == 0)
          {
            col = (lamp / 10);
            row = (lamp % 10);
          }
          else if(isStern == 1)
          {
            col = ((lamp - 1) % 8) + 1;
            row = ((lamp - 1) / 8) + 1;
          }
                if(row >= 1 && row <= maxrow)
                {
                        if(col >= 1 && col <= 8)
                        {
                          sprintf(message, "<LxxBx>");
                          message[2] = col-1;
                          message[3] = row-1;
                          message[5] = bright;
                          SendMessage(message, 7);
                          initial_wait = 0;
                        }
            else printf("Invalid SETLB command: col must be (1-8)\n");
                }
          else printf("Invalid SETLB command: row must be (1-%i)\n", maxrow);
        }
        else printf("Invalid SETLB command: lamp must be (1-88)\n");
      }
      else printf("Invalid SETLB command: bright must be (0-8)\n");
    }
    else printf("Invalid SETLB command: SETLB=75,5\n");
  }
  else if(strncmp(command, "CHECK", 5) == 0)
  {
    int maxrow = (isStern ? 10 : 8);

    //change to manual mode
    sprintf(message, "<Tx>");
    message[2] = 1;
    SendMessage(message, 4);
    initial_wait = 0;

    //draw the checkboard
                for(col=1; col<=8; col++)
                {
                        for(row=1; row<=maxrow; row++)
                        {
                          sprintf(message, "<LxxBx>");
                          message[2] = col-1;
                          message[3] = row-1;
                          message[5] = ((col+row)%2)*8;
                          SendMessage(message, 7);
                          initial_wait = 0;
                        }
                }
  }
  for(i=initial_wait; i>0; i--)
  {
    bytes_read = read(fd_tty, &buffer[total_bytes], 32);
    if(bytes_read > 0)
    {
      //if(bytes_read >= 4) printf("Bytes: %i\n",bytes_read);
      total_bytes+=bytes_read;
      i=2;
	    if(total_bytes > 4000) i=0;
    }
    else
    {
      //printf("Nothing read\n");
      //usleep(10000);
    }
  }

  if(command_read)
  {
switch(total_bytes)
{
case 1118: //valid Williams
  if(isWPC) retry = 0;
  break;

case 1230: //valid Stern
	if(isStern) retry = 0;
  break;
  
case 1127: //valid CapcomA
case 1124: //valid CapcomA
	if(isCapcomB) retry = 0;
  break;
  
case 1133: //valid CapcomB
case 1130: //valid CapcomB
	if(isCapcomB) retry = 0;
  break;
  
default:
  //printf("Invalid number of bytes (%i) read\n", total_bytes);
  if(retry == 0)
  {
    printf("ERROR: Unable to read valid data from board\n");
  }
  total_bytes = 0;
  break;
}
  }
  else retry = 0;
} //end of big retry for loop

  for(i=0; i<total_bytes; i++)
  {
      if(found_sync == -1)
      {
        if(buffer[i] == '{')
        {
          found_sync = i;
        }
      }
      else if(buffer[i] == '}')
      {
        ParseMessage(&buffer[found_sync], i-found_sync+1);
        found_sync = -1;
      }
      if(i-found_sync+1 >= 20) found_sync = -1;
  }
  
  return 0;
}

int ParseMessage(uint8_t *data, uint8_t size)
{
  int i;
  uint8_t temp;

  switch(data[1])
  {
  case 'Z':
    if(size == 6)
    {
      //printf("WATCHDOG=%c%c%c\n", data[2], data[3], data[4]);
    }
    break;
  case 'V':
    if(size == 5)
    {
      printf("LED OCD Firmware v%c%c\n", data[2], data[3]);
    }
    break;
  case 'G':
    if(size == 5)
    {
      printf("GI OCD Firmware v%c%c\n", data[2], data[3]);
    }
    break;
  case 'L':
    if((data[4] == 'P') && (size == 7))
    {
      if(isStern > 0)
      {
        temp = data[3] * 8 + data[2] + 1;
      }
      else
      {
        temp = ((data[2] + 1) * 10) + (data[3] + 1);
      }
      printf("L%02iP=%i\n", temp, data[5] + 1);
    }
    break;
  case 'P':
    if((data[3] == 'N') && (size == 13))
    {
      printf("P%iNAME=",data[2] + 1);
      for(i=0; i<8; i++)
      {
        if(data[i+4] != 0) printf("%c", data[i+4]);
      }
      printf("\n");
    }
    if((data[3] == 'D') && (size == 6))
    {
      printf("P%iDELAY=%i\n", data[2] + 1, data[4]);
    }
    if((data[3] == 'B') && (data[5] == 'V') && (size == 8))
    {
      printf("P%iB%i=%i\n",data[2] + 1, data[4] + 1, data[6]);
    }
    break;
  default:
    //printf("Read: ");
    for(i=0; i<size; i++)
    {
      //printf("%02x ", data[i]);
    }
    //printf("\n");
    break;
  }
}

/*****************************************************************************
* Function Name: ConfigureUart
****************************************************************************/
static int ConfigureUart(uint32_t baudrate)
{
    struct termios options;
    uint32_t baud_macro;
   
    switch(baudrate)
    {
    case 9600:
        baud_macro = B9600;
        break;

    case 19200:
        baud_macro = B19200;
        break;

    case 57600:
        baud_macro = B57600;
        break;

    case 115200:
        baud_macro = B115200;
        break;

    default:
        return -1;
    }
    
    // get the current options
    tcgetattr(fd_tty, &options);
    //bzero(&options, sizeof(options));
   
    options.c_iflag = 0;
    options.c_iflag |= IGNPAR;

    options.c_cflag |= CLOCAL; /* No modem control lines */
    options.c_cflag |= CREAD;  /* Enable serial receiver */
    options.c_cflag &= ~CRTSCTS;  /* Disable handshake */

    //baud rate
    //options.c_cflag = options.c_cflag & ~CBAUD;
    //options.c_cflag = options.c_cflag & ~CBAUDEX;
    //options.c_cflag |= baud_macro;
    cfsetispeed(&options, baud_macro);
    cfsetospeed(&options, baud_macro);

    //no parity
    options.c_cflag &= ~PARENB;

    //stop bit(s)
    options.c_cflag &= ~CSTOPB; //1
    //options.c_cflag |= CSTOPB; //2

    //8 bits
    options.c_cflag &= ~CSIZE;
    options.c_cflag |= CS8;

    //raw output
    options.c_oflag = 0;
    //raw local handling
    options.c_lflag = 0;

    //return from read after VMIN consecutive chars
    options.c_cc[VMIN] = 0;
    //return from read after VTIME tenths of a second between chars
    options.c_cc[VTIME] = 4;
   
    // Clear the line
    tcflush(fd_tty,TCIFLUSH);
   
    // Update the options and do it NOW
    if (tcsetattr(fd_tty,TCSANOW,&options) != 0)
    {
        perror("SetUartBaud");
        return -2;
    }
   
    return 0;
}

int SendMessage(uint8_t *data, uint16_t size)
{
    int status = 0;
    ssize_t write_size;
    uint8_t *ptr = data;
    int i;
    char relay[3];
    
    relay[0] = '<';
    relay[1] = '1';
    relay[2] = '>';
 
    if(isCapcomB)
    {
    	for(i=0; i<3; i++)
	    {
	      if((write_size = write(fd_tty, &relay[i], size)) < 0)
	      {
	        printf("ERROR: write failed\n");
	        status = -1;
	      }
	      //usleep(1000);
	    }
	    usleep(40000);
	  }    

    for(i=0; i<size; i++)
    {
      if((write_size = write(fd_tty, ptr++, size)) < 0)
      {
        printf("ERROR: write failed\n");
        status = -1;
      }
      //usleep(1000);
    }    
    usleep(40000);
    return status;
}

/***************************************************************************
* FUNCTION: usage
* 
* DESCRIPTION: Displays information about how to use the application
*
* INPUTS: 
*
***************************************************************************/
static void usage(void)
{
    printf("\nUsage: ledocd_cli [options]\n");
    printf(" -d <port>          tty for LED OCD interface (default /dev/ttyUSB0)\n");
    printf(" -c <command>       set the command to send (default VERSION)\n");
    printf("   Valid commands:\n");
    printf("   VERSION - read firmware version\n");
    printf("   READ    - read settings\n");
    printf("   SETPN   - set profile name\n");
    printf("   SETPB   - set profile brightness\n");
    printf("   SETPD   - set profile delay\n");
    printf("   SETLP   - set lamp profile\n");
    printf("   SETLB   - set lamp brightness (in manual mode)\n");
    printf("   SAVE    - save settings\n");
    printf("   NORMAL  - normal OCD\n");
    printf("   MANUAL  - manual control mode\n");
    printf("   PASS    - passthrough mode\n");
    printf("   CHECK   - change to manual and draw checkerboard pattern\n");
    printf(" -m <manufacturer>  set the game manufacturer (default WPC)\n");
    printf("   Valid manufacturers:\n");
    printf("   WPC     - Williams, Data East, or Spooky\n");
    printf("   STERN   - Stern\n");
    printf("   CAPCOMA - Capcom (matrix A)\n");
    printf("   CAPCOMB - Capcom (matrix B)\n");
    printf("\n");
    printf("Examples:\n");
    printf(" ledocd_cli\n");
    printf(" ledocd_cli -d /dev/ttyUSB3 -m CAPCOMA -c SETLP=11,3\n");
    printf("\n");
}



