#include <NAxisMotion.h>
#include <Wire.h>
#include "Belt.h"
#include "logs.h"

// Belt sensors
int PhotoSenspin1 = 0; // premier photo sensor
int PhotoSensReading1; //premier photo sensor
int PhotoSenspin2 = 1; // deuxieme photo sensor
int PhotoSensReading2; //deuxieme photo sensor

//Object that for the sensor
NAxisMotion mySensor;

//Flag to indicate if an interrupt was detected
bool intDetected = false;

//At a Range of 4g, the threshold
//is set at 39.05mg or 0.3830m/s2.
//This Range is the default for NDOF Mode
int threshold = 5;

//At a filter Bandwidth of 62.5Hz,
//the duration is 8ms.
//This Bandwidth is the default for NDOF Mode
int duration = 1;
bool anyMotion = true;

//To store the last streamed time stamp
unsigned long lastStreamTime = 0;
const int streamPeriod = 500;

Belt::Belt()
{
    _pin_pressure_bottom = 0;
    _pin_pressure_top = 1;
    _pin_flexion = 2;
    _pin_gyro_x = 3;
    _pin_gyro_y = 4;
    _pin_gyro_z = 5;
}

void Belt::read_pressure_top()
{
    _pressure_top = analogRead(_pin_pressure_top);
}
void Belt::read_pressure_bottom()
{
    _pressure_bottom = analogRead(_pin_pressure_bottom);
}
void Belt::read_flexion()
{
    _flexion = analogRead(_pin_flexion);
}
void Belt::read_angle_x()
{
    _angle_x = analogRead(_pin_gyro_x);
}
void Belt::read_angle_y()
{
    _angle_y = analogRead(_pin_gyro_y);
}
void Belt::read_angle_z()
{
    _angle_z = analogRead(_pin_gyro_z);
}

static Belt belt;

static void send_pressure_bottom(int pressure)
{
    serial_printf("PRESSURE_BOTTOM:%d\n", pressure);
}
static void send_pressure_top(int pressure)
{
    serial_printf("PRESSURE_TOP:%d\n", pressure);
}
static void send_flexion(int flexion)
{
    serial_printf("FLEXION:%d\n", flexion);
}
static void send_angle_x(int angle)
{
    serial_printf("ANGLE_X:%d\n", angle);
}
static void send_angle_y(int angle)
{
    serial_printf("ANGLE_Y:%d\n", angle);
}
static void send_angle_z(int angle)
{
    serial_printf("ANGLE_Z:%d\n", angle);
}

void setup()
{
    Serial.begin(115200); //Initialize the Serial Port to view information on the Serial Monitor
    I2C.begin(); //Initialize I2C communication to the let the library communicate with the sensor.
    mySensor.initSensor(); //The I2C Address can be changed here inside this function in the library
    mySensor.setOperationMode(OPERATION_MODE_NDOF); //Can be configured to other operation modes as desired
    mySensor.setUpdateMode(MANUAL); //The default is AUTO. Changing to MANUAL requires calling the relevant update functions prior to calling the read functions
    //Setting to MANUAL requires fewer reads to the sensor
}

void loop()
{
    if ((millis() - lastStreamTime) < streamPeriod) {
        return;
    }

    lastStreamTime = millis();
    mySensor.updateEuler(); //Update the Euler data into the structure of the object
    mySensor.updateCalibStatus(); //Update the Calibration Status
    PhotoSensReading1 = analogRead(PhotoSenspin1);
    PhotoSensReading2 = analogRead(PhotoSenspin2);

    /* fetch the values from the sensors */
    belt.read_pressure_bottom();
    belt.read_pressure_top();
    belt.read_flexion();
    belt.read_angle_x();
    belt.read_angle_y();
    belt.read_angle_z();

    int heading = mySensor.readEulerHeading();
    int pitch = mySensor.readEulerPitch();
    int roll = mySensor.readEulerRoll();

    serial_printf("pitch:        [%d]\n", pitch);
    //pitch = map(pitch, 1, 1023, 90, 270);
    serial_printf("pitch_mapped: [%d]\n", pitch);
    serial_puts("-------");
    serial_printf("roll:         [%d]\n", roll);
    //roll = map(roll, 1, 1023, 90, 270);
    serial_printf("roll_mapped:  [%d]\n", roll);
    serial_puts("-------");
    serial_printf("heading:         [%d]\n", heading);
    //heading = map(heading, 1, 1023, 90, 270);
    serial_printf("heading_mapped:  [%d]\n", heading);

    serial_printf("ANGLE_X:%d\n", pitch);
    serial_printf("ANGLE_Y:%d\n", roll);
    serial_printf("ANGLE_Z:%d\n", heading);

    serial_printf("PRESSURE_TOP:%d\n", PhotoSenspin1);
    serial_printf("PRESSURE_BOTTOM:%d\n", PhotoSenspin1);
}