//
// begin license header
//
// This file is part of Pixy CMUcam5 or "Pixy" for short
//
// All Pixy source code is provided under the terms of the
// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
// Those wishing to use Pixy source code, software and/or
// technologies under different licensing terms should contact us at
// cmucam@cs.cmu.edu. Such licensing terms are available for
// all portions of the Pixy codebase presented here.
//
// end license header
//

#include <string.h>
#include <stdio.h>
#include "pixyinterpreter.hpp"

PixyInterpreter::PixyInterpreter()
{
  init_ = false;
  receiver_    = NULL;
}

PixyInterpreter::~PixyInterpreter()
{
  close();
}

int PixyInterpreter::init()
{
  int USB_return_value;

  if(init_ == true)
  {
    fprintf(stderr, "libpixy: Already initialized.");
    return 0;
  }

  USB_return_value = link_.open();

  if(USB_return_value < 0) {
    return USB_return_value;
  }

  receiver_ = new ChirpReceiver(&link_, this);

  init_ = true;

  return 0;
}

void PixyInterpreter::close()
{  
  if (receiver_)  
  {
    delete receiver_;
    receiver_ = NULL;
  }
  init_ = false;
}

int PixyInterpreter::get_blocks(int max_blocks, Block * blocks)
{
  uint16_t number_of_blocks_to_copy;
  uint16_t index;

  // Check parameters //

  if(max_blocks < 0 || blocks == 0) {
    return PIXY_ERROR_INVALID_PARAMETER;
  }

  number_of_blocks_to_copy = (max_blocks >= blocks_.size() ? blocks_.size() : max_blocks);
  
  // Copy blocks //
  
  for (index = 0; index != number_of_blocks_to_copy; ++index) {
    memcpy(&blocks[index], &blocks_[index], sizeof(Block));
  }

  blocks_are_new_ = false;

  return number_of_blocks_to_copy;
}

int PixyInterpreter::send_command(const char * name, ...)
{
  va_list arguments;
  int     return_value;

  va_start(arguments, name);
  return_value = send_command(name, arguments);
  va_end(arguments);

  return return_value;
}

int PixyInterpreter::send_command(const char * name, va_list args)
{
  ChirpProc procedure_id;
  int       return_value;
  va_list   arguments;

  va_copy(arguments, args);

  // Request chirp procedure id for 'name'. //
  procedure_id = receiver_->getProc(name);

  // Was there an error requesting procedure id? //
  if (procedure_id < 0) {
    // Request error //
    va_end(arguments);
    return PIXY_ERROR_INVALID_COMMAND;
  }

  // Execute chirp synchronous remote procedure call //
  return_value = receiver_->call(SYNC, procedure_id, arguments); 
  va_end(arguments);

  return return_value;
}

int PixyInterpreter::service()
{
    if(!init_) return -1;
    receiver_->service(false);
    return 0; //success
}


void PixyInterpreter::interpret_data(const void * chirp_data[])
{
  uint8_t  chirp_message;
  uint32_t chirp_type;
  if (chirp_data[0]) {

    chirp_message = Chirp::getType(chirp_data[0]);

    switch(chirp_message) {
      
      case CRP_TYPE_HINT:
        
        chirp_type = * static_cast<const uint32_t *>(chirp_data[0]);

        switch(chirp_type) {

          case FOURCC('B', 'A', '8', '1'):
            break;
          case FOURCC('C', 'C', 'Q', '1'):
            break;
          case FOURCC('C', 'C', 'B', '1'):
            interpret_CCB1(chirp_data + 1);
            break;
          case FOURCC('C', 'C', 'B', '2'):
            interpret_CCB2(chirp_data + 1);
            break;
          case FOURCC('C', 'M', 'V', '1'):
            break;
          default:
            fprintf(stderr,"libpixy: Chirp hint [%u] not recognized.\n", chirp_type);
            break;
        }

        break;

      case CRP_HSTRING:

        break;
      
      default:
       
       fprintf(stderr, "libpixy: Unknown message received from Pixy: [%u]\n", chirp_message);
       break;
    }
  } 
}

void PixyInterpreter::interpret_CCB1(const void * CCB1_data[])
{
  uint32_t       number_of_blobs;
  const BlobA *  blobs;
  
  // Add blocks with normal signatures //
  
  number_of_blobs = * static_cast<const uint32_t *>(CCB1_data[3]);
  blobs           = static_cast<const BlobA *>(CCB1_data[4]);
  
  number_of_blobs /= sizeof(BlobA) / sizeof(uint16_t);

  add_normal_blocks(blobs, number_of_blobs);
  blocks_are_new_ = true;
}


void PixyInterpreter::interpret_CCB2(const void * CCB2_data[])
{
  uint32_t       number_of_blobs;
  const BlobA *  A_blobs;
  const BlobB *  B_blobs;

  // The blocks container will only contain the newest //
  // blocks                                            //
  blocks_.clear();

  // Add blocks with color code signatures //

  number_of_blobs = * static_cast<const uint32_t *>(CCB2_data[5]);
  B_blobs         = static_cast<const BlobB *>(CCB2_data[6]);
  
  number_of_blobs /= sizeof(BlobB) / sizeof(uint16_t);
  add_color_code_blocks(B_blobs, number_of_blobs);

  // Add blocks with normal signatures //

  number_of_blobs = * static_cast<const uint32_t *>(CCB2_data[3]);
  A_blobs         = static_cast<const BlobA *>(CCB2_data[4]);
  
  number_of_blobs /= sizeof(BlobA) / sizeof(uint16_t);
  
  add_normal_blocks(A_blobs, number_of_blobs);
  blocks_are_new_ = true;
}

void PixyInterpreter::add_normal_blocks(const BlobA * blocks, uint32_t count)
{
  uint32_t index;
  Block    block;

  for (index = 0; index != count; ++index) {

    // Decode CCB1 'Normal' Signature Type //

    block.type      = PIXY_BLOCKTYPE_NORMAL;
    block.signature = blocks[index].m_model;
    block.width     = blocks[index].m_right - blocks[index].m_left;
    block.height    = blocks[index].m_bottom - blocks[index].m_top;
    block.x         = blocks[index].m_left + block.width / 2;
    block.y         = blocks[index].m_top + block.height / 2;

    // Angle is not a valid parameter for 'Normal'  //
    // signature types. Setting to zero by default. //
    block.angle     = 0;
      
    // Store new block in block buffer //

    if (blocks_.size() == PIXY_BLOCK_CAPACITY) {
      // Blocks buffer is full - replace oldest received block with newest block //
      blocks_.erase(blocks_.begin());
      blocks_.push_back(block);
    } else {
      // Add new block to blocks buffer //
      blocks_.push_back(block);
    }
  }
}

void PixyInterpreter::add_color_code_blocks(const BlobB * blocks, uint32_t count)
{
  uint32_t index;
  Block    block;
    
  for (index = 0; index != count; ++index) {

    // Decode 'Color Code' Signature Type //

    block.type      = PIXY_BLOCKTYPE_COLOR_CODE;
    block.signature = blocks[index].m_model;
    block.width     = blocks[index].m_right - blocks[index].m_left;
    block.height    = blocks[index].m_bottom - blocks[index].m_top;
    block.x         = blocks[index].m_left + block.width / 2;
    block.y         = blocks[index].m_top + block.height / 2;
    block.angle     = blocks[index].m_angle;

    // Store new block in block buffer //

    if (blocks_.size() == PIXY_BLOCK_CAPACITY) {
      // Blocks buffer is full - replace oldest received block with newest block //
      blocks_.erase(blocks_.begin());
      blocks_.push_back(block);
    } else {
      // Add new block to blocks buffer //
      blocks_.push_back(block);
    }
  }
}

int PixyInterpreter::blocks_are_new()
{
  if (blocks_are_new_) {
    // Fresh blocks!! :D //
    return 1;
  } else {
    // Stale blocks... :\ //
    return 0;
  }
}