/* Multitasking Example

	This example code is in the Public Domain (or CC0 licensed, at your option.)

	Unless required by applicable law or agreed to in writing, this
	software is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
	CONDITIONS OF ANY KIND, either express or implied.
*/

#include <stdio.h>
#include <string.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "driver/gpio.h"
#include "driver/i2c.h"
#include "driver/uart.h"
#include "driver/adc.h"
#include "esp_adc_cal.h"
#include "driver_mpu6050_basic.h"

#define SAMPLE_TIME 100
#define GX_BUFFER_SIZE 512

#define LED1 (GPIO_NUM_13)
#define LED2 (GPIO_NUM_12)
#define LED3 (GPIO_NUM_14)
#define LED4 (GPIO_NUM_27)
#define BLINK_TIME 500

#define BAT_BLINK_FREQ_SLOW 1
#define BAT_BLINK_FREQ_FAST 4

#define LED_BAT (GPIO_NUM_25)
#define ANALOG_IN (GPIO_NUM_32)

static esp_adc_cal_characteristics_t adc1_chars;

// handle of queue from sensor_task to dsp_task
static QueueHandle_t xQueue = NULL;

// data structure for gyro x axis data
struct gx_block {
	float data[GX_BUFFER_SIZE];
};

void sensor_task(void *pvParameter)
{
	uint8_t err_code;
	float acc_data[3];
	float gyro_data[3];
	
	mpu6050_basic_init(MPU6050_ADDRESS_AD0_LOW);
	
	struct gx_block gx_send_buffer;

	while (1) 
	{
		// accumulate 512 points of data to be sent to dsp_task
		for (int i = 0; i < GX_BUFFER_SIZE; i++) 
		{
			err_code = mpu6050_basic_read(acc_data, gyro_data);
			// printf("gyro_x = %.2f\n", gyro_data[0]);
			
			gx_send_buffer.data[i] = gyro_data[0];

			vTaskDelay(SAMPLE_TIME / portTICK_PERIOD_MS);
		}

		// send the data block to dsp_task
		xQueueSend(xQueue, &gx_send_buffer, 0);
	}
	vTaskDelete( NULL );
}

void dsp_task(void *pvParameter) 
{
	struct gx_block gx_recv_buffer;
	float min_gx, max_gx, avg_gx, sum_gx;	

	while (1)
	{
		xQueueReceive(xQueue, &gx_recv_buffer, portMAX_DELAY);
		
		// initialize values to be computed
		min_gx = 0; 
		max_gx = 0;
		avg_gx = 0;
		sum_gx = 0;

		// iterate over received gyro data 
		for (int i = 0; i < GX_BUFFER_SIZE; i++) 
		{
			if (gx_recv_buffer.data[i] < min_gx) min_gx = gx_recv_buffer.data[i];
			if (gx_recv_buffer.data[i] > max_gx) max_gx = gx_recv_buffer.data[i];
			sum_gx += gx_recv_buffer.data[i];
		}

		avg_gx = sum_gx / GX_BUFFER_SIZE;

		// do other stuff with calculated parameters:
		// ...
		// printf("gx block read, stats:\n");
		// printf("min: %.2f, max: %.2f, avg: %.2f\n", min_gx, max_gx, avg_gx);
	}

}

void led_task(void *pvParameter)
{
	gpio_reset_pin(LED1);
	gpio_reset_pin(LED2);
	gpio_reset_pin(LED3);
	gpio_reset_pin(LED4);

	gpio_set_direction(LED1, GPIO_MODE_OUTPUT);
	gpio_set_direction(LED2, GPIO_MODE_OUTPUT);
	gpio_set_direction(LED3, GPIO_MODE_OUTPUT);
	gpio_set_direction(LED4, GPIO_MODE_OUTPUT);
	
	uint8_t led_value = 0;
	uint8_t pattern = 0b10100000;

	while (1) 
	{
		if (pattern & 0b10000000) gpio_set_level(LED1, led_value);
		if (pattern & 0b01000000) gpio_set_level(LED2, led_value);
		if (pattern & 0b00100000) gpio_set_level(LED3, led_value);
		if (pattern & 0b00010000) gpio_set_level(LED4, led_value);

		led_value = 1 - led_value;
		vTaskDelay(BLINK_TIME / portTICK_PERIOD_MS);
	}
	vTaskDelete( NULL );
}

enum battery_led_state {OFF, SLOW,	FAST};
enum charging_state {CHARGING,	DISCHARGING, UNKNOWN};

static void battery_task(void *arg)
{
	uint32_t voltage;
	esp_adc_cal_characterize(ADC_UNIT_1, ADC_ATTEN_DB_11, ADC_WIDTH_BIT_12, 0, &adc1_chars);
	ESP_ERROR_CHECK(adc1_config_width(ADC_WIDTH_BIT_12));
	ESP_ERROR_CHECK(adc1_config_channel_atten(ADC1_CHANNEL_4, ADC_ATTEN_DB_11));

	// variables to keep track of states 
	battery_led_state bl_state;
	charging_state c_state = UNKNOWN;  
	uint32_t prev_voltage;

	gpio_reset_pin(LED_BAT);
	gpio_set_direction(LED_BAT, GPIO_MODE_OUTPUT);
	uint8_t led_value = 0;

	while (1) 
	{
		voltage = esp_adc_cal_raw_to_voltage(adc1_get_raw(ADC1_CHANNEL_4), &adc1_chars);
		
		// jump out on first loop, necessary?
		if (voltage > prev_voltage) c_state = CHARGING;
		else c_state = DISCHARGING;

		switch (c_state) {
			case UNKNOWN: 
				break;

			case CHARGING: 
				if (voltage >= 2700) bl_state = OFF;
				else if (voltage < 2200) bl_state = FAST;
				else bl_state = SLOW;
				break;

			case DISCHARGING:
				if (voltage >= 1500) bl_state = OFF;
				else if (voltage < 1200) bl_state = FAST;
				else bl_state = SLOW;
		}

		// choose blinking frequency according to bl_state
		uint8_t f_blink = (bl_state == FAST) ? BAT_BLINK_FREQ_FAST : BAT_BLINK_FREQ_SLOW;

		// printf("volt = %d\n", voltage);

		for (int i = 0; i < f_blink; i++) {
			if (bl_state == OFF) {
				led_value = 0;
			} 
			else {
				led_value = !led_value;
			}
			gpio_set_level(LED_BAT, led_value);
			vTaskDelay(BLINK_TIME / (portTICK_PERIOD_MS * f_blink));	
		}

		prev_voltage = voltage;
	}
}

extern "C" void app_main()
{
	xQueue = xQueueCreate(1, sizeof(gx_block));
	xTaskCreate(&sensor_task, "sensor_task", 16384,	NULL, 5, NULL);
	xTaskCreate(&dsp_task, "dsp_task", 16384,	NULL, 5, NULL);
	xTaskCreate(&led_task, "led_task", 16384, NULL, 5, NULL);
	xTaskCreate(&battery_task, "battery_task", 16384, NULL, 10, NULL);
}