/*!
  \file
  \brief パケット処理

  \author Satofumi KAMIMURA

  $Id: packet_handler.c 1687 2010-02-11 05:26:50Z satofumi $
*/

#include "packet_handler.h"
#include "serial_connection.h"


enum {
  PacketSize = 3,
  PacketType = 4,
  NumberOfData = 6,
  DataFirst = 7,

  WriteResponsePacketSize = 8,
};


static robot_t* robot_ = 0x0;


void packet_initialize(robot_t* robot)
{
  robot_ = robot;
}


static void setChecksum(char *packet, int checksum_index)
{
  char sum = 0x00;
  int i;

  for (i = 0; i < checksum_index; ++i) {
    sum += packet[i];
  }
  packet[checksum_index] = sum;
}


static void responseReadPacket(char *packet, int packet_size)
{
  char *p = &packet[DataFirst];
  int number_of_data = packet[NumberOfData];
  int i;
  int j;

  for (i = 0; i < number_of_data; ++i) {
    unsigned char offset = *p++;
    int byte_size = *p++;
    long value;

    switch (byte_size) {
    case 1:
      value = *((unsigned char*)(((unsigned char*)robot_) + offset));
      break;

    case 2:
      value = *((unsigned short*)(((unsigned char*)robot_) + offset));
      break;

    case 4:
      value = *((unsigned long*)(((unsigned char*)robot_) + offset));
      break;

    default:
      value = 0;
    }


    for (j = 0; j < byte_size; ++j) {
      *p++ = value >> (8 * (byte_size - j - 1));
    }
  }

  setChecksum(packet, packet_size - 1);
  serial_send(packet, packet_size);
}


static void responseWritePacket(char *packet)
{
  unsigned char *p = (unsigned char*)&packet[DataFirst];
  int number_of_data = packet[NumberOfData];
  int i;
  int j;

  for (i = 0; i < number_of_data; ++i) {
    int offset = *p++;
    int byte_size = *p++;
    unsigned long value = 0;

    // 32 bit の変数に代入し、その変数を所定オフセット位置に書き込む
    for (j = 0; j < byte_size; ++j) {
      value <<= 8;
      value |= *p++;
    }

    switch (byte_size) {
    case 1:
      *((unsigned char*)(((unsigned char*)robot_) + offset)) = value;
      break;

    case 2:
      *((unsigned short*)(((unsigned char*)robot_) + offset)) = value;
      break;

    case 4:
      *((unsigned long*)(((unsigned char*)robot_) + offset)) = value;
      break;
    }
  }

  packet[NumberOfData] = 0x00;
  setChecksum(packet, WriteResponsePacketSize - 1);
  serial_send(packet, WriteResponsePacketSize);
}


void packet_update(void)
{
  enum {
    BufferSize = 256,
    HeaderSize = 6,
  };
  static char buffer_[BufferSize];
  static int filled_ = 0;
  static int header_received_ = 0;
  static int packet_size_ = HeaderSize;
  int n;
  int i;

  // ヘッダ部の受信後、データ部の受信を評価できるように２回ループさせている
  for (i = 0; i < 2; ++i) {
    n = serial_receive(&buffer_[filled_], packet_size_ - filled_);
    if (n > 0) {
      filled_ += n;
    }

    if (filled_ == packet_size_) {
      if (! header_received_) {
        packet_size_ = buffer_[PacketSize];

        // !!! 最初の３文字のチェックと、チェックサムを確認すべき
        // !!! チェックに失敗したら、static 変数を初期化し、
        // !!! 通信を再開できるようにする
        // !!! 最初の３文字を探すルーチンに入るように調整すべき

        header_received_ = 1;
        continue;

      } else {
        // パケットへの応答処理

        // !!! チェックサムを確認すべき

        if (buffer_[PacketType] == 'R') {
          responseReadPacket(buffer_, packet_size_);
        } else if (buffer_[PacketType] == 'W') {
          responseWritePacket(buffer_);
        }

        filled_ = 0;
        header_received_ = 0;
        packet_size_ = HeaderSize;
      }
    }
    break;
  }
}
