//
// Sensor module common operations library
//
// Department of Mechatronics
// Tallinn University of Technology
// © Copyrights 2009
//

//
// Use timer operations
//
#if defined (__AVR_ATmega128__)
#include "timer128.h"
#elif defined (__AVR_ATmega2561__)
#include "timer2561.h"
#endif

//
// Include delay.h for delays
//
#include <util/delay.h>
#include "pin.h"
#include "bit.h"

//
// Take use of program space and interrupt functions
//
#include <avr/pgmspace.h>
#include <avr/interrupt.h>

//
// Include sensors.h for constants
//
#include "module/sensors.h"

//
// Temperature to ADC value conversion table(s)
//
const unsigned short THERMISTOR_TEMP_ADC_TABLE[] PROGMEM =
{
	91,96,102,107,113,119,125,132,139,146,153,
	160,168,176,184,192,201,210,219,228,238,247,
	257,267,277,288,298,309,319,330,341,352,364,
	375,386,398,409,421,432,444,455,467,478,489,
	501,512,523,534,545,556,567,578,588,599,609,
	619,629,639,649,658,667,677,685,694,703,711,
	720,728,736,743,751,758,766,773,780,786,793,
	799,805,811,817,823,829,834,839,844,849,854,
	859,863,868,872,876,880,884,888,892,896,899,
	903,906,909,912,915,918,921,924,927,929,932,
	934,937,939,941,943,945,947,949,951,953,955
};

//
// Default IR distance sensors
// 
const ir_distance_sensor GP2Y0A21YK = { 5461, -17, 2 };

//
// Ultrasonic distance sensor pins
//
static pin ultrasonic_trigger  = PIN(E, 5);
static pin ultrasonic_echo     = PIN(E, 4);
 
//
// Ultrasonic distance sensor configuration
//
#define ULTRASONIC_TIMEOUT_DISTANCE  100   // cm
#define ULTRASONIC_MAX_DISTANCE      300   // cm
#define ULTRASONIC_SPEED_OF_SOUND    33000 // cm/s
#define ULTRASONIC_CM_PERIOD         (F_CPU / ULTRASONIC_SPEED_OF_SOUND * 2)
#define ULTRASONIC_DELAY_PERIOD      400

//
// Ultrasonic distance sensor data structure
//
static struct
{
	volatile unsigned char state;	
	volatile bool last_echo_value;
	volatile unsigned short counter;
	volatile unsigned short result;
	void (*result_function_pointer)(bool);
}
ultrasonic;

//
// Ultrasonic states
//
#define ULTRASONIC_STATE_IDLE      0
#define ULTRASONIC_STATE_WAIT_ECHO 1
#define ULTRASONIC_STATE_MEASURE   2
#define ULTRASONIC_STATE_DONE      3
#define ULTRASONIC_STATE_FAILED    4
#define ULTRASONIC_STATE_DELAY     5

//
// Ultrasonic state callback routine
//
#define ULTRASONIC_RESULT_CALLBACK(result) \
	if (ultrasonic.result_function_pointer != 0) \
	{ \
		ultrasonic.result_function_pointer(result); \
	}

//
// Calculate temperature in celsius
// from the temperature to ADC value conversion table
//
signed short thermistor_calculate_celsius_from_table(
	const unsigned short *table_ptr,
	signed short min_temp,
	signed short max_temp,
	unsigned short adc_value)
{
	signed short celsius;

	// Get celsius temperature from table
	for (celsius = max_temp - min_temp; celsius >= 0; celsius--)
	{
		if (adc_value >= pgm_read_word(&table_ptr[celsius]))
		{
			return celsius + min_temp;
		}
	}

	// If value not in table, return minimum temperature
	return min_temp;
}

//
// Calculate IR distance sensor distance in centimeters from ADC value.
// Returns -1 if cannot calculate.
//
signed short ir_distance_calculate_cm(ir_distance_sensor sensor, unsigned short adc_value)
{
	if (adc_value + sensor.b <= 0)
	{
		return -1;
	}
	
	return sensor.a / (adc_value + sensor.b) - sensor.k;
}

//
// Ultrasonic initialization
//
void ultrasonic_async_init(pin trigger, pin echo)
{
	// Initial state
	ultrasonic.state  = ULTRASONIC_STATE_IDLE;
	ultrasonic.result = 0;
	ultrasonic.result_function_pointer = 0;
	
	// Set pins
	ultrasonic_trigger = trigger;
	ultrasonic_echo    = echo;
 
	// Pin configuration
	pin_setup_output(ultrasonic_trigger);
	pin_setup_input_with_pullup(ultrasonic_echo);

	// Set timer 3 to CTC mode
	timer3_init_ctc(
		TIMER3_NO_PRESCALE,
		TIMER3_CTC_TOP_ICR);
 
	// Timer top value:
	// CPU clock / speed of sound * 2
	timer3_set_input_capture_value(ULTRASONIC_CM_PERIOD);
}

//
// Ultrasonic initialization
//
void ultrasonic_async_setup_result_callback(void (*function_pointer)(bool))
{
	ultrasonic.result_function_pointer = function_pointer;
}

//
// Ultrasonic stop
//
inline void ultrasonic_async_stop(void)
{
	// Disable timer interrupt
	timer3_input_capture_interrupt_enable(false);	
}

//
// Timer interrupt
//
ISR(TIMER3_CAPT_vect)
{	
	// Get echo pin value
	unsigned char echo = pin_get_value(ultrasonic_echo);

	// Increase counter
	ultrasonic.counter++;

	// Lower trigger pin
	pin_clear(ultrasonic_trigger);

	// What state ?
	switch (ultrasonic.state)
	{
		// Waiting for echo
		case ULTRASONIC_STATE_WAIT_ECHO:

			// Rising edge ?
			if ((echo) && (!ultrasonic.last_echo_value))
			{
				// Reset timer
				timer3_set_value(0);
				ultrasonic.counter = 0;

				// Clear timer capture flag (just in case)
				timer3_input_capture_flag_clear();
		
				// New state - start measuring
				ultrasonic.state = ULTRASONIC_STATE_MEASURE;				
			}

			// Timeout ?
			else if (ultrasonic.counter == ULTRASONIC_TIMEOUT_DISTANCE)
			{
				// Stop ultrasonic
				ultrasonic_async_stop();

				// New state - failure
				ultrasonic.state = ULTRASONIC_STATE_FAILED;
				
				// Callback function.
				ULTRASONIC_RESULT_CALLBACK(false);				

				// Result 0
				ultrasonic.result = 0;
			}
			break;

		// Measuring state
		case ULTRASONIC_STATE_MEASURE:

			// Falling edge ?
			if ((!echo) && (ultrasonic.last_echo_value))
			{				
				// Get result
				ultrasonic.result = ultrasonic.counter;	
				
				// New state - delay
				ultrasonic.state = ULTRASONIC_STATE_DELAY;
				ultrasonic.counter = 0;				
			}

			// Too far distance ?
			else if (ultrasonic.counter == ULTRASONIC_MAX_DISTANCE)
			{			
				// Maximum result
				ultrasonic.result = ULTRASONIC_MAX_DISTANCE;		
				
				// New state - delay
				ultrasonic.state = ULTRASONIC_STATE_DELAY;
				ultrasonic.counter = 0;				
			}
			break;
			
		// Delay state
		case ULTRASONIC_STATE_DELAY:
			
			// Wait a while
			if (ultrasonic.counter >= ULTRASONIC_DELAY_PERIOD)
			{
				// Stop ultrasonic
				ultrasonic_async_stop();
			
				// New state - done
				ultrasonic.state = ULTRASONIC_STATE_DONE;
				
				// Callback function.
				ULTRASONIC_RESULT_CALLBACK(true);
			}
			break;
	}

	// Remember last echo pin value
	ultrasonic.last_echo_value = echo;
}

//
// Check the readiness of ultrasonic sensor
//
bool ultrasonic_async_is_ready(void)
{
	return ((ultrasonic.state == ULTRASONIC_STATE_IDLE) ||
	        (ultrasonic.state == ULTRASONIC_STATE_DONE) ||
			(ultrasonic.state == ULTRASONIC_STATE_FAILED));
}

//
// Check the presence of errors
//
inline bool ultrasonic_async_has_errors(void)
{
	return (ultrasonic.state == ULTRASONIC_STATE_FAILED);
}

//
// Check the presence of result of ultrasonic sensors
//
inline bool ultrasonic_async_has_measured(void)
{
	return (ultrasonic.state == ULTRASONIC_STATE_DONE);
}

//
// Start ultrasonic distance measuring with SRF04 (backward compatibility)
//
void ultrasonic_async_start_measuring(void)
{
	// Measuring can only be started when sensor is ready
	if (!ultrasonic_async_is_ready())
	{
		return;
	}

	// New state - wait for echo
	ultrasonic.state = ULTRASONIC_STATE_WAIT_ECHO;

	// Reset counter
	ultrasonic.counter = 0;
	ultrasonic.last_echo_value = pin_get_value(ultrasonic_echo);
 
	// Create trigger pulse rising edge
	pin_set(ultrasonic_trigger);
	
	// Start timer (for timeout check)
	timer3_input_capture_flag_clear();
	timer3_input_capture_interrupt_enable(true);
}

//
// Get the last result of ultrasonic distance
// measuring result in centimeters
//
inline unsigned short ultrasonic_async_get_result_cm(void)
{
	return ultrasonic.result;
}

//
// Instant ultrasonic distance measuring with SRF04
//
unsigned short ultrasonic_measure_srf04(pin trigger, pin echo)
{	
	// Pin setup
	pin_setup_output(trigger);
	pin_setup_input_with_pullup(echo);

	// Set timer 3 to normal mode
	// with period of F_CPU / 8
	timer3_init_normal(TIMER3_PRESCALE_8);		

	// Create trigger pulse
	pin_set(trigger);
	
	// Reset timer
	timer3_overflow_flag_clear();
	timer3_set_value(0);	
	
	// Wait ~10 us
	while (timer3_get_value() < 18) {}
	
	// End trigger pulse
	pin_clear(trigger);
	
	// Wait for echo start
	while (!pin_get_value(echo))
	{
		// Timeout ?
		if (timer3_overflow_flag_is_set())
		{
			return 0;
		}
	}
	
	// Reset timer again
	timer3_set_value(0);
	
	// Wait for echo end	
	while (pin_get_value(echo))
	{
		// Timeout ?
		if (timer3_overflow_flag_is_set())
		{
			return 0;
		}
	}
	
	// Convert time to distance:
	//   distance = timer * (1 / (F_CPU / 8)) * speed / 2
	return (unsigned long)timer3_get_value() *
		ULTRASONIC_SPEED_OF_SOUND / (F_CPU / 4);
}

//
// Instant ultrasonic distance measuring with SRF05
//
unsigned short ultrasonic_measure_srf05(pin triggerecho)
{   
    // Pin setup
    pin_setup_output(triggerecho);

    // Set timer 3 to normal mode
    // with period of F_CPU / 8
    timer3_init_normal(TIMER3_PRESCALE_8);      

    // Create trigger pulse
    pin_set(triggerecho);
    
    // Reset timer
    timer3_overflow_flag_clear();
    timer3_set_value(0);    
    
    // Wait ~10 us
    while (timer3_get_value() < 18) {}
    
    // End trigger pulse
    pin_clear(triggerecho);
    
    sw_delay_ms(1);
    // Wait for echo start
    pin_setup_input_with_pullup(triggerecho);
    while (!pin_get_value(triggerecho))
    {
        // Timeout ?
        if (timer3_overflow_flag_is_set())
        {
            return 0;
        }
    }
    
    // Reset timer again
    timer3_set_value(0);
    
    // Wait for echo end    
    while (pin_get_value(triggerecho))
    {
        // Timeout ?
        if (timer3_overflow_flag_is_set())
        {
            return 0;
        }
    }
    
    // Convert time to distance:
    //   distance = timer * (1 / (F_CPU / 8)) * speed / 2
    return (unsigned long)timer3_get_value() *
        ULTRASONIC_SPEED_OF_SOUND / (F_CPU / 4);
}

uint16_t dht_hum = 1;
uint16_t dht_temp = 1;

// Wait while input is low with 100us timeout
static int8_t waitlow(pin dht_pin)
{
    uint8_t timeout = 0;
    while(!pin_get_value(dht_pin))
    {
        _delay_us(1);
        timeout++;
        if(timeout > 200) return -1;
    }
    return 0;
}

// Wait while input is high with 100us timeout
static int8_t waithigh(pin dht_pin)
{
    uint16_t timeout = 0;
    while(pin_get_value(dht_pin))
    {
        _delay_us(1);
        timeout++;
        if(timeout > 200) return -1;
    }
    return 0;
}

// Read high pulse length in us. over 30us = 1, under = 0;
static int8_t GetPulse(pin dht_pin)
{
    uint8_t time = 0;

    // Wait for pulse
    if(waitlow(dht_pin) == -1)
        return -1;

    // Measure pulse width in ~us
    while(*dht_pin.pin & dht_pin.mask)
    {
		time++;
        _delay_us(1);
        if(time > 100) return -1;
    }
    // Return value
	
    if(time > 30)
        return 1;
    return 0;
}

// Read humidity and temperature data and update global variables
// Input sensor type, I/O pin (DHT11, DHT22)
//
int8_t DHT_update(uint8_t sensor,pin dht_pin)
{
    uint8_t humh = 0;
    uint8_t huml = 0;
    uint8_t temph = 0;
    uint8_t templ = 0;
    uint8_t crc = 0;
    uint8_t data_pos = 0;

    // Send start impulse of 20ms low signal
    pin_setup_output(dht_pin);
	pin_set(dht_pin);
    _delay_ms(1);

    pin_clear(dht_pin);
    _delay_ms(20);
    pin_set(dht_pin);
    _delay_us(20);

    // Start reading input
    pin_setup_input_with_pullup(dht_pin);
    _delay_us(20);
    // Ignore first bit
    waitlow(dht_pin);
    waithigh(dht_pin);
    // Read humidity high byte
    for(data_pos = 8; data_pos > 0; data_pos--)
    {
        int8_t temp = GetPulse(dht_pin);

        if (temp == -1) return -1;
        else humh |= (temp << (data_pos-1));
    }
    // Read humidity low byte
    for(data_pos = 8; data_pos > 0; data_pos--)
    {
        int8_t temp = GetPulse(dht_pin);

        if (temp == -1) return -1;
        else huml |= (temp << (data_pos-1));
    }
    // Read temperature high byte
    for(data_pos = 8; data_pos > 0; data_pos--)
    {
        int8_t temp = GetPulse(dht_pin);

        if (temp == -1) return -1;
        else temph |= (temp << (data_pos-1));
    }
    // Read temperature low byte
    for(data_pos = 8; data_pos > 0; data_pos--)
    {
        int8_t temp = GetPulse(dht_pin);

        if (temp == -1) return -1;
        else templ |= (temp << (data_pos-1));
    }
    // Read CRC byte
    for(data_pos = 8; data_pos > 0; data_pos--)
    {
        int8_t temp = GetPulse(dht_pin);

        if (temp == -1) return -1;
        else crc |= (temp << (data_pos-1));
    }
    // Check CRC and update temp and humidity values
    if(((uint16_t)(humh+huml+temph+templ) & 0xFF) == crc)
    {
        if(sensor == DHT11)
        {
            dht_hum = humh;
            dht_temp = temph;
        }
        else if(sensor == DHT22)
        {
            dht_hum = (humh << 8)| huml;
            dht_temp = (temph << 8)| templ;
        }
        return 1;
    }
    else
        return 0;
}