/****************************************************************************
*
*   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>
#include <avr/signal.h>
#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 "I2CDefs.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)

int ProcessCommand( I2c_Data_t *packet );

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

//***************************************************************************
/**
*   Main loop for the I2C I/O program.
*/
uint8_t servoNum = 4;
uint16_t pulseWidthUSec;

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.

    InitServoTimer( 3 );
    pulseWidthUSec = 1500;

    SetServo( servoNum, pulseWidthUSec );

    while( 1 )
    {
        uint16_t    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;

        if(pulse2_usec < 1100u) pulse2_usec = 1100u;
        else if(pulse2_usec > 1900u) pulse2_usec = 1900u;

        SetServo( servoNum, pulse2_usec );

        // 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 );

            }
        }
    }

  count =0;

    while ( 1 )
    {
        tick_t prevTick;

        switch ( count )
        {
            case   0:   LED_ON();       break;
            case  10:   LED_OFF();      break;
            case  20:   LED_ON();       break;
            case  30:   LED_OFF();      break;
            case 100:   count = -1;     break;
        }
        count++;

        prevTick = gTickCount;
        while ( gTickCount == prevTick )
        {
        }
    }

    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;
            servoNum = (int)(req->pwn_no);
            pulseWidthUSec = req->pulsevalue;
            return 2;

} // ProcessCommand