/*
 * Copyright 2009-2010  Stefan Gehn <stefan@srcbox.net>
 * 
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as
 * published by the Free Software Foundation; either version 2 of 
 * the License, or (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "Packet.hpp"
#include <qbytearray.h>
#include <qdebug.h>

namespace IrTouch
{

bool Packet::sCorrectXPos = false;
bool Packet::sFlipYPos = false;
bool Packet::sFlipXPos = false;
bool Packet::sCorrectYPos = false;
QMatrix Packet::sCalibrationMatrix = QMatrix();

Packet::Packet()
{
  clear();
}

Packet::Packet(const QTime &timestamp,
				 const QList<RawValue> &rawXValues,
				 const QList<RawValue> &rawYValues)
{
	mState = Finished;
	mCrc = 0;
	mCols = rawXValues.count();
	mRows = rawYValues.count();
	mTimestamp = timestamp;
	mRawXValues = rawXValues;
	mRawYValues = rawYValues;
}


void Packet::clear()
{
  mState = Head;
  mCrc = 0x55;
  mRows = 0;
  mCols = 0;
  mRawXValues.clear();
  mRawYValues.clear();
}

bool Packet::parse(QByteArray &data)
{
  bool continueParsing = true;

  // read as much as possible from data array
  // loop is left as soon as a parse method returns false (not enough data!)
  while (continueParsing)
  {
    switch (mState)
    {
    case Head:
      continueParsing = parseHead(data);
      break;

    case PacketSize:
      continueParsing = parsePacketSize(data);
      break;

    case Payload:
      continueParsing = parsePayload(data);
      break;

    case CRC:
      continueParsing = parseCrc(data);
      break;

    case Finished:
      qWarning() << "IrPacket::parse() called although parsing already finished";
      continueParsing = false;
      break;

    case Invalid:
      qWarning() << "IrPacket::parse() called in invalid state";
      continueParsing = false;
      break;
    }
  }

  return (mState == Finished); // return if parsing is finished already
}

char Packet::calcCrc(const QByteArray &data, int len) // static
{
  Q_ASSERT(len <= data.size());
  char val = 0;
  for (int i = 0; i < len; ++i)
  {
    val += data.at(i); // possible overflow is on purpose!
  }
  return val;
}

bool Packet::parseHead(QByteArray &data)
{
  // first byte: 0xAA, start of every packet
  //qDebug() << "parseHead()";

  if (data.size() < 1)
    return false;

  unsigned char startByte = (unsigned char) data.at(0);
  if (startByte == 0xAA)
  {
    //qDebug() << "Found start-byte 0xAA";
    mCrc += Packet::calcCrc(data, 1);
    mState = PacketSize;
  }
  else
  {
    qWarning("Illegal start-byte 0x%X", (unsigned char) data.at(0));
    // Sync to the start byte and ignore
    // anything read before (usually garbage or data received on the
    // serial port before we started reading)
  }

  data.remove(0, 1);
  return true;
}

bool Packet::parsePacketSize(QByteArray &data)
{
  // first byte: number of columns (X values) following
  // second byte: number of rows (Y values) following

  //qDebug() << "IrPacket::parsePacketSize()";

  if (data.size() < 2)
    return false;

  mCrc += Packet::calcCrc(data, 2);

  mCols = (unsigned char) data.at(0);
  mRows = (unsigned char) data.at(1);
  //qDebug() << "packet has " << mCols << "columns and " << mRows << "rows";

  data.remove(0, 2);
  mState = Payload;
  return true;
}

bool Packet::parsePayload(QByteArray &data)
{
  // byte 1: first X value (lower 8 bit)
  // byte 2: first X value (upper 8 bit)
  // byte 3: first X extent/width (lower 8 bit)
  // byte 4: first Y extent/width (upper 8 bit)
  // ... (repeated for numCols times) ...
  // ... (same data for numRows times) ...

  int cnt = mCols * 4 + mRows * 4;

  //qDebug() << "IrPacket::parsePayload()";

  // packet without rows and columns means "end of touch". We can
  // simply go on and parse the CRC
  if (cnt > 0)
  {
		// Not enough data passed yet, try again later
    if (data.size() < cnt)
      return false;

    //qDebug() << "Parsing" << cnt << "bytes of payload";
    mCrc += Packet::calcCrc(data, cnt);

    int i = 0;
    while (mCols-- > 0)
    {
      int val = (unsigned char) data.at(i) | ((unsigned char) data.at(i + 1) << 8);
      int ext = (unsigned char) data.at(i + 2) | ((unsigned char) data.at(i + 3) << 8);
      i += 4;

      if (Packet::sFlipXPos)
        val = 4095 - val;

      if (Packet::sCorrectXPos)
        val -= ext;

      val += sCalibrationMatrix.dx();
      val *= sCalibrationMatrix.m11();

      ext *= sCalibrationMatrix.m11();

      if (ext < 1)
        continue;

      if (val < 0)
      {
        ext += val;
        if (ext < 1)
          continue;
        val = 0;
      }

      if (Packet::sFlipXPos)
        mRawXValues.prepend(RawValue(val, ext));
      else
        mRawXValues.append(RawValue(val, ext));
    }

    while (mRows-- > 0)
    {
      qint16 val = (unsigned char) data.at(i) | ((unsigned char) data.at(i + 1) << 8);
      qint16 ext = (unsigned char) data.at(i + 2) | ((unsigned char) data.at(i + 3) << 8);
      i += 4;

      if (Packet::sFlipYPos)
        val = 4095 - val;

      if (Packet::sCorrectYPos)
        val -= ext;

      val += sCalibrationMatrix.dy();
      val *= sCalibrationMatrix.m22();

      ext *= sCalibrationMatrix.m22();

      if (ext < 1)
        continue;

      if (val < 0)
      {
        ext += val;
        if (ext < 1)
          continue;
        val = 0;
      }

      if (Packet::sFlipYPos)
        mRawYValues.prepend(RawValue(val, ext));
      else
        mRawYValues.append(RawValue(val, ext));
    }

    data.remove(0, cnt);
  }

  mState = CRC;
  return true;
}

bool Packet::parseCrc(QByteArray &data)
{
  // byte 1: CRC which is 0x55 + sum(all previous bytes)
  //
  // NOTE: mCrc is summed up before by calling calcCrc() with the
  // incoming bytearray

  //qDebug() << "IrPacket::parseCrc()";

  if (data.size() < 1)
    return false;

  unsigned char crcByte = data.at(0);

  mState = (crcByte == mCrc) ? Finished : Invalid;
	if (mState == Finished)
	{
		mTimestamp = QTime::currentTime();
	}
	else
  {
    qWarning("CRC error, received 0x%X, calculated 0x%X", crcByte, mCrc);
    clear(); // Restart from scratch
  }

  data.remove(0, 1);
  return false;
} // CRC

} // namespace IrTouch




QDataStream &operator<<(QDataStream &ds, const IrTouch::Packet &packet)
{
	// Only serialize to stream if packet contains valid data
	Q_ASSERT(packet.isValid());

	ds << packet.timestamp();
	ds << packet.rawXValues();
	ds << packet.rawYValues();

	return ds;
}

QDataStream &operator>>(QDataStream &ds, IrTouch::Packet &packet)
{
	QTime ts;
	QList<IrTouch::RawValue> xv;
	QList<IrTouch::RawValue> yv;

	ds >> ts;
	ds >> xv;
	ds >> yv;

	Q_ASSERT(ts.isValid());

	packet = IrTouch::Packet(ts, xv, yv);

	return ds;
}

QDataStream &operator<<(QDataStream &ds, const IrTouch::RawValue &rv)
{
	ds << rv.value();
	ds << rv.extent();
	return ds;
}

QDataStream &operator>>(QDataStream &ds, IrTouch::RawValue &rv)
{
	qint16 v,e;

	ds >> v;
	ds >> e;

	rv = IrTouch::RawValue(v, e);
	return ds;
}