/*
 ============================================================================
 * @file    main.cpp (180.ARM_Peripherals/Sources/main.cpp)
 * @brief   Basic C++ demo using GPIO class
 *
 *  Created on: 10/1/2016
 *      Author: podonoghue
 ============================================================================
 */
#include <stdio.h>
#include <stdlib.h>
#include "system.h"
#include "derivative.h"
#include "hardware.h"
#include "lcd.h"
#include "spi.h"
#include "delay.h"
#include <math.h>
#include "i2c.h"
#include "mma845x.h"
#include <pit.h>

using namespace USBDM;

// I2C interface
I2c0     i2c0;

// Accelerometer via I2C
MMA845x  accelerometer(i2c0, MMA845x::ACCEL_2Gmode);

// SPI interface
Spi0 spi;

// LCD interface using SPI
Lcd lcd(spi);

using Ground = GpioC<10>;
using fPin = GpioA<13>;

/* ************************************************** */

/// LCD derived dimensions
static constexpr int LCD_WIDTH  = (LCD_X_MAX-LCD_X_MIN);
static constexpr int LCD_HEIGHT = (LCD_Y_MAX-LCD_Y_MIN);
static constexpr int CENTRE_X   = ((LCD_X_MAX-LCD_X_MIN)/2);
static constexpr int CENTRE_Y   = ((LCD_Y_MAX-LCD_Y_MIN)/2);

// Colour for LCD background
static constexpr int BACKGROUND_COLOUR = (PURPLE);

// Colour for LCD foreground
static constexpr int FOREGROUND_COLOUR = (GREEN);

// Radius used for the moving circle
static constexpr int CIRCLE_RADIUS = (15);

/*
 * Draws a cursor on the lcd screen
 *
 * @param x       x position
 * @param y       y position
 * @param colour  Colour of cursor
 *
 * @note Done this way so a more sophisticated cursor can be added
 */

void drawCursor(int x, int y, int colour) {
   lcd.drawCircle(x, y, CIRCLE_RADIUS, colour);

   lcd.drawLine((x - CIRCLE_RADIUS), y, (x + CIRCLE_RADIUS), y, colour);
   lcd.drawLine(x, (y - CIRCLE_RADIUS), x, (y + CIRCLE_RADIUS), colour);
}

// Timer LED connection - change as required
/**
 * This example uses FTM interrupts.
 *
 * It is necessary enable these in Configure.usbdmProject under the "Peripheral Parameters"->FTM tab
 * Select irqHandlerInstalled option.
 */

// Timer channel being used - change as required
using Note = Ftm1;

using NoteChannel = Ftm1Channel<1>;

// Half-period for timer
static volatile uint16_t timerHalfPeriod;

static const float WAVEFORM_PERIOD = 100*ms;

/**
 * Interrupt handler for Timer interrupts
 * This sets the next interrupt/pin toggle for a half-period from the last event
 *
 * @param[in] status Flags indicating interrupt source channel(s)
 */
static void ftmCallback(uint8_t status) {

	// Check channel
	if (status & NoteChannel::CHANNEL_MASK) {

		// Note: The pin is toggled directly by hardware
		// Re-trigger at last interrupt time + timerHalfPeriod
		NoteChannel::setDeltaEventTime(timerHalfPeriod);
		//Note::setTickFrequency(timerHalfPeriod, 10);
		//printf("calll \n");
		//printf("freq = %i\n", Note::getTickFrequency());
	}
}

void playNote(int distanceToCenter, double note_scale_factor) {
   timerHalfPeriod = 8000 - (100 + (distanceToCenter*note_scale_factor));

   printf("Half Period = %i \n", timerHalfPeriod);
}

int main() {

	Ground::setOutput();

	Ground::off();

	printf("Starting\n");

	uint8_t id = accelerometer.readID();
	printf("Device ID = 0x%02X (should be 0x1A)\n", id);

	printf("Doing simple calibration\n"
	  "Make sure the device is level!\n");
	  waitMS(2000);

	if (!accelerometer.calibrateAccelerometer()) {
		printf("Calibration failed!\n");
		__asm__("bkpt");
	}

	// Make sure we have new values
	waitMS(100);

	printf("After calibration\n");

	// Draw pretty pattern
	lcd.clear(BACKGROUND_COLOUR);

	// Cursor position on screen
	double note_scale_factor = 107.608695652;
	double LCD_scale_factor = 0.0113636364; //(LCD_WIDTH-2*CIRCLE_RADIUS)/ACCELEROMETER_RANGE //// (130-2*15)/8800
	int x = 65, y = 65;
	int xOld, yOld;
	int distanceToCenter = 0;

	int accelStatus;
	int16_t accelX,accelY,accelZ;

	// Do default configure
	Note::configure(FtmMode_LeftAlign, FtmClockSource_System, FtmPrescale_1);

	Note::setMeasurementPeriod(1.1*WAVEFORM_PERIOD/2.0);

    // Set handler for channel 1 programmatically
	Note::setChannelCallback(ftmCallback);

	timerHalfPeriod = Note::convertSecondsToTicks(WAVEFORM_PERIOD/2.0);
	//printf("Half Period = %i \n", timerHalfPeriod);

	Note::enableNvicInterrupts();

    // Setup debouncer to execute @PIT_FREQUENCY
	//Note::configureChannelInTicks(1, ::SystemBusClock/timerHalfPeriod);



    // Enable interrupts on the channel
	//Note::enableInterrupts(1);

	NoteChannel::setDriveStrength(PinDriveStrength_High);

	NoteChannel::setRelativeEventTime(100);

	NoteChannel::configure(FtmChMode_OutputCompareToggle, FtmChannelIrq_Enable);

    // Check for errors so far
    checkError();

	for(;;) {

	  lcd.drawLine(LCD_X_MIN, CENTRE_Y, LCD_X_MAX, CENTRE_Y, FOREGROUND_COLOUR);
	  lcd.drawLine(CENTRE_X, LCD_Y_MIN, CENTRE_X, LCD_Y_MAX, FOREGROUND_COLOUR);

	  accelerometer.readAccelerometerXYZ(accelStatus, accelX, accelY, accelZ);

	  x = 15 + (accelX+4400)*LCD_scale_factor;
	  y = 130 - (15 + (accelY+4400)*LCD_scale_factor);

	  //printf("accelX = %i			accelY = %i\n", accelX, accelY);
	  //printf("     x = %i				 y = %i\n", x, y);
	  //printf("distance = %i \n", distanceToCenter);
	  //printf("fPin value = %i \n", fPin::read());

	  drawCursor(yOld, xOld, BACKGROUND_COLOUR);

	  drawCursor(y, x, FOREGROUND_COLOUR);

	  xOld = x;
	  yOld = y;

	  distanceToCenter = sqrt(pow((x-65),2) + pow((y-65),2)); // range = [0, 92] **integers**

	  playNote(distanceToCenter, note_scale_factor);
	}
}