/************************************************************/
/* main.ino                                                   */
/*                                                          */
/* Main Program                                              */
/*                                                          */
/* Phone Thant, ENGD2103, December, 2022                    */
/************************************************************/
#include "hal.h"
#include "Concurrent.h"
#include "HeartBeat.h"
#include "Switch1.h"
#include "Counter.h"
#include "Switch2.h"
#include "Scheduler.h"
#include "Traffic.h"
#include "Accelerometer.h"
#include "LED.h"
#include<Wire.h>                //I2C Wire Library 

///Creating instances for each class///
HeartBeat    heartBeatControl;     
Traffic      trafficControl;
Switch1      switch1Control;
Switch2      switch2Control;
Scheduler    SchedulerControl;
LED          LEDControl;
Accelerometer accelerometerControl;
Counter    counterControl;

switch_state_t SW1_state;        //Global Variable for storing the debounce state of the first switch
switch_state_2 SW2_state;      //Global Variable for storing the debounce state of the second switch
unsigned char disp_counter = 0;     //Global varibal for storing the charachter
unsigned char disp_heartbeat = 0;       //Global varibal for storing the HeatBeat Status
unsigned char disp_accelerometer = 0;
unsigned char disp_default = B01111000;
const int MPU=0x68;        //MPU6050 I2C Address
bool traffic, counter, accelerometer, switch1;
//                             F          b          L          U          l          r    
unsigned char number[] = {B01110001, B00111100, B00111000, B00111110, B00000110, B01010000};


void setup() {
  switch2Control.setRunning(true);
  Serial.begin(9600);
  HAL_setupGPIO ();   // GPIO function
  SchedulerControl.setRunning(true);
  heartBeatControl.setRunning(true);
  LEDControl.setRunning(true);
  trafficControl.setTime(6000,6000);

  Wire.begin();                      // Initialize comunication
  Wire.beginTransmission(MPU);       // Start communication with MPU6050 // MPU=0x68
  Wire.write(0x6B);                  // Talk to the register 6B
  Wire.write(0x00);                  // Make reset - place a 0 into the 6B register
  Wire.endTransmission(true);        //end the transmission
  
}


void loop() {
  heartBeatControl.process();
  switch2Control.process();
  SW2_state = switch2Control.getSwitch2State ();
  SchedulerControl.process(SW2_state);

  traffic=SchedulerControl.getRunTraffic();
  counter=SchedulerControl.getRunCounter();
  accelerometer=SchedulerControl.getRunAccelerometer();
  switch1=SchedulerControl.getRunSW1();

  trafficControl.setRunning(traffic);
  counterControl.setRunning(counter);
  accelerometerControl.setRunning(accelerometer);
  switch1Control.setRunning(switch1);

  trafficControl.process();
  accelerometerControl.process();

  Serial.println("Traffic  Counter  Accelerometer  Switch1      X       Y      Z");
  Serial.print(traffic);
  Serial.print("        ");
  Serial.print(counter);
  Serial.print("         ");
  Serial.print(accelerometer);
  Serial.print("            ");
  Serial.print(switch1);
  Serial.print("             ");
  Serial.print(accelerometerControl.getAccelerometerX());
  Serial.print("       ");
  Serial.print(accelerometerControl.getAccelerometerY());
  Serial.print("      ");
  Serial.println(accelerometerControl.getAccelerometerZ());
  
  switch1Control.process();
  SW1_state = switch1Control.getSwitch1State ();
  
  counterControl.process(SW1_state);
  disp_counter = counterControl.getCHState();

  disp_heartbeat = heartBeatControl.getHeartBeatState();
  disp_accelerometer = accelerometerControl.getAccelerometerState();

  if(disp_accelerometer==number[4]) 
    {
      trafficControl.setTime(8000,4000);
    }
  else if (disp_accelerometer==number[5])
    {
      trafficControl.setTime(4000,8000);
    }
  else trafficControl.setTime(6000,6000);

  if(counter)
    {
      //LEDControl.process(disp_counter);
      LEDControl.process(disp_heartbeat|disp_counter);

    }

  else if(accelerometer)
    {
      //LEDControl.process(disp_accelerometer);
      LEDControl.process(disp_heartbeat|disp_accelerometer);
    }

  else 
    {
      //LEDControl.process(disp_default);
      LEDControl.process(disp_heartbeat|disp_default);
    }
  //Serial.println(accelerometer);
  //Serial.println(disp_accelerometer, BIN);

  Serial.println(disp_heartbeat, BIN);
  Serial.println(disp_heartbeat|disp_default, BIN);
}