/****************************************************************************
*
*   Copyright (c) 2006 Dave Hylands     <dhylands@gmail.com>
*
*   This program is free software; you can redistribute it and/or modify
*   it under the terms of the GNU General Public License version 2 as
*   published by the Free Software Foundation.
*
*   Alternatively, this software may be distributed under the terms of BSD
*   license.
*
*   See README and COPYING for more details.
*
****************************************************************************/
/**
*
*   @file   i2c-io.c 
*
*   @brief  This file implements a set of I2C commands which allows the
*           robostix I/O to be controlled by the gumstix.
*
*****************************************************************************/

/* ---- Include Files ----------------------------------------------------- */

#include <avr/io.h>
#if defined( __AVR_LIBC_VERSION__ )
#   include <avr/interrupt.h>
#else
#   include <avr/signal.h>
#endif
#include <compat/twi.h>
#include <stdio.h>
#include <inttypes.h>

#include "i2c-io.h"
#include "Hardware.h"
#include "i2c-slave-boot.h"
#include "Log.h"
#include "Delay.h"
#include "Timer.h"
#include "UART.h"

#include "Servo.h"
#include "a2d.h"


#include "svn-version.h"

/* ---- Public Variables -------------------------------------------------- */

/* ---- Private Constants and Types --------------------------------------- */

/* ---- Private Variables ------------------------------------------------- */

/* ---- Private Function Prototypes --------------------------------------- */

#undef  LED_ON
#undef  LED_OFF

#define LED_OFF()   do { CFG_BOOTLOADER_BEAT_PORT &= ~CFG_BOOTLOADER_BEAT_MASK; } while (0)
#define LED_ON()    do { CFG_BOOTLOADER_BEAT_PORT |=  CFG_BOOTLOADER_BEAT_MASK; } while (0)

typedef struct
{
  uint8_t pwm_no; // 0=1a, 1=1b, 2=1c, 3=3a, 4=3b, 5=3c
  uint8_t pulseDelay;
  uint16_t pulse_value;  // given in usec
} I2c_pwm_packet_t;

int ProcessCommand( I2C_Data_t *packet );

/* ---- Functions --------------------------------------------------------- */

//***************************************************************************
/**
*   Main loop for the I2C I/O program.
*/

uint8_t servoStart = 3;
uint8_t servoEnd = 5;

uint8_t servoNum;
uint16_t pulseWidthUSec[6];
uint8_t pulseDelay[6];
uint8_t pulseCt[6];

int main(void)
{
//    int     count=0;


    InitHardware();

    CFG_BOOTLOADER_BEAT_DDR |= CFG_BOOTLOADER_BEAT_MASK;

    // The first handle opened for read goes to stdin, and the first handle
    // opened for write goes to stdout. So u0 is stdin, stdout, and stderr

    if ( !I2C_SlaveBootInit( ProcessCommand ))
    {
//        LogError( "I2C_SlaveBootInit failed\n" );

        LED_ON();
        while ( 1 )
        {
            ;
        }
    }

    sei();

    // The main loop just does an interesting heartbeat with a two pulses
    // close together, followed by a longer pause.

    int     i;

    InitHardware();

    // The first handle opened for read goes to stdin, and the first handle
    // opened for write goes to stdout.

//#if defined( __AVR_LIBC_VERSION__ )
//    fdevopen( UART0_PutCharStdio, UART0_GetCharStdio );
//#else
//    fdevopen( UART0_PutCharStdio, UART0_GetCharStdio, 0 );
//#endif

    InitServoTimer( 3 );

    for(servoNum = servoStart; servoNum <= servoEnd; servoNum++)
    {
      pulseWidthUSec[servoNum] = 1500; // Default inital value

      SetServo( servoNum, pulseWidthUSec[servoNum] );
      pulseDelay[servoNum] = 0;
      pulseCt[servoNum] = 1;
    }

    while( 1 )
    {
//        int    pulse2_usec;

        // "nominal" servo pulses are between 1 ms and 2 ms (1000 usec and 2000 usec)
        //  Some servos have a wider range, so I used 500usec thru 2500 usec range
        // and the user can use this to find the limits.
        //
        // We then need to multiply the pulse in usec by 2, since out timer 
        // runs with 1/2 usec ticks.

//        pulse2_usec = pulseWidthUSec;

        for(servoNum = servoStart; servoNum <= servoEnd; servoNum++)
        {
          if(pulseDelay[servoNum])
          {
            if(--pulseCt[servoNum])
            {
              SetServo(servoNum, 1500);
            }
            else
            {
              SetServo( servoNum, pulseWidthUSec[servoNum] );
              pulseCt[servoNum] = pulseDelay[servoNum];
            }
          }
        }
        // We put a delay in here so we aren't trying to update the OCR 
        // registers too frequently. Waiting to 2 ticks coincides with the
        // pulse rate.

        for ( i = 0; i < 2; i++ ) 
        {
            // Toggle our heartbeat LED and update the display 4 times a second.
            // gTickCount increments every 10 msec

            WaitForTimer0Rollover();

            if (( gTickCount % 25 ) == 0 )
            {
                // The main loop runs once every 30 msec, and we toggle every 8th
                // time through, so we toggle about 4 times/sec.

                LED_TOGGLE( RED );

            }
        }
    }

    return 0;

} // main

//***************************************************************************
/**
*   I2C Interrupt service routine
*/

SIGNAL(SIG_2WIRE_SERIAL)
{
    if ( !I2C_SlaveBootHandler() )
    {
        LogError( "Unrecognized status: 0x%x\n", TW_STATUS );
    }

    // Now that we've finished dealing with the interrupt, set the TWINT flag
    // which will stop the clock stretching and clear the interrupt source

    TWCR |= ( 1 << TWINT );

} // SIG_2WIRE_SERIAL

//***************************************************************************
/**
*   Callback called to process incoming i2c commands.
*/

int ProcessCommand( I2C_Data_t *packet )
{
  I2c_pwm_packet_t *req = (I2c_pwm_packet_t *)&packet->m_data[2];
  servoNum = req->pwm_no;
  if(pulseWidthUSec[servoNum] != req->pulse_value || pulseDelay[servoNum] != req->pulseDelay)
  {
    pulseWidthUSec[servoNum] = req->pulse_value;
    pulseDelay[servoNum] = req->pulseDelay;
    pulseCt[servoNum] = 1;
  }

  return 2;

} // ProcessCommand