/*!
  * @file
  * @brief test accellerometer and LCD
  *
  * note that both the LCD and MCU are i2c devices; so wiring is really simple.
	* 
	* Calculates the veclocity and position (ignores rotation cos we don't have a physics model for feedback)
	* 
	* By modulating the acceleration we can control the velocity and postion.
	* 
	* Physics model here: (works with single dimension or 3-d vector)
	* 	- \f$ dv = a * dt				\f$
	* 	- \f$ v_1 = v_0 + dv		\f$
	* 
	* 	- \f$ dp = v * dt				\f$
	* 	- \f$ p_1 = p_0 + dp		\f$
	* 
	*/

#include <LiquidCrystal_I2C.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>

// objects
const int lcd_w = 20; ///< width of lcd
const int lcd_h = 4; ///< height of lcd

/*!
 * display
 */
LiquidCrystal_I2C lcd(0x27,lcd_w,lcd_h);

/*!
 * the accelerometer
 */
Adafruit_MPU6050 mpu;

/*!
 * @brief clear top row
 */
void show_top_clear()
{
  lcd.setCursor(0,0);
  //        |....v....1....v....2
  lcd.print("                    ");
  lcd.setCursor(0,0);
}

/*!
 * @brief show top row legend
 */
void show_top_legend(const char * Legend)
{
  show_top_clear();
  lcd.print(Legend);
  
  Serial.println(Legend);
}

/*!
 * @brief show a value
 */
void show_val(
  const int X,
  const int Y,
  const char * Legend,
  int Val
  )
{
  lcd.setCursor(X,Y);
  lcd.print(Legend);
  //lcd.setCursor(X+1,Y);
  //        |-99.99
  //        |....v..
  //lcd.print("      ");
  lcd.setCursor(X+1,Y);
  lcd.print(Val);
  
  //Serial.print(Legend);
  //Serial.println(Val);
}

/*!
 * @brief show a value
 */
void show_val(
  const int X,
  const int Y,
//   const char * Legend,
  float Val
  )
{
  //lcd.setCursor(X+1,Y);
  //        |-99.99
  //        | -9.99
  //        | 99.99
  //        |  9.99
  //        |....v..
  //lcd.print("      ");

  lcd.setCursor(X,Y);
  if(Val<0){
    if(Val>-10) lcd.print(" ");
    }
  else{
    lcd.print(" ");
    if(Val<10) lcd.print(" ");
    }

  lcd.print(Val);
  
  //Serial.print(Legend);
  //Serial.println(Val);
}

/*!
 * @brief show a value
 */
void show_val(
  const int X,
  const int Y,
  const char * Legend,
  unsigned long Val
  )
{
  lcd.setCursor(X,Y);
  lcd.print(Legend);
  //lcd.setCursor(X+1,Y);
  //        |-99.99
  //        |....v..
  lcd.print("       ");
  lcd.setCursor(X+1,Y);
  lcd.print(Val);
  
  //Serial.print(Legend);
  //Serial.println(Val);
}

///////////////////////
/// vectors

 /*!
	* vector
	*/
class TVector{
public:
// protected:
	float x; ///< x
	float y; ///< y
	float z; ///< z
public:
	/*!
	 * @brief constructor
	 */
	TVector():x(0),y(0),z(0){}
	/*!
	 * @brief constructor
	 */
	TVector(float X,float Y,float Z){
		x=X;
		y=Y;
		z=Z;
		}
	/*!
	 * @brief add to vector
	 */
	void add(TVector &That){
		x+=That.x;
		y+=That.y;
		z+=That.z;
		}
	/*!
	 * @brief add to vector
	 */
	void add(float X,float Y,float Z){
		x+=X;
		y+=Y;
		z+=Z;
		}
};
/// vectors
///////////////////////////////

unsigned long t0; ///< last time

/*!
 * @brief setup the machine
 */
void setup() {
  Serial.begin(9600);
  Serial.println("booting...");
  lcd.begin(lcd_w,lcd_h);
  lcd.backlight();
  show_top_legend("lcd running");

  while (!mpu.begin()) {
    show_top_legend("MPU6050 not connected!");
    delay(1000);
  }
  show_top_legend("MPU6050 ready!");

  show_top_legend("booted.");

  lcd.clear();
	t0 = millis();
 }


TVector position; ///< position
TVector velocity; ///< velocity

sensors_event_t a; ///< the sensor structs
sensors_event_t g; ///< the sensor structs
sensors_event_t temp; ///< the sensor structs

const int zzz = 100; ///< how long to kip
//int N=0;

/*!
  @brief loop
  */
void loop() {
  //Serial.println(N++);

  mpu.getEvent(&a,&g,&temp);

  unsigned long t1 = millis();
  unsigned long dt_millis = t1 - t0; // need to guard against overflow
  
//   show_val( 0,3,"t",t1);
//   show_val(15,3,"d",dt_millis);
   
  show_val( 0,0/*,"X"*/,a.acceleration.x);
  show_val( 6,0/*,"Y"*/,a.acceleration.y);
  show_val(12,0/*,"Z"*/,a.acceleration.z);

  show_val( 0,1/*,"x"*/,g.gyro.x);
  show_val( 6,1/*,"y"*/,g.gyro.y);
  show_val(12,1/*,"z"*/,g.gyro.z);
	
	// some calcs
	float dt = dt_millis/1000.0;
	
	velocity.add(
		(a.acceleration.x*dt),
		(a.acceleration.y*dt),
		(a.acceleration.z*dt)
		);
	position.add(
		(velocity.x*dt),
		(velocity.y*dt),
		(velocity.z*dt)
		);
  show_val( 0,2,velocity.x);
  show_val( 6,2,velocity.y);
  show_val(12,2,velocity.z);

  show_val( 0,3,position.x);
  show_val( 6,3,position.y);
  show_val(12,3,position.z);
	
  // time for tea
  t0 = t1;
  delay(zzz);
}
//eof