/*
 * Copyright (c) 2014-2015, Sascha Schade
 * Copyright (c) 2014-2017, Niklas Hauser
 *
 * This file is part of the modm project.
 *
 * This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, You can obtain one at http://mozilla.org/MPL/2.0/.
 */
// ----------------------------------------------------------------------------

#include <modm/board.hpp>
#include <modm/processing.hpp>
#include <modm/driver/position/vl6180.hpp>
#include <modm/debug.hpp>

using Usart2 = BufferedUart<UsartHal2>;
modm::IODeviceWrapper< Usart2, modm::IOBuffer::BlockIfFull > device;

// Set all four logger streams to use the UART
modm::log::Logger modm::log::debug(device);
modm::log::Logger modm::log::info(device);
modm::log::Logger modm::log::warning(device);
modm::log::Logger modm::log::error(device);

// Set the log level
#undef	MODM_LOG_LEVEL
#define	MODM_LOG_LEVEL modm::log::DEBUG

/**
 * Example to demonstrate a MODM driver for distance sensor VL6180
 *
 * This example uses I2cMaster2 of STM32F407
 *
 * SDA		PB11
 * SCL		PB10
 *
 * GND and +3V are connected to the sensor.
 */

typedef I2cMaster2 MyI2cMaster;
// typedef BitBangI2cMaster<GpioB10, GpioB11> MyI2cMaster;

modm::vl6180::Data data;
modm::Vl6180<MyI2cMaster> distance(data);

modm::Fiber fiber_sensor([]
{
	MODM_LOG_DEBUG << "Ping the device from ThreadOne" << modm::endl;
	while (not distance.ping()) modm::this_fiber::sleep_for(100ms);
	MODM_LOG_DEBUG << "Device responded" << modm::endl;

	distance.initialize();
	MODM_LOG_DEBUG << "Device initialized" << modm::endl;

	distance.setIntegrationTime(10);

	while (true)
	{
		auto stamp = modm::Clock::now();

		if (distance.readDistance())
		{
			const auto error = distance.getRangeError();
			if (error == distance.RangeErrorCode::NoError)
			{
				const uint8_t mm = distance.getData().getDistance();
				MODM_LOG_DEBUG << "mm: " << mm;
				Board::LedGreen::set(mm > 160);
				Board::LedBlue::set(mm > 110);
				Board::LedRed::set(mm > 25);
			}
			else {
				MODM_LOG_DEBUG << "Error: " << (uint8_t(error) >> 4);
				Board::LedGreen::set();
				Board::LedBlue::set();
				Board::LedRed::set();
			}
		}

		MODM_LOG_DEBUG << "\tt=" << (modm::Clock::now() - stamp);
		stamp = modm::Clock::now();

		if (distance.readAmbientLight())
		{
			modm::vl6180::ALS_ErrorCode error = distance.getALS_Error();
			if (error == distance.ALS_ErrorCode::NoError)
			{
				uint32_t lux = distance.getData().getAmbientLight();
				MODM_LOG_DEBUG << "\tLux: " << lux;
			}
			else {
				MODM_LOG_DEBUG << "\tError: " << (uint8_t(error) >> 4);
			}
		}

		MODM_LOG_DEBUG << " \tt=" << (modm::Clock::now() - stamp) << modm::endl;

		modm::this_fiber::sleep_for(40ms);
	}
});

modm::Fiber fiber_blink([]
{
	Board::LedOrange::setOutput();
	while(true)
	{
		Board::LedOrange::toggle();
		modm::this_fiber::sleep_for(0.5s);
	}
});

// ----------------------------------------------------------------------------
int
main()
{
	Board::initialize();

	Usart2::connect<GpioA2::Tx>();
	Usart2::initialize<Board::SystemClock, 115200_Bd>();

	MyI2cMaster::connect<GpioB11::Sda, GpioB10::Scl>();
	MyI2cMaster::initialize<Board::SystemClock, 400_kHz>();

	MODM_LOG_INFO << "\n\nWelcome to VL6180X demo!\n\n";

	modm::fiber::Scheduler::run();
	return 0;
}
