/*
	AP_OpticalFlow_ADNS3080.cpp - ADNS3080 OpticalFlow Library for Ardupilot Mega
	Code by Randy Mackay. DIYDrones.com

	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.

	External ADNS3080 OpticalFlow is connected via Serial port 2 (in SPI mode)
	TXD2 = MOSI = pin PH1
	RXD2 = MISO = pin PH0
	XCK2 = SCK = pin PH2
	Chip Select pin is PC4 (33)   [PH6 (9)]
	We are using the 16 clocks per conversion timming to increase efficiency (fast)
	The sampling frequency is 400Hz (Timer2 overflow interrupt)
	So if our loop is at 50Hz, our needed sampling freq should be 100Hz, so
	we have an 4x oversampling and averaging.

	Methods:
		Init() : Initialization of interrupts an Timers (Timer2 overflow interrupt)
		Read() : Read latest values from OpticalFlow and store to x,y, surface_quality parameters

*/

#include "AP_OpticalFlow_ADNS3080.h"
#include "SPI.h"
#if defined(ARDUINO) && ARDUINO >= 100
	#include "Arduino.h"
#else
	#include "WProgram.h"
#endif

#define AP_SPI_TIMEOUT 1000

// We use Serial Port 2 in SPI Mode
#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
	#define AP_SPI_DATAIN      15    // MISO
	#define AP_SPI_DATAOUT     14    // MOSI
	#define AP_SPI_CLOCK       PJ2   // SCK
#else  // normal arduino SPI pins...these need to be checked
	# error Please check the Tools/Board menu to ensure you have selected Arduino Mega as your target.
#endif

// mask for saving bit order and data mode to avoid interference with other users of the bus
#define UCSR3C_MASK 0x07

// SPI3 setting for UCSR3C
#define SPI3_MODE_SPI 0xC0	// UMSEL31 = 1, UMSEL30 = 1

// settings for phase and polarity bits of UCSR3C
#define SPI3_MODE_MASK 0x03
#define SPI3_MODE0 0x00
#define SPI3_MODE1 0x01
#define SPI3_MODE2 0x02
#define SPI3_MODE3 0x03
#define SPI3_MODE SPI3_MODE3

// settings for phase and polarity bits of UCSR3C
#define SPI3_ORDER_MASK 0x04
#define SPI3_MSBFIRST 0x00
#define SPI3_LSBFIRST 0x04

#define SPI3_SPEED 0x04 // 2 megahertz?

#define SPI3_DELAY 20	// delay in microseconds after sending data


union NumericIntType
{
   int         intValue;
   unsigned int uintValue;
   byte        byteValue[2];
};

	// Constructors ////////////////////////////////////////////////////////////////
AP_OpticalFlow_ADNS3080_APM2::AP_OpticalFlow_ADNS3080_APM2(int cs_pin, int reset_pin) : _cs_pin(cs_pin), _reset_pin(reset_pin)
{
	num_pixels = ADNS3080_PIXELS_X;
	field_of_view = AP_OPTICALFLOW_ADNS3080_08_FOV;
	scaler = AP_OPTICALFLOW_ADNS3080_SCALER;
}

// SPI Methods
// *** INTERNAL FUNCTIONS ***
unsigned char AP_OpticalFlow_ADNS3080_APM2::SPI_transfer(uint8_t data)
{

	/* Wait for empty transmit buffer */
	while ( !( UCSR3A & (1<<UDRE3)) );

	/* Put data into buffer, sends the data */
	UDR3 = data;

	/* Wait for data to be received */
	while ( !(UCSR3A & (1<<RXC3)) );
	
	/* Get and return received data from buffer */
	return UDR3;
}

// Public Methods //////////////////////////////////////////////////////////////
// init - initialise sensor
// initCommAPI parameter controls whether SPI interface is initialised (set to false if other devices are on the SPI bus and have already initialised the interface)
bool
AP_OpticalFlow_ADNS3080_APM2::init(bool initCommAPI)
{
    int retry = 0;

	pinMode(AP_SPI_DATAOUT,OUTPUT);
	pinMode(AP_SPI_DATAIN,INPUT);
	pinMode(AP_SPI_CLOCK,OUTPUT);
	pinMode(_cs_pin,OUTPUT);
	if( _reset_pin != 0)
	    pinMode(ADNS3080_RESET,OUTPUT);

	digitalWrite(_cs_pin,HIGH);                 // disable device (Chip select is active low)

	// reset the device
	reset();

	// start the SPI library:
	if( initCommAPI ) {
		// Setup Serial Port3 in SPI mode (MSPI), Mode 0, Clock: 8Mhz
		UBRR3 = 0;
		DDRJ |= (1<<PJ2);                                   // SPI clock XCK3 (PJ2) as output. This enable SPI Master mode
		// put UART3 into SPI master mode
		UCSR3C = SPI3_MODE_SPI | SPI3_MODE;
		// Enable receiver and transmitter.
		UCSR3B = (1<<RXEN3)|(1<<TXEN3);
		// Set Baud rate
		UBRR3 = SPI3_SPEED;         // SPI running at 8Mhz
	}
	
	delay(10);

	// check the sensor is functioning
	while( retry < 3 ) {
	    if( read_register(ADNS3080_PRODUCT_ID) == 0x17 ) {
	        return true;
		}
	    retry++;
	}

	return false;
}

//
// backup_spi_settings - checks current SPI settings (clock speed, etc), sets values to what we need
//
void AP_OpticalFlow_ADNS3080_APM2::backup_spi_settings()
{

	uint8_t temp;

	/* Wait for empty transmit buffer */
	while ( !( UCSR3A & (1<<UDRE3)) );

	// store current spi values
	orig_spi_settings_ucsr3c = UCSR3C;
	orig_spi_settings_ubrr3 = UBRR3;

	// decide new value for UCSR3C
	temp = (orig_spi_settings_ucsr3c & ~UCSR3C_MASK) | SPI3_MODE | SPI3_MSBFIRST;
	UCSR3C = temp;
	UBRR3 = SPI3_SPEED;         // SPI running at 1Mhz
}

// restore_spi_settings - restores SPI settings (clock speed, etc) to what their values were before the sensor used the bus
void AP_OpticalFlow_ADNS3080_APM2::restore_spi_settings()
{
	/* Wait for empty transmit buffer */
	while ( !( UCSR3A & (1<<UDRE3)) );

	// restore UCSRC3C and UBRR3
	UCSR3C = orig_spi_settings_ucsr3c;
    UBRR3 = orig_spi_settings_ubrr3;
}

// Read a register from the sensor
byte
AP_OpticalFlow_ADNS3080_APM2::read_register(byte address)
{
    byte result = 0, junk = 0;

    backup_spi_settings();

	// take the chip select low to select the device
    digitalWrite(_cs_pin, LOW);
	
    // send the device the register you want to read:
    junk = SPI_transfer(address);
	
	// small delay
	delayMicroseconds(SPI3_DELAY);
	
	// send a value of 0 to read the first byte returned:
    result = SPI_transfer(0x00);

    // take the chip select high to de-select:
    digitalWrite(_cs_pin, HIGH);

	restore_spi_settings();

	return result;
}

// write a value to one of the sensor's registers
void
AP_OpticalFlow_ADNS3080_APM2::write_register(byte address, byte value)
{
    byte junk = 0;

    backup_spi_settings();

	// take the chip select low to select the device
    digitalWrite(_cs_pin, LOW);

	// send register address
    junk = SPI_transfer(address | 0x80 );

	// small delay
	delayMicroseconds(SPI3_DELAY);

	// send data
	junk = SPI_transfer(value);

    // take the chip select high to de-select:
    digitalWrite(_cs_pin, HIGH);

	restore_spi_settings();
}

// reset sensor by holding a pin high (or is it low?) for 10us.
void
AP_OpticalFlow_ADNS3080_APM2::reset()
{
    // return immediately if the reset pin is not defined
    if( _reset_pin == 0)
	    return;

    digitalWrite(_reset_pin,HIGH);                 // reset sensor
	delayMicroseconds(10);
	digitalWrite(_reset_pin,LOW);                  // return sensor to normal
}

// read latest values from sensor and fill in x,y and totals
bool
AP_OpticalFlow_ADNS3080_APM2::update()
{
    byte motion_reg;
    surface_quality = (unsigned int)read_register(ADNS3080_SQUAL);
	delayMicroseconds(SPI3_DELAY);  // small delay

    // check for movement, update x,y values
	motion_reg = read_register(ADNS3080_MOTION);
	_overflow = ((motion_reg & 0x10) != 0);  // check if we've had an overflow
	if( (motion_reg & 0x80) != 0 ) {
		raw_dx = ((char)read_register(ADNS3080_DELTA_X));
		delayMicroseconds(SPI3_DELAY);  // small delay
		raw_dy = ((char)read_register(ADNS3080_DELTA_Y));
		_motion = true;
	}else{
	    raw_dx = 0;
		raw_dy = 0;
	}

	last_update = millis();

	apply_orientation_matrix();

	return true;
}

void
AP_OpticalFlow_ADNS3080_APM2::disable_serial_pullup()
{
    byte regVal = read_register(ADNS3080_EXTENDED_CONFIG);
	regVal = (regVal | ADNS3080_SERIALNPU_OFF);
	delayMicroseconds(SPI3_DELAY);  // small delay
	write_register(ADNS3080_EXTENDED_CONFIG, regVal);
}

// get_led_always_on - returns true if LED is always on, false if only on when required
bool
AP_OpticalFlow_ADNS3080_APM2::get_led_always_on()
{
	return ( (read_register(ADNS3080_CONFIGURATION_BITS) & 0x40) > 0 );
}

// set_led_always_on - set parameter to true if you want LED always on, otherwise false for only when required
void
AP_OpticalFlow_ADNS3080_APM2::set_led_always_on( bool alwaysOn )
{
    byte regVal = read_register(ADNS3080_CONFIGURATION_BITS);
    regVal = (regVal & 0xbf) | (alwaysOn << 6);
	delayMicroseconds(SPI3_DELAY);  // small delay
	write_register(ADNS3080_CONFIGURATION_BITS, regVal);
}

// returns resolution (either 400 or 1600 counts per inch)
int
AP_OpticalFlow_ADNS3080_APM2::get_resolution()
{
    if( (read_register(ADNS3080_CONFIGURATION_BITS) & 0x10) == 0 )
	    return 400;
	else
	    return 1600;
}

// set parameter to 400 or 1600 counts per inch
void
AP_OpticalFlow_ADNS3080_APM2::set_resolution(int resolution)
{
    byte regVal = read_register(ADNS3080_CONFIGURATION_BITS);

    if( resolution == ADNS3080_RESOLUTION_400 )	{
	    regVal &= ~0x10;
		scaler = AP_OPTICALFLOW_ADNS3080_SCALER;
	}else if( resolution == ADNS3080_RESOLUTION_1600) {
	    regVal |= 0x10;
		scaler = AP_OPTICALFLOW_ADNS3080_SCALER * 4;
	}

	delayMicroseconds(SPI3_DELAY);  // small delay
	write_register(ADNS3080_CONFIGURATION_BITS, regVal);

	// this will affect conversion factors so update them
	update_conversion_factors();
}

// get_frame_rate_auto - return whether frame rate is set to "auto" or manual
bool
AP_OpticalFlow_ADNS3080_APM2::get_frame_rate_auto()
{
    byte regVal = read_register(ADNS3080_EXTENDED_CONFIG);
    if( (regVal & 0x01) != 0 ) {
	    return false;
	}else{
	    return true;
	}
}

// set_frame_rate_auto - set frame rate to auto (true) or manual (false)
void
AP_OpticalFlow_ADNS3080_APM2::set_frame_rate_auto(bool auto_frame_rate)
{
    byte regVal = read_register(ADNS3080_EXTENDED_CONFIG);
	delayMicroseconds(SPI3_DELAY);  // small delay
	if( auto_frame_rate == true ) {
		// set specific frame period
		write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER,0xE0);
		delayMicroseconds(SPI3_DELAY);  // small delay
		write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER,0x1A);
		delayMicroseconds(SPI3_DELAY);  // small delay

		// decide what value to update in extended config
        regVal = (regVal & ~0x01);
	}else{
	    // decide what value to update in extended config
	    regVal = (regVal & ~0x01) | 0x01;
	}
	write_register(ADNS3080_EXTENDED_CONFIG, regVal);
}

// get frame period
unsigned int
AP_OpticalFlow_ADNS3080_APM2::get_frame_period()
{
    NumericIntType aNum;
	aNum.byteValue[1] = read_register(ADNS3080_FRAME_PERIOD_UPPER);
	delayMicroseconds(SPI3_DELAY);  // small delay
    aNum.byteValue[0] = read_register(ADNS3080_FRAME_PERIOD_LOWER);
	return aNum.uintValue;
}

// set frame period
void
AP_OpticalFlow_ADNS3080_APM2::set_frame_period(unsigned int period)
{
    NumericIntType aNum;
	aNum.uintValue = period;

	// set frame rate to manual
	set_frame_rate_auto(false);
	delayMicroseconds(SPI3_DELAY);  // small delay

	// set specific frame period
	write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER,aNum.byteValue[0]);
	delayMicroseconds(SPI3_DELAY);  // small delay
	write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER,aNum.byteValue[1]);

}

unsigned int
AP_OpticalFlow_ADNS3080_APM2::get_frame_rate()
{
    unsigned long clockSpeed = ADNS3080_CLOCK_SPEED;
	unsigned int rate = clockSpeed / get_frame_period();
	return rate;
}

void
AP_OpticalFlow_ADNS3080_APM2::set_frame_rate(unsigned int rate)
{
    unsigned long clockSpeed = ADNS3080_CLOCK_SPEED;
    unsigned int period = (unsigned int)(clockSpeed / (unsigned long)rate);

	set_frame_period(period);
}

// get_shutter_speed_auto - returns true if shutter speed is adjusted automatically, false if manual
bool
AP_OpticalFlow_ADNS3080_APM2::get_shutter_speed_auto()
{
    byte regVal = read_register(ADNS3080_EXTENDED_CONFIG);
	if( (regVal & 0x02) > 0 ) {
	    return false;
	}else{
	    return true;
	}
}

// set_shutter_speed_auto - set shutter speed to auto (true), or manual (false)
void
AP_OpticalFlow_ADNS3080_APM2::set_shutter_speed_auto(bool auto_shutter_speed)
{
    byte regVal = read_register(ADNS3080_EXTENDED_CONFIG);
	delayMicroseconds(SPI3_DELAY);  // small delay
	if( auto_shutter_speed ) {
		// return shutter speed max to default
		write_register(ADNS3080_SHUTTER_MAX_BOUND_LOWER,0x8c);
		delayMicroseconds(SPI3_DELAY);  // small delay
		write_register(ADNS3080_SHUTTER_MAX_BOUND_UPPER,0x20);
		delayMicroseconds(SPI3_DELAY);  // small delay

		// determine value to put into extended config
        regVal &= ~0x02;
	}else{
	    // determine value to put into extended config
	    regVal |= 0x02;
	}
	write_register(ADNS3080_EXTENDED_CONFIG, regVal);
	delayMicroseconds(SPI3_DELAY);  // small delay
}

// get_shutter_speed_auto - returns true if shutter speed is adjusted automatically, false if manual
unsigned int
AP_OpticalFlow_ADNS3080_APM2::get_shutter_speed()
{
    NumericIntType aNum;
	aNum.byteValue[1] = read_register(ADNS3080_SHUTTER_UPPER);
	delayMicroseconds(SPI3_DELAY);  // small delay
    aNum.byteValue[0] = read_register(ADNS3080_SHUTTER_LOWER);
	return aNum.uintValue;
}


// set_shutter_speed_auto - set shutter speed to auto (true), or manual (false)
void
AP_OpticalFlow_ADNS3080_APM2::set_shutter_speed(unsigned int shutter_speed)
{
    NumericIntType aNum;
	aNum.uintValue = shutter_speed;

	// set shutter speed to manual
    set_shutter_speed_auto(false);
	delayMicroseconds(SPI3_DELAY);  // small delay

	// set specific shutter speed
	write_register(ADNS3080_SHUTTER_MAX_BOUND_LOWER,aNum.byteValue[0]);
	delayMicroseconds(SPI3_DELAY);  // small delay
	write_register(ADNS3080_SHUTTER_MAX_BOUND_UPPER,aNum.byteValue[1]);
	delayMicroseconds(SPI3_DELAY);  // small delay

	// larger delay
	delay(50);

	// need to update frame period to cause shutter value to take effect
	aNum.byteValue[1] = read_register(ADNS3080_FRAME_PERIOD_UPPER);
	delayMicroseconds(SPI3_DELAY);  // small delay
    aNum.byteValue[0] = read_register(ADNS3080_FRAME_PERIOD_LOWER);
	delayMicroseconds(SPI3_DELAY);  // small delay
	write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER,aNum.byteValue[0]);
	delayMicroseconds(SPI3_DELAY);  // small delay
	write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER,aNum.byteValue[1]);
	delayMicroseconds(SPI3_DELAY);  // small delay
}

// clear_motion - will cause the Delta_X, Delta_Y, and internal motion registers to be cleared
void
AP_OpticalFlow_ADNS3080_APM2::clear_motion()
{
    write_register(ADNS3080_MOTION_CLEAR,0xFF);  // writing anything to this register will clear the sensor's motion registers
	x = 0;
	y = 0;
	dx = 0;
	dy = 0;
	_motion = false;
}

// get_pixel_data - captures an image from the sensor and stores it to the pixe_data array
void
AP_OpticalFlow_ADNS3080_APM2::print_pixel_data(Stream *serPort)
{
    int i,j;
	bool isFirstPixel = true;
	byte regValue;
	byte pixelValue;

    // write to frame capture register to force capture of frame
	write_register(ADNS3080_FRAME_CAPTURE,0x83);

	// wait 3 frame periods + 10 nanoseconds for frame to be captured
	delayMicroseconds(1510);  // min frame speed is 2000 frames/second so 1 frame = 500 nano seconds.  so 500 x 3 + 10 = 1510

    // display the pixel data
	for( i=0; i<ADNS3080_PIXELS_Y; i++ ) {
	    for( j=0; j<ADNS3080_PIXELS_X; j++ ) {
		    regValue = read_register(ADNS3080_FRAME_CAPTURE);
		    if( isFirstPixel && (regValue & 0x40) == 0 ) {
			    serPort->println("failed to find first pixel");
			}
			isFirstPixel = false;
			pixelValue = ( regValue << 2);
			serPort->print(pixelValue,DEC);
			if( j!= ADNS3080_PIXELS_X-1 )
				serPort->print(",");
			delayMicroseconds(SPI3_DELAY);
		}
		serPort->println();
	}

	// hardware reset to restore sensor to normal operation
	reset();
}