// ----------------------------------------------------------------------------------------
//
//  avr/sensors/sca3000/sca3000.cpp
//    
//  Bob Cook Development, Robotics Library
//  http://www.bobcookdev.com/rl/
//    
//  This file implements support for the VTI SCA3000-D01 accelerometer via SPI.
//
//  Copyright (C) 2008 Bob Cook
//
//      This program is free software; you can redistribute it and/or modify it
//      under the terms of the GNU General Public License as published by the
//      Free Software Foundation; either version 2 of the License, or (at your
//      option) any later version.
//    
//      This program 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 General Public License 
//      for more details.
//    
//      You should have received a copy of the GNU General Public License along
//      with this program; if not, write to the Free Software Foundation, Inc., 
//      51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
//
// ----------------------------------------------------------------------------------------


#include "sca3000.h"

#include <avr/io.h>

#include "packages/avr/device/spi.h"
#include "packages/avr/device/spinwait.h"


// ----------------------------------------------------------------------------------------

#include "project_defs.h"

#if defined( PRJ_SCA3000_ENABLE_DEBUG )

#include <stdio.h>
#include <avr/pgmspace.h>
#include "packages/common/util/printutils.h"

#endif


#if defined( PRJ_SCA3000_ENABLE )


// ----------------------------------------------------------------------------------------

#if !defined( PRJ_SCA3000_SELECT_DDR )
#error PRJ_SCA3000_SELECT_DDR is required!
#endif

#if !defined( PRJ_SCA3000_SELECT_PORT )
#error PRJ_SCA3000_SELECT_PORT is required!
#endif

#if !defined( PRJ_SCA3000_SELECT_PIN )
#error PRJ_SCA3000_SELECT_PIN is required!
#endif

#if defined( PRJ_SCA3000_USE_HW_RESET )

#if !defined( PRJ_SCA3000_RESET_DDR )
#error PRJ_SCA3000_RESET_DDR is required!
#endif

#if !defined( PRJ_SCA3000_RESET_PORT )
#error PRJ_SCA3000_RESET_PORT is required!
#endif

#if !defined( PRJ_SCA3000_RESET_PIN )
#error PRJ_SCA3000_RESET_PIN is required!
#endif

#endif  // PRJ_SCA3000_USE_HW_RESET


// ----------------------------------------------------------------------------------------
//  SCA3000 register definitions

const uint8_t SCA3000_REGISTER_REVID  = 0x00;
const uint8_t SCA3000_REGISTER_STATUS = 0x02;

const uint8_t SCA3000_REGISTER_X_LSB = 0x04;
const uint8_t SCA3000_REGISTER_X_MSB = 0x05;
const uint8_t SCA3000_REGISTER_Y_LSB = 0x06;
const uint8_t SCA3000_REGISTER_Y_MSB = 0x07;
const uint8_t SCA3000_REGISTER_Z_LSB = 0x08;
const uint8_t SCA3000_REGISTER_Z_MSB = 0x09;

const uint8_t SCA3000_REGISTER_TEMP_LSB = 0x12;
const uint8_t SCA3000_REGISTER_TEMP_MSB = 0x13;

const uint8_t SCA3000_REGISTER_MODE = 0x14;

const uint8_t SCA3000_REGISTER_INT_STATUS = 0x16;
const uint8_t SCA3000_REGISTER_INT_MASK   = 0x21;


// ----------------------------------------------------------------------------------------
    
inline void device_select()
{
    PRJ_SCA3000_SELECT_PORT &= ~( 1 << PRJ_SCA3000_SELECT_PIN );
}

inline void device_unselect()
{
    PRJ_SCA3000_SELECT_PORT |= ( 1 << PRJ_SCA3000_SELECT_PIN );
}


// ----------------------------------------------------------------------------------------

static uint8_t read_8bit_register( uint8_t address )
{
    uint8_t result;

    device_select();

    spi_write( address << 2 );  // bit 1 = 0 for read, bit 0 is always 0
    result = spi_read();

    device_unselect();

    return result;
}


// ----------------------------------------------------------------------------------------

static uint16_t read_16bit_register( uint8_t address )
{
    uint16_t result;

    device_select();

    spi_write( address << 2 );  // bit 1 = 0 for read, bit 0 is always 0
    result = spi_read() << 8;
    result |= spi_read();

    device_unselect();

    return result;
}


// ----------------------------------------------------------------------------------------
//
//  The high nibble is 011x when out of range on the positive side.
//  The high nibble is 100x when out of range on the negative side.
//

static int16_t read_axis_value( uint8_t msb_address )
{
    uint16_t raw = read_16bit_register( msb_address );

    if ( ( raw & 0xE000 ) == 0x6000 )
    {
        return 23032;  // max positive value 2303.25 mg (rounded down)
    }

    if ( ( raw & 0xE000 ) == 0x8000 )
    {
        return -23040;  // max negative value -2304.00 mg
    }

    bool is_negative = false;

    if ( ( raw & 0x8000 ) == 0x8000 )
    {
        raw = ~raw;
        is_negative = true;
    }

    raw >>= 3; // least significant 3 bits are ignored

    bool add_pt_75 = ( raw & 0x0001 ) != 0;
    raw >>= 1;

    bool add_1_pt_5 = ( raw & 0x0001 ) != 0;
    raw >>= 1;

    int16_t result = 0;
    int16_t bit_scaling_factor = 3;

    for ( uint8_t i = 0; i < 10; i++ )
    {
        if ( ( raw & 0x0001 ) != 0 )
        {
            result += bit_scaling_factor;
        }
        raw >>= 1;
        bit_scaling_factor *= 2;
    }

    result *= 10;

    if ( add_1_pt_5 )
    {
        result += 15;
    }

    if ( add_pt_75 )
    {
        result += 8; // 7.5 rounded up
    }

    if ( is_negative )
    {
        result *= -1;
    }

    return result;
}


// ----------------------------------------------------------------------------------------
    
bool sca3000_init()
{
    //--    Configure the SPI hardware; the caller might have done it but it doesn't
    //      hurt to do it again.

    spi_init();

    //--    Configure our select line.

    PRJ_SCA3000_SELECT_DDR |= ( 1 << PRJ_SCA3000_SELECT_PIN );
    device_unselect();

    //--    Configure our reset line (if supplied); it should be pulled high normally.

#if defined( PRJ_SCA3000_USE_HW_RESET )

    PRJ_SCA3000_RESET_DDR  |= ( 1 << PRJ_SCA3000_RESET_PIN );
    PRJ_SCA3000_RESET_PORT |= ( 1 << PRJ_SCA3000_RESET_PIN );

#endif

    return true;
}


// ----------------------------------------------------------------------------------------
    
#if defined( PRJ_SCA3000_USE_HW_RESET )

void sca3000_reset()
{
    //--    Momentarily drop the reset line, trigger a hardware reset.

    PRJ_SCA3000_RESET_PORT &= ~( 1 << PRJ_SCA3000_RESET_PIN );
    spinwait_delay_ms( 10 );
    PRJ_SCA3000_RESET_PORT |= ( 1 << PRJ_SCA3000_RESET_PIN );
    spinwait_delay_ms( 10 );
}

#endif  // #if defined( PRJ_SCA3000_USE_HW_RESET )


// ----------------------------------------------------------------------------------------

int16_t sca3000_read_x()
{
    return read_axis_value( SCA3000_REGISTER_X_MSB );
}


// ----------------------------------------------------------------------------------------

int16_t sca3000_read_y()
{
    return read_axis_value( SCA3000_REGISTER_Y_MSB );
}


// ----------------------------------------------------------------------------------------

int16_t sca3000_read_z()
{
    return read_axis_value( SCA3000_REGISTER_Z_MSB );
}


// ----------------------------------------------------------------------------------------
//
//  the formula to convert the raw value to celcius is contained in the datasheet.
//

int16_t sca3000_read_temp()
{
    uint16_t raw = read_16bit_register( SCA3000_REGISTER_TEMP_MSB );
    raw >>= 5;  // ignore the least significant five bits

    int16_t temp = static_cast< int16_t >( raw ) - 256;
    temp *= 100;
    temp /= 180;
    temp += 2300;

    return temp;
}


// ----------------------------------------------------------------------------------------

#if defined( PRJ_SCA3000_ENABLE_DEBUG )
    
static void print_8bit_register( const prog_char* label, uint8_t address )
{
    printf_P( label );
    print_bits( read_8bit_register( address ) );
    putchar( '\n' );
}

void sca3000_debug_print_registers()
{
    printf_P( PSTR("SCA3000 registers:\n") );

    print_8bit_register( PSTR("         REVID : "), SCA3000_REGISTER_REVID );
    print_8bit_register( PSTR("        STATUS : "), SCA3000_REGISTER_STATUS );
    print_8bit_register( PSTR("          MODE : "), SCA3000_REGISTER_MODE );
    print_8bit_register( PSTR("    INT_STATUS : "), SCA3000_REGISTER_INT_STATUS );
    print_8bit_register( PSTR("      INT_MASK : "), SCA3000_REGISTER_INT_MASK );

    printf_P( PSTR("             X : ") );
    print_hex( read_16bit_register( SCA3000_REGISTER_X_MSB ) );
    putchar( '\n' );

    printf_P( PSTR("             Y : ") );
    print_hex( read_16bit_register( SCA3000_REGISTER_Y_MSB ) );
    putchar( '\n' );

    printf_P( PSTR("             Z : ") );
    print_hex( read_16bit_register( SCA3000_REGISTER_Z_MSB ) );
    putchar( '\n' );

    printf_P( PSTR("          TEMP : ") );
    print_hex( read_16bit_register( SCA3000_REGISTER_TEMP_MSB ) );
    putchar( '\n' );
}

#endif  // PRJ_SCA3000_ENABLE_DEBUG


// ----------------------------------------------------------------------------------------

#endif  // PRJ_SCA3000_ENABLE


// ----------------------------------------------------------------------------------------
//  end of sca3000.cpp
// ----------------------------------------------------------------------------------------

