/*
 *  AP_HAL_AVR I2C driver. derived from:
 *  I2C.cpp - I2C library
 *  Copyright (c) 2011 Wayne Truchsess.  All right reserved.
 *  Rev 2.0 - September 19th, 2011
 *         - Added support for timeout function to prevent
 *           and recover from bus lockup (thanks to PaulS
 *           and CrossRoads on the Arduino forum)
 *         - Changed return type for stop() from void to
 *           uint8_t to handle timeOut function
 *  Rev 1.0 - August 8th, 2011
 *
 *  This is a modified version of the Arduino Wire/TWI
 *  library.  Functions were rewritten to provide more functionality
 *  and also the use of Repeated Start.  Some I2C devices will not
 *  function correctly without the use of a Repeated Start.  The
 *  initial version of this library only supports the Master.
 *
 *  This library is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU Lesser General Public
 *  License as published by the Free Software Foundation; either
 *  version 2.1 of the License, or (at your option) any later version.
 *
 *  This library is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *  Lesser General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public
 *  License along with this library; if not, write to the Free Software
 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 */

#include <AP_HAL.h>
#if (CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2)

#include <inttypes.h>

#include <avr/io.h>
#include <avr/interrupt.h>

#include <AP_HAL.h>
#include "I2CDriver.h"
using namespace AP_HAL_AVR;

extern const AP_HAL::HAL& hal;

#ifndef F_CPU
#define CPU_FREQ 16000000L
#else
#define CPU_FREQ F_CPU
#endif

#define START           0x08
#define REPEATED_START  0x10
#define MT_SLA_ACK      0x18
#define MT_DATA_ACK     0x28
#define MR_SLA_ACK      0x40
#define MR_DATA_ACK     0x50
#define MR_DATA_NACK    0x58
#define TWI_STATUS      (TWSR & 0xF8)

#define SLA_W(address)  (address << 1)
#define SLA_R(address)  ((address << 1) + 0x01)

#define cbi(sfr, bit)   (_SFR_BYTE(sfr) &= ~_BV(bit))
#define sbi(sfr, bit)   (_SFR_BYTE(sfr) |= _BV(bit))

void AVRI2CDriver::begin() {
    // activate internal pull-ups for twi
    // as per note from atmega128 manual pg204
    sbi(PORTD, 0);
    sbi(PORTD, 1);

    // initialize twi prescaler and bit rate
    cbi(TWSR, TWPS0);
    cbi(TWSR, TWPS1);
    TWBR = ((CPU_FREQ / 100000) - 16) / 2;
    // enable twi module, acks, and twi interrupt
    TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA);
}

void AVRI2CDriver::end() {
    TWCR = 0;
}
 
void AVRI2CDriver::setHighSpeed(bool active) {
    if (active) {
        TWBR = ((CPU_FREQ / 400000) - 16) / 2;
    } else {
        TWBR = ((CPU_FREQ / 100000) - 16) / 2;
    }
}

uint8_t AVRI2CDriver::write(uint8_t addr, uint8_t len, uint8_t* data){
    uint8_t stat = _start();
    if (stat) goto error;
    stat = _sendAddress(SLA_W(addr));
    if (stat) goto error;
    for (uint8_t i = 0; i < len; i++)
    {
        stat = _sendByte(data[i]);
        if (stat) goto error;
    }
    stat = _stop();
    if (stat) goto error;
    return stat;
error:
    _lockup_count++;
    return stat;
}

uint8_t AVRI2CDriver::writeRegisters(uint8_t addr, uint8_t reg,
                                    uint8_t len, uint8_t* data){
    uint8_t stat = _start();
    if (stat) goto error;
    stat = _sendAddress(SLA_W(addr));
    if (stat) goto error;
    stat = _sendByte(reg);
    if (stat) goto error;
    for (uint8_t i = 0; i < len; i++)
    {
        stat = _sendByte(data[i]);
        if (stat) goto error;
    }
    stat = _stop();
    if (stat) goto error;
    return stat;
error:
    _lockup_count++;
    return stat;
}

uint8_t AVRI2CDriver::writeRegister(uint8_t addr, uint8_t reg, uint8_t val) {
        /* Sometimes avr-gcc fails at dereferencing a uint8_t arg. */
        uint8_t data[1];
        data[0] = val;
        return writeRegisters(addr, reg, 1, data);
}

uint8_t AVRI2CDriver::read(uint8_t addr, uint8_t len, uint8_t* data){
    uint8_t stat;
    if ( len == 0)
        len = 1;
    uint8_t nackposition = len - 1;
    stat = 0;
    stat = _start();
    if(stat) goto error;
    stat = _sendAddress(SLA_W(addr));
    if(stat) goto error;
    stat = _start();
    if(stat) goto error;
    stat = _sendAddress(SLA_R(addr));
    if(stat) goto error;
    for(uint8_t i = 0; i < len ; i++) {
        if ( i == nackposition ) {
            stat = _receiveByte(false);
            if (stat != MR_DATA_NACK) goto error;
        } else {
            stat = _receiveByte(true);
            if (stat != MR_DATA_ACK) goto error;
        }
        data[i] = TWDR;
    }
    stat = _stop();
    if (stat) goto error;
    return stat;
error:
    _lockup_count++;
    return stat;
}

uint8_t AVRI2CDriver::readRegisters(uint8_t addr, uint8_t reg,
                                    uint8_t len, uint8_t* data){
    uint8_t stat;
    if ( len == 0)
        len = 1;
    uint8_t nackposition = len - 1;
    stat = 0;
    stat = _start();
    if(stat) goto error;
    stat = _sendAddress(SLA_W(addr));
    if(stat) goto error;
    stat = _sendByte(reg);
    if(stat) goto error;
    stat = _start();
    if(stat) goto error;
    stat = _sendAddress(SLA_R(addr));
    if(stat) goto error;
    for(uint8_t i = 0; i < len ; i++) {
        if ( i == nackposition ) {
            stat = _receiveByte(false);
            if (stat != MR_DATA_NACK) goto error;
        } else {
            stat = _receiveByte(true);
            if (stat != MR_DATA_ACK) goto error;
        }
        data[i] = TWDR;
    }
    stat = _stop();
    if (stat) goto error;
    return stat;
error:
    _lockup_count++;
    return stat;
}

uint8_t AVRI2CDriver::readRegister(uint8_t addr, uint8_t reg, uint8_t* data) {
    return readRegisters(addr, reg, 1, data);
}

uint8_t AVRI2CDriver::_start() {
    TWCR = _BV(TWINT) | _BV(TWSTA) | _BV(TWEN);
    uint8_t stat = _waitInterrupt();
    if (stat) return stat;

    if ((TWI_STATUS == START) || (TWI_STATUS == REPEATED_START)) {
        return 0;
    } else {
        return TWI_STATUS;
    }
}

uint8_t AVRI2CDriver::_stop() {
    TWCR = _BV(TWINT) | _BV(TWEN) | _BV(TWSTO);
    return _waitStop();
}

uint8_t AVRI2CDriver::_sendAddress(uint8_t addr) {
    TWDR = addr;
    TWCR = _BV(TWINT) | _BV(TWEN);
    return _waitInterrupt();
}

uint8_t AVRI2CDriver::_sendByte(uint8_t data) {
    TWDR = data;
    TWCR = _BV(TWINT) | _BV(TWEN);
    uint8_t stat = _waitInterrupt();
    if (stat) return stat;

    if (TWI_STATUS  == MT_DATA_ACK) {
        return 0;
    } else {
        return TWI_STATUS;
    }
}

uint8_t AVRI2CDriver::_receiveByte(bool ack) {
    if (ack) {
        TWCR = _BV(TWINT) | _BV(TWEN) | _BV(TWEA);
    } else {
        TWCR = _BV(TWINT) | _BV(TWEN);
    }
    uint8_t stat = _waitInterrupt();
    if (stat) return stat;
    return TWI_STATUS;
}

void AVRI2CDriver::_handleLockup() {
    TWCR = 0; /* Releases SDA and SCL lines to high impedance */
    TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA); /* Reinitialize TWI */
    _lockup_count++;
}

uint8_t AVRI2CDriver::_waitInterrupt() {
    uint32_t start = hal.scheduler->millis();
    if (_timeoutDelay == 0) {
        /* Wait indefinitely for interrupt to go off */
        while (!(TWCR & _BV(TWINT))) { }
    } else {
        /* Wait while polling for timeout */
        while (!(TWCR & _BV(TWINT))) {
            uint32_t current = hal.scheduler->millis();
            if ( current - start >= _timeoutDelay ) {
                _handleLockup();
                return 1;
            }
        }
    }
    return 0;
}

uint8_t AVRI2CDriver::_waitStop() {
    uint32_t start = hal.scheduler->millis();
    if (_timeoutDelay == 0) {
        /* Wait indefinitely for stop condition */
        while( TWCR & _BV(TWSTO) ) { }
    } else  {
        /* Wait while polling for timeout */
        while( TWCR & _BV(TWSTO) ) {
            uint32_t current = hal.scheduler->millis();
            if (current - start >= _timeoutDelay) {
                _handleLockup(); 
                return 1;
            }
        }
    }
    return 0;
}

SIGNAL(TWI_vect)
{
    switch(TWI_STATUS) {
    case 0x20:
    case 0x30:
    case 0x48:
        TWCR = _BV(TWINT) | _BV(TWEN) | _BV(TWSTO);  // send a stop
        break;
    case 0x38:
    case 0x68:
    case 0x78:
    case 0xB0:
        TWCR = 0;  //releases SDA and SCL lines to high impedance
        TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA);  //reinitialize TWI
        break;
    }
}


#endif