/*
 ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010,
 2011,2012 Giovanni Di Sirio.

 This file is part of ChibiOS/RT.

 ChibiOS/RT 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 3 of the License, or
 (at your option) any later version.

 ChibiOS/RT 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, see <http://www.gnu.org/licenses/>.

 ---

 A special exception to the GPL can be applied should you wish to distribute
 a combined work that includes ChibiOS/RT, without being obliged to provide
 the source code for any proprietary components. See the file exception.txt
 for full details of how and when the exception can be applied.
 */

#include <stdio.h>
#include <string.h>

#include "ch.h"
#include "hal.h"
// #include "test.h"
#include "shell.h"
#include "evtimer.h"
#include "chprintf.h"

#include "shellCmds.h"

#include "ap_math.h"
#include "spiInterface.h"
#include "SSD1306.h"
#include "font.h"

static MUTEX_DECL(mutexDisplay);

/*===========================================================================*/
/* Command line related.                                                     */
/*===========================================================================*/

#define SHELL_WA_SIZE THD_WORKING_AREA_SIZE(1024)
#define TEST_WA_SIZE THD_WORKING_AREA_SIZE(256)

#define RPM_ID 0x10d


/*
 * Internal loopback mode, 500KBaud, automatic wakeup, automatic recover
 * from abort mode.
 */

// CAN_BTR_SJW(n), where n = SJW - 1
// CAN_BTR_BRP(n), where n = prescaler - 1
// CAN_BTR_TS1(n), where n = Seg 1 - 1
// CAN_BTR_TS2(n), where n = Seg 2 - 1

static const CANConfig cancfg500 = {
	CAN_MCR_ABOM | CAN_MCR_AWUM | CAN_MCR_TXFP,
	CAN_BTR_LBKM | CAN_BTR_SJW(0) | CAN_BTR_TS2(2) |
		CAN_BTR_TS1(3) | CAN_BTR_BRP(8)};

static const CANConfig cancfg1000 = {
	CAN_MCR_ABOM | CAN_MCR_AWUM | CAN_MCR_TXFP,
	CAN_BTR_LBKM | CAN_BTR_SJW(0) | CAN_BTR_TS2(2) |
		CAN_BTR_TS1(4) | CAN_BTR_BRP(3)};

static const ShellConfig shell_cfg1 = {
	(BaseSequentialStream *)&SD1,
	shellCommands};
////////
// end of shell stuff

uint16_t sampIndex;

void setDriveA(uint8_t bit)
{
	if (bit)
	{
		palSetPad(GPIOA, GPIOA_A1);
		palClearPad(GPIOA, GPIOA_A2);
	}
	else
	{
		palClearPad(GPIOA, GPIOA_A1);
		palSetPad(GPIOA, GPIOA_A2);
	}
}

void setDriveB(uint8_t bit)
{
	if (bit)
	{
		palSetPad(GPIOA, GPIOA_B1);
		palClearPad(GPIOA, GPIOA_B2);
	}
	else
	{
		palClearPad(GPIOA, GPIOA_B1);
		palSetPad(GPIOA, GPIOA_B2);
	}
}

// dial settings
volatile int origin = 0;
;
volatile int target = 0;
volatile int count = 0;

volatile bool pause;
//

void setPause(bool p)
{
	pause = p;
}

static THD_WORKING_AREA(waGaugeThread, 512);
static THD_FUNCTION(gaugeThread, p) {
	(void)p;
	unsigned const fast = 10;
	unsigned const slow = 30;
	unsigned const range = slow - fast;
	unsigned del = fast;
	int step = 1;
	chRegSetThreadName("Step");
	while (TRUE)
	{

		while (pause)
			chThdSleep(1000);

		switch (count % 4)
		{
		case 0:
			setDriveA(1);
			setDriveB(0);
			break;
		case 1:
			setDriveA(1);
			setDriveB(1);
			break;
		case 2:
			setDriveA(0);
			setDriveB(1);
			break;
		case 3:
			setDriveA(0);
			setDriveB(0);
			break;
		}

		// all this calculates minimum distance from
		// target or origin
		int d1 = abs(count - origin);
		int d2 = abs(count - target);
		// finally, minimum distance
		int dist = min(d1,d2); 
		
		del = fast;
		if (dist < range) // inside lower bound of distance
		{
			del = slow - dist;
		}
		chThdSleep(del);

		if (count < target)
		{
			step = 1;
		}
		if (count > target)
		{
			step = -1;
		}
		if (count == target)
		{
			step = 0;
		}
		count = count + step;
	}
}

/*
 * Command Shell Thread
 */
static THD_WORKING_AREA(waShell, 512);
static THD_FUNCTION(shell, p) {
	(void) p;
	thread_t *shelltp = NULL;

	chRegSetThreadName("Shell ");
	/*
	 * in this demo it just performs
	 * a shell respawn upon its termination.
	 */
	while (true)
	{
		if (!shelltp)
		{

			shelltp = chThdCreateFromHeap(NULL, SHELL_WA_SIZE, "shell", NORMALPRIO, shellThread, (void *)&shell_cfg1);
		}
		else
		{
			/* If the previous shell exited.*/
			if (chThdTerminatedX(shelltp))
			{
				/* Recovers memory of the previous shell.*/
				chThdRelease(shelltp);
				shelltp = NULL;
			}
		}
		chThdSleepMilliseconds(500);
	}
}


	static THD_WORKING_AREA(waCanRx, 1024);
static THD_FUNCTION(canRx, p) {
	(void) p;

	// initialize can bus hardware
	event_listener_t el;
	CANRxFrame rxmsg;

	canStart(&CAND1, &cancfg1000);

	chRegSetThreadName("CAN receiver");
	chEvtRegister(&CAND1.rxfull_event, &el, 0);
	while (true)
	{
		if (chEvtWaitAnyTimeout(ALL_EVENTS, TIME_MS2I(100)) == 0)
			continue;
		while (canReceive(&CAND1, CAN_ANY_MAILBOX, &rxmsg, TIME_IMMEDIATE) == MSG_OK)
		{
			/* Process message.*/
		}
	}
	chEvtUnregister(&CAND1.rxfull_event, &el);
}
/*
 * Application entry point.
 */
int main(void)
{
	//  struct EventListener el0, el1;

	/*
	 * System initializations.
	 * - HAL initialization, this also initializes the configured device drivers
	 *   and performs the board-specific initializations.
	 * - Kernel initialization, the main() function becomes a thread and the
	 *   RTOS is active.
	 */
	halInit();
	chSysInit();

	/*
	 * Initializes serial port.
	 */
	sdStart(&SD1, NULL);

	/*
	 * Shell manager initialization.
	 */
	//	shellInit();
	/*
	 * initialise approximate maths
	 */
	ap_init();

	chMtxLock(&mutexDisplay);

	/* start the SPI hardware for display */
	ssd1306spiInit();

	ssd1306_begin(SSD1306_SWITCHCAPVCC, 0);

	clearDisplay();

	display();

	chMtxUnlock(&mutexDisplay);

	/*
	 * Creates the PLL thread
	 */
	chThdCreateStatic(waGaugeThread, sizeof(waGaugeThread), NORMALPRIO, gaugeThread, NULL);

	chThdCreateStatic(waCanRx, sizeof(waCanRx), NORMALPRIO, canRx, NULL);

	chThdCreateStatic(waShell, sizeof(waShell), NORMALPRIO, shell, NULL);

	// reset gauge
	origin = 540;
	count = 540;
	target = 0;
	chThdSleepMilliseconds(1000);
	target = 0;

	int frac = 0;
	/* start the SPI hardware for display */
	while (1)
	{

		chThdSleepMilliseconds(100);
		unsigned const SCALE = frac * 32 + 16;
		frac = (++frac) % 10;

		// read the dial

		origin = count;

		target = target + 1;

		// target = lidar * 630L / 2000L;

		if (target > 540)
			target = 0;

		chMtxLock(&mutexDisplay);

		font_gotoxy(0, 0);
		clearDisplay();

		//    print_scaled_string("cm:",0,16,4,SCALE);

		print_digits(0, 0, 4, 1, target, SCALE);

		chMtxUnlock(&mutexDisplay);

		display();
	}
}