#include <stdio.h>
#include <string.h>
#include <stdarg.h>

#include "logutils.h"
#include "ioutils.h"
#include "orcutils.h"
#include "orcd.h"

#define LOGCHANNEL "ORCUTIL"

unsigned char checksum(void *cin, int len)
{
  return checksum(0, cin, len);
}

unsigned char checksum(unsigned char chk, void *cin, int len)
{
  unsigned char *c=(unsigned char*) cin;

  for (int i=0;i<len;i++)
    chk=(chk<<1)+c[i]+(chk&0x80 ? 1 : 0);

  return chk;
}

void getMasterData(orcd_t *o)
{
  unsigned char cmd[ORC_MAX_PACKETSIZE];
  
  cmd[0]=237;
  cmd[1]=5;
  cmd[2]=HOSTMASTER;
  cmd[3]='*';
  cmd[4]=checksum(cmd, cmd[1]-1);

  doTransaction(o, cmd, cmd[1], NULL);
}

void lcdHome(orcd_t *o)
{
  unsigned char cmd[ORC_MAX_PACKETSIZE];
  
  cmd[0]=237;
  cmd[1]=5;
  cmd[2]=HOSTLCD;
  cmd[3]='C';
  cmd[4]=checksum(cmd, cmd[1]-1);

  doTransaction(o, cmd, cmd[1], NULL);
}

void lcdFill(orcd_t *o, unsigned char a, unsigned char b)
{
  unsigned char cmd[ORC_MAX_PACKETSIZE];
  
  cmd[0]=237;
  cmd[1]=7;
  cmd[2]=HOSTLCD;
  cmd[3]='F';
  cmd[4]=a;
  cmd[5]=b;
  cmd[6]=checksum(cmd, cmd[1]-1);

  doTransaction(o, cmd, cmd[1], NULL);
}

void lcdPrint(orcd_t *o, int x, int y, char font, const char *format, ...)
{
  unsigned char cmd[ORC_MAX_PACKETSIZE];
  va_list ap;
  int len;

  cmd[0]=237;
  cmd[1]=0xff; // length will be filled in later
  cmd[2]=HOSTLCD;
  cmd[3]='T';
  cmd[4]=x;
  cmd[5]=y;
  cmd[6]=font;

  va_start(ap, format);
  len=vsnprintf((char*) &cmd[7], 127, format, ap);
  va_end(ap);

  cmd[1]=8+len;
  cmd[7+len]=checksum(cmd, cmd[1]-1);

  doTransaction(o, cmd, cmd[1], NULL);
}

void lcdConsole(orcd_t *o, const char *format, ...)
{
  unsigned char cmd[ORC_MAX_PACKETSIZE];
  va_list ap;
  int len;

  cmd[0]=237;
  cmd[1]=0xff; // length will be filled in later
  cmd[2]=HOSTLCD;
  cmd[3]='W';

  va_start(ap, format);
  len=vsnprintf((char*) &cmd[4], 127, format, ap);
  va_end(ap);

  cmd[1]=5+len;
  cmd[4+len]=checksum(cmd, cmd[1]-1);

  doTransaction(o, cmd, cmd[1], NULL);
}

void lcdConsoleGoto(orcd_t *o, int x, int y)
{
  unsigned char cmd[ORC_MAX_PACKETSIZE];

  cmd[0]=237;
  cmd[1]=7;
  cmd[2]=HOSTLCD;
  cmd[3]='G';
  cmd[4]=x;
  cmd[5]=y;
  cmd[6]=checksum(cmd, cmd[1]-1);

  doTransaction(o, cmd, cmd[1], NULL);
}

void lcdTopPrint(orcd_t *o, int x, const char *format, ...)
{
  unsigned char cmd[ORC_MAX_PACKETSIZE];
  va_list ap;
  int len;

  cmd[0]=237;
  cmd[1]=0xff; // length will be filled in later
  cmd[2]=HOSTLCD;
  cmd[3]='t';
  cmd[4]=x;

  va_start(ap, format);
  len=vsnprintf((char*) &cmd[5], 127, format, ap);
  va_end(ap);

  cmd[1]=6+len;
  cmd[5+len]=checksum(cmd, cmd[1]-1);

  doTransaction(o, cmd, cmd[1], NULL);

}

void lcdRawWrite(orcd_t *o, int x, int y, char *buf, int buflen)
{
  unsigned char cmd[ORC_MAX_PACKETSIZE];

  cmd[0]=237;
  cmd[1]=7+buflen;
  cmd[2]=HOSTLCD;
  cmd[3]='R';
  cmd[4]=x;
  cmd[5]=y;

  memcpy(&cmd[6], buf, buflen);

  cmd[6+buflen]=checksum(cmd, cmd[1]-1);

  doTransaction(o, cmd, cmd[1], NULL);
}


/*
int keyPoll(orcd_t *orcd)
{
  int v;

  pthread_mutex_lock(&orcd->keymutex);
  v=orcd->arrowdata | orcd->buttondata<<3;
  orcd->arrowdata=0;
  orcd->buttondata=0;
  pthread_mutex_unlock(&orcd->keymutex);

  return v;
}

int keyWait(orcd_t *orcd)
{
  int v;
  pthread_mutex_lock(&orcd->keymutex);

  orcd->arrowdata=0;
  orcd->buttondata=0;

  pthread_cond_wait(&orcd->keycond, &orcd->keymutex);
  v=orcd->arrowdata | (orcd->buttondata<<3);
  orcd->arrowdata=0;
  orcd->buttondata=0;
  pthread_mutex_unlock(&orcd->keymutex);

  while(keyPoll(orcd)!=0);

  return v;
}
*/