#ifndef ENCODER_C
#define ENCODER_C

//----------------------------------
// Includes and forward declarations
//----------------------------------
#include <cyg/io/encoder.h>
#include <cyg/io/devtab.h>
#include <cyg/io/io.h>

//--------------------
// Function prototypes
//--------------------
static Cyg_ErrNo encoder_read(cyg_io_handle_t a_handle, void * a_buf, cyg_uint32 * a_len);
static Cyg_ErrNo encoder_set_config(cyg_io_handle_t a_handle, cyg_uint32 a_key, const void * a_buf, cyg_uint32 * a_len);
static Cyg_ErrNo encoder_get_config(cyg_io_handle_t a_handle, cyg_uint32 a_key, void * a_buf, cyg_uint32 * a_len);

//------------------------------------
// Register the driver with the kernel
//------------------------------------
DEVIO_TABLE(encoder_devio,
            0,
            encoder_read,
	    0,
            encoder_get_config,
            encoder_set_config);

//-----------------------
// The callback functions
//-----------------------
static void encoder_init(encoder_channel * a_channel, bool a_force);
static void encoder_latch(encoder_channel * a_channel, cyg_uint16 a_count);
ENCODER_CALLBACKS(encoder_callbacks,
                  encoder_init,
                  encoder_latch);

//-------------------------
// Function implementations
//-------------------------
// encoder_init
static void encoder_init(encoder_channel * a_channel, bool a_force)
{
  if(a_force)
  {
    a_channel->m_latched = false;
    a_channel->m_latchmode = ENCODER_LATCH_MODE_DEFAULT;

    a_channel->m_previous_count = a_channel->m_initial_count;
    a_channel->m_latch_position = 0;
    a_channel->m_position = a_channel->m_initial_count;
  }
}

// encoder_latch
static void encoder_latch(encoder_channel * a_channel, cyg_uint16 a_count)
{
  if(a_channel->m_latchmode != ENCODER_LATCH_MODE_NO_LATCH)
  { // Only do this if a valid latch mode is selected
    cyg_int16 delta = (cyg_int16)(a_count - a_channel->m_previous_count);
  
    a_channel->m_previous_count = a_count;
    a_channel->m_position += delta;

    a_channel->m_latch_position = a_channel->m_position;
    a_channel->m_latched = true;
  }
}

// encoder_read
static Cyg_ErrNo encoder_read(cyg_io_handle_t a_handle, void * a_buf, cyg_uint32 * a_len)
{
  if(*a_len == sizeof(cyg_int32))
  {
    cyg_devtab_entry_t * t = (cyg_devtab_entry_t *)a_handle;
    encoder_channel * chan = (encoder_channel *)t->priv;
    encoder_funs * funs = chan->m_funs;

    // Note : the following only works if we can guarantee that the difference between two readouts
    // will not exceed 0x7fff !! The encoder must be read on a regular timebase
    cyg_uint16 actual_count = funs->read_position(chan);
    cyg_int16 delta = (cyg_int16)(actual_count - chan->m_previous_count);
  
    chan->m_previous_count = actual_count;
    chan->m_position += delta;
  
    *(cyg_int32 *)a_buf = chan->m_position;
 
    return ENOERR;
  }
  else
    return -EINVAL;
}

// encoder_set_config
static Cyg_ErrNo encoder_set_config(cyg_io_handle_t a_handle, cyg_uint32 a_key, const void * a_buf, cyg_uint32 * a_len)
{
  cyg_devtab_entry_t * t = (cyg_devtab_entry_t *)a_handle;
  encoder_channel * chan = (encoder_channel *)t->priv;
  encoder_funs * funs = chan->m_funs;

  Cyg_ErrNo result = ENOERR;

  switch(a_key)
  {
    case ENCODER_IO_SET_CONFIG_LATCHMODE:
      {
        if(*a_len == sizeof(ENCODER_LATCH_MODE)) // Validate the parameter
        { // Changing the lachmode means re-starting the latch afterwards
          funs->stop_latch(chan);
          chan->m_latched = false;
          chan->m_latchmode = *(ENCODER_LATCH_MODE *)a_buf;
        }
        else
          result = -EINVAL;
      }
      break;

    case ENCODER_IO_SET_CONFIG_LATCHENABLE:
      {
        funs->stop_latch(chan);
        chan->m_latched = false;

        if(chan->m_latchmode == ENCODER_LATCH_MODE_INDEX)
          funs->latch_index(chan);
        if(chan->m_latchmode == ENCODER_LATCH_MODE_PROBE)
          funs->latch_probe(chan);
      }
      break;

    case ENCODER_IO_SET_CONFIG_LATCHDISABLE:
      { // Do not touch the latchmode
        funs->stop_latch(chan);
        chan->m_latched = false;
      }
      break;

    case ENCODER_IO_SET_CONFIG_OFFSET:
      {
        if(*a_len == sizeof(cyg_int32))
        { // Allways change the offset with respect to the current situation
          chan->m_position += *(cyg_int32 *)a_buf;  
        }
        else
          result = -EINVAL;
      }
      break;

    default:
      result = -EINVAL;
      break;
  }

  return result;
}

// encoder_get_config
static Cyg_ErrNo encoder_get_config(cyg_io_handle_t a_handle, cyg_uint32 a_key, void * a_buf, cyg_uint32 * a_len)
{
  cyg_devtab_entry_t * t = (cyg_devtab_entry_t *)a_handle;
  encoder_channel * chan = (encoder_channel *)t->priv;

  Cyg_ErrNo result = ENOERR;

  switch(a_key)
  {
    case ENCODER_IO_GET_CONFIG_LATCHED:
      {
        if(*a_len == sizeof(bool))
          *(bool *)a_buf = chan->m_latched;
        else
          result = -EINVAL;
      }
      break;

    case ENCODER_IO_GET_CONFIG_LATCHPOSITION:
      {
        if(*a_len == sizeof(cyg_int32))
          *(cyg_int32 *)a_buf = chan->m_latch_position;
        else
          result = -EINVAL;
      }
      break;

    default:
      result = -EINVAL;
      break;
  }

  return result;
}

#endif // ENCODER_C
