/* g++ -g -Wall stDev.cpp -o stDev  `pkg-config --cflags --libs opencv4`  */

#include <iostream>
#include <fstream>
#include <string>
#include <cmath>
#include <unistd.h>
#include <opencv2/core/core.hpp>

// Library to include for
// drawing shapes
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc.hpp>
using namespace cv;
using namespace std;

 int num=0;
 int val=0;
 double sumxyz=0.0;
 double avg=0.0;
 double stDev=0.0;
 double err=0.0;
 ifstream myFile_Handler;
int ReadSensor(string FileName);
int main(){
string FileX="/sys/bus/iio/devices/iio:device2/in_accel_x_raw";
string FileY="/sys/bus/iio/devices/iio:device2/in_accel_y_raw";
string FileZ="/sys/bus/iio/devices/iio:device2/in_accel_z_raw";

	Mat image(300, 700, CV_8UC3,Scalar(0, 0, 0)); //creates a new blank image

	// Check if the image is created successfully
	if (!image.data) {
		cout << "Could not open or find" << " the image";

		return 0;
	}
//assume variant coords +/- 50
/*
for (;;){
sumxyz=ReadSensor(FileX) + ReadSensor(FileY) + ReadSensor(FileZ) ;
	//cout << " X: " << ReadSensor(FileX) << " Y: " << ReadSensor(FileY) << " Z: "<< ReadSensor(FileZ) << endl;
	cout << " XYZ : " <<sumxyz << endl;
	sleep(1);
	cout << "\x1b[A" << "\33[2K\r" << std::flush;
}*/
for (;;){
//init lines at p1 (0,0) abs() because I only care about magnitude

	Point p1(0, 0);
	Point p2(5,  abs(ReadSensor(FileX)));  //blue
	Point p3(10, abs(ReadSensor(FileY ))/100); //red
	Point p4(15, abs(ReadSensor(FileZ))/100); //green
	int thickness = 2;

//	sleep(1);
image.setTo(Scalar(0,0,0)); clears the old image by writing zeros

	// write new Antialiased lines to image
	line(image, p1, p2, Scalar(0, 0, 255), thickness, LINE_AA);
	line(image, p1, p3, Scalar(255, 0, 0), thickness, LINE_AA);
	line(image, p1, p4, Scalar(0,  255, 0), thickness, LINE_AA);
		
	// Show our image inside window
	imshow("Output", image);
	waitKey(1);  //loops every 1 ms

	}
		    return 0;
}
//reads a file handle String for whatever sensor and returns its value
 int ReadSensor(string FileName){
	     int val;
	     ifstream myFile_Handler;
    string myLine;
    myFile_Handler.open(FileName);
// /sys/bus/iio/devices/iio\:device2/in_accel_x_raw
    if(myFile_Handler.is_open())    {
        while(getline(myFile_Handler, myLine)){
               val = std::stoi(myLine);
        }
    myFile_Handler.close();
    }    else    {
        cout << "Unable to open the file!";
    }

  return val;
	 }