#include <Wire.h>
#include <TimerOne.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_NeoPixel.h>
#include "Adafruit_APDS9960.h"
#include <QTRSensors.h>
#include <Servo.h>
// movement defined values
#define SET_PWM 95 //0 to 100
#define STNDBY1 36
#define STNDBY2 37
#define RUN_DELAY1 4900
#define RUN_DELAY2 5340
#define RUN_DELAY3 1200
#define mdelay 4000
// servo defined values
#define LIFTER_PIN 7
#define LIFTER_ANGLE 100 // 90 degrees max
#define LIFTER_SPEED 10 // milliseconds
#define DELAYVAL 500
// rotaryencoder defined values
#define CLK 19
#define DT 18
#define SW 6
// oled defined values
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define SCREEN_ADDRESS 0x3C // LCD I2C Address
// rgbled neopixel defined values
#define RGB_P 10
#define RGB_C 1
// servomotor defined values
#define COUNTER_WIDTH_MIN 500 //0 angle
#define COUNTER_WIDTH_MAX 2500 //270 angle
// device declarations
Servo myservo;
Adafruit_NeoPixel dio(RGB_C, RGB_P, NEO_GRB + NEO_KHZ800);
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);
Adafruit_APDS9960 apds;
QTRSensors qtr_front;
QTRSensors qtr_back;
QTRSensors qtr_left;
QTRSensors qtr_right;
// enumeration of all possible directions
enum Direction{
  Forward,
  Backward,
  Left,
  Right,
  Forward_Left,
  Forward_Right,
  Backward_Left,
  Backward_Right,
  CW_Center_Center,
  CCW_Center_Center,
  CW_Back_Center,
  CCW_Back_Center,
  CW_Front_Center,
  CCW_Front_Center,
  CW_Center_Left,
  CCW_Center_Left,
  CW_Center_Right,
  CCW_Center_Right,
  Stop
};
// movement variables
unsigned int _pwmvalue = 0;
static bool stopmotor = true;
static int pwmcounter = 0;
bool test_directions = true;
// status variables and rotary encoder variables
boolean RGB_STATE=0;
boolean LIFTER_STATE=0;
boolean COL_SEN=0;
int currentStateCLK;              // gets the current state of the CLK pin
int lastStateCLK;                 // stores the previous state of the CLK pin
String currentDir = "";
unsigned long lastButtonPress = 0;
// encoder counter
int counter = 0;
// count counters
int counter2 = 0;
int counter3 = 0;
int x=0;
int y=0;
int z=0;
int c=0;
int cury=0;
//line sensor variables
const uint8_t SensorCount = 4;
uint16_t sensorValues[SensorCount];

void setup() {
  //init movement
  pinMode(LED_BUILTIN, OUTPUT);
  setupMecanum();
  //init rotary
  pinMode(CLK, INPUT);
  pinMode(DT, INPUT);
  pinMode(SW, INPUT_PULLUP);
  //init neopixel
  dio.begin();
  dio.clear();
  //init comm and display
  Serial.begin(9600);
  lastStateCLK = digitalRead(CLK); // Read the initial state of CLK
  if (!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
  Serial.println(F("SSD1306 allocation failed"));
  for (;;); // Don't proceed, loop forever
  }
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(0,0);
  display.println("RIZAL_HS_READY!");
  display.println("SELECT_ROUTINE");
  display.display();
  //init color sensor
  if(!apds.begin()){
    Serial.println("failed to initialize device! Please check your wiring.");
  }
  else Serial.println("Device initialized!");
  apds.enableColor(true);  
  //init servo
  myservo.attach(LIFTER_PIN);
  setServoAngle(0);
  dio.clear(); 
  dio.setPixelColor(0, dio.Color(0, 0, 0)); 
  dio.show();
}

void loop() {
  currentStateCLK = digitalRead(CLK);   // Read the current state of CLK
  if (currentStateCLK != lastStateCLK  && currentStateCLK == 0) {
    if (digitalRead(DT) != currentStateCLK) { // If the DT state is different than the CLK state then
      if (counter !=3){
      counter ++; // Encoder is rotating CW so increment
      currentDir = "CW";
      }
    } 
    else if (counter!=0){ // Encoder is rotating CCW so decrement
      counter --;
      currentDir = "CCW";
    }
    switch (counter){ //screen display
    case 1:
    selection_in();
    display.setCursor(0,20);  
    display.print("- Movement -");
    Serial.println("- Movement -");
    Serial.println(counter);
    display.display();
    break;
    case 2:
    selection_in();
    display.setCursor(0,20);  
    display.print("- Check I/O -");
    Serial.println("- Check I/O -");    
    Serial.println(counter);
    display.display();
    break;
    case 3:  
    selection_in();
    display.setCursor(0,20);
    display.print("- Calibration -");
    Serial.println("- Calibration -");
    Serial.println(counter);
    display.display();
    break;
    }
  }
  lastStateCLK = currentStateCLK; // Remember last CLK state
  int btnState = digitalRead(SW); // Read the button state
  if (btnState == LOW) { // If we detect LOW signal, button is pressed
    if (millis() - lastButtonPress > 50) { // if 50ms have passed since last LOW pulse, it means that the
      // button has been pressed, released and pressed again
    switch (counter){ // routine execution
    case 1:
    selection_in();
    display.setCursor(0,20);  
    display.println("RUNNING");
    display.println("MOVEMENT ROUTINE");
    Serial.println("- Movement -");
    Serial.println(counter);
    display.display();
    // insert move fucntion
    movement();
    Serial.println("- Movement -");
    Serial.println(counter);
    selection_in();
    display.setCursor(0,20);  
    display.println("MOVEMENT ROUTINE");
    display.println("COMPLETE");    
    display.display();
    counter=0; // resets counter for rotary encoder
    break;
    case 2:
    selection_in();
    display.setCursor(0,20);  
    display.println("RUNNING");
    display.println("I/O CHECK");
    Serial.println("- Check I/O -");    
    Serial.println(counter);
    display.display();
    // input output check functions
    rgbshow();
    dio.clear(); 
    dio.setPixelColor(0, dio.Color(0, 0, 0)); 
    dio.show();
    delay(2000);
    RGB_STATE=1; // enables cyclergb func
    cyclergb();
    dio.clear(); 
    dio.setPixelColor(0, dio.Color(0, 0, 0)); 
    dio.show();
    delay(2000);
    countdown();
    selection_in();
    display.setCursor(0,20);  
    display.println("INPUT/OUTPUT ROUTINE");
    display.println("COMPLETE");    
    display.display();
    counter=0; // resets counter for rotary encoder
    break;
    case 3:  
    selection_in();
    display.setCursor(0,20);
    display.println("RUNNING");
    display.println("CALIBRATION ROUTINE");
    Serial.println("- Calibration -");
    Serial.println(counter);
    display.display();
    // calib functions
    cal_linesensor();
    delay (2000);
    COL_SEN=1; // enables color_rec function
    color_rec();
    dio.clear(); 
    dio.setPixelColor(0, dio.Color(0, 0, 0)); 
    dio.show();
    LIFTER_STATE=1; // enables sensor_lifter function
    sensor_lifter();
    dio.clear(); 
    dio.setPixelColor(0, dio.Color(0, 0, 0)); 
    dio.show();
    selection_in();
    display.setCursor(0,20);  
    display.println("CALIBRATION ROUTINE");
    display.println("COMPLETE");    
    display.display();
    counter=0; // resets counter for rotary encoder
    break;
    default: // case button is pressed witout rotating encoder
    display.clearDisplay();
    selection_in();
    display.setCursor(0,20);
    display.println("SELECTION ERROR");
    Serial.println("SELECTION ERROR");
    Serial.println(counter);
    display.display();
    }
    }
    lastButtonPress = millis();// Remember last button press event
  }
  delay(1);// Put in a slight delay to help debounce the reading
}

//functions
void selection_in()// Selection prompt
{
  display.clearDisplay();
  display.setCursor(0,0);
  display.println("RIZAL_HS_READY!");
  display.println("SELECT_ROUTINE");
}

//movement function
void movement(){
    digitalWrite(LED_BUILTIN, HIGH);
    delay(mdelay);  
    run(Forward, RUN_DELAY1);
    delay(mdelay);    
    run(Backward, RUN_DELAY1);
    delay(mdelay); 
    run(CW_Center_Center, RUN_DELAY3);
    delay(mdelay); 
    run(CCW_Center_Center, RUN_DELAY3); 
    delay(mdelay);
    run(Left, RUN_DELAY2);
    delay(mdelay); 
    run(Right, RUN_DELAY2);
    run(Stop, 0);
    stopMotors();
    digitalWrite(LED_BUILTIN, LOW);
}
// important functions for movement functions
void setupMecanum()
{
  pinMode(STNDBY1, OUTPUT);
  pinMode(STNDBY2, OUTPUT);
  DDRA = DDRA | 0b11111111; //set PORTA to output
  DDRL = DDRL | 0x0F;       //set PORTL to output (Pins 46,47,48,49)
  // DDRA |= 0xFF;
  PORTA = 0b00000000; //set PORT A to LOW
  PORTL &= ~0x0F;     //reverses value of PORTL
  initMotorPwm();
  setMotorPWM(SET_PWM);
}
/*mechanum wheels direction*/
void setMotorDir(Direction _cmd)
{
  switch (_cmd)
  {
  case Forward:
    PORTA = 0xAA;
    break;
  case Backward:
    PORTA = 0x55;
    break;
  case Left:
    PORTA = 0x99;
    break;
  case Right:
    PORTA = 0x66;
    break;
  case Forward_Left:
    PORTA = 0x88;
    break;
  case Forward_Right:
    PORTA = 0x22;
    break;
  case Backward_Left:
    PORTA = 0x44;
    break;
  case Backward_Right:
    PORTA = 0x11;
    break;
  case CW_Center_Center:
    PORTA = 0x69;
    break;
  case CCW_Center_Center:
    PORTA = 0x96;
    break;
  case CW_Back_Center:
    PORTA = 0x5F;
    break;
  case CCW_Back_Center:
    PORTA = 0xAF;
    break;
  case CW_Front_Center:
    PORTA = 0xF5;
    break;
  case CCW_Front_Center:
    PORTA = 0xFA;
    break;
  case CW_Center_Left:
    PORTA = 0x7D;
    break;
  case CCW_Center_Left:
    PORTA = 0xBE;
    break;
  case CW_Center_Right:
    PORTA = 0xEB;
    break;
  case CCW_Center_Right:
    PORTA = 0xD7;
    break;
  case Stop:
    PORTA = 0xFF;
    break;
  default:
    break;
  }
}

void initMotorPwm(void)
{
  Timer1.initialize(100); // 1000us interval for each % of duty cycle
  Timer1.attachInterrupt(MotorPwmHandler);
  Timer1.stop();
}
void prepMotors(void)
{
  stopmotor = false;

  PORTL |= 0x0F;
  // reset pwm
  pwmcounter = 0;
  stopmotor = false;
  // initialize timer
  initMotorPwm();
  // turn on stndbay pins
  // to do: port manipulation
  digitalWrite(STNDBY1, HIGH);
  digitalWrite(STNDBY2, HIGH);
}

void stopMotors(void)
{
  Timer1.stop();
  setMotorDir(Stop);
  delay(200);
  digitalWrite(STNDBY1, LOW);
  digitalWrite(STNDBY2, LOW);
}
void runMotors(void)
{
  pwmcounter = 0;
  digitalWrite(STNDBY1, HIGH);
  digitalWrite(STNDBY2, HIGH);
  Timer1.start();
}
// Handle interrupt every 10% duty cycle
// Initially, PWM pins are high
void MotorPwmHandler(void)
{
  // Evaluate counter
  if (pwmcounter > _pwmvalue)
  {
    PORTL &= ~0x0F; // digital low of pwm
  }
  pwmcounter++;

  // reset pwm counter on overflow
  if (pwmcounter >= 100)
  {
    pwmcounter = 0;
    PORTL |= 0x0F; // digital high of pwm
  }
}

void setMotorPWM(int pwm)
{
  _pwmvalue = pwm;
}

void run(Direction direction, uint16_t duration)
{
  setMotorDir(direction);
  runMotors();
  if (duration > 0)
  {
    delay(duration);
    stopMotors();
  }
}
//end of other functions for movement
// input output functions
void rgbshow()
{
  display.clearDisplay();
  display.setCursor(0,0);
  display.println("ENJOY THE LIGHT SHOW!");
  display.display();
  dio.clear();
  dio.setPixelColor(0, dio.Color(255, 0, 0)); // red
  dio.show();
  delay(500);
  dio.setPixelColor(0, dio.Color(0, 255, 0)); // green
  dio.show();
  delay(500);
  dio.setPixelColor(0, dio.Color(0, 0, 255)); // green
  dio.show();
  delay(500);
  dio.setPixelColor(0, dio.Color(255, 255, 255)); // white
  dio.show();
  delay(500);
} 

void cyclergb(){
  display.clearDisplay();
  display.setCursor(0,0);
  display.println("ROTATE THE ENCODER");
  display.display();
  dio.clear();      
  dio.setPixelColor(0, dio.Color(0, 0, 0)); //no light
  dio.show();
  Serial.println("RGB CYCLER");
  while (RGB_STATE==1){ //will execute while RGB_STATE is equal to 1
  lastStateCLK = currentStateCLK; // updates value of lastStateCLK
  currentStateCLK = digitalRead(CLK); // Read the current state of CLK
  if (currentStateCLK != lastStateCLK  && currentStateCLK == 0) {
    if (digitalRead(DT) != currentStateCLK) { // If the DT state is different than the CLK state then
    currentDir = "CW";
    Serial.print("Direction: ");
    Serial.println(currentDir);
      dio.clear();
      dio.setPixelColor(0, dio.Color(255, 0, 0)); //red
      dio.show();
      delay(500);
      dio.setPixelColor(0, dio.Color(0, 255, 0)); //green
      dio.show();
      delay(500);
      dio.setPixelColor(0, dio.Color(0, 0, 255)); //blue
      dio.show();
      delay(500);
      dio.setPixelColor(0, dio.Color(255, 255, 255)); //white
      dio.show();
      delay(500);
    }
    else { // Encoder is rotating CCW so decrement
    currentDir = "CCW";
    Serial.print("Direction: ");
    Serial.println(currentDir);
      dio.clear();
      dio.setPixelColor(0, dio.Color(255, 0, 0)); // red
      dio.show();
      delay(500);
      dio.setPixelColor(0, dio.Color(255, 255, 255)); // white
      dio.show();
      delay(500);
      dio.setPixelColor(0, dio.Color(0, 255, 0)); // green
      dio.show();
      delay(500);
      dio.setPixelColor(0, dio.Color(0, 0, 255)); // blue
      dio.show();
      delay(500);
    }
  }
  int btnState = digitalRead(SW); // Read the button state
  if (btnState == LOW) { //If we detect LOW signal, button is pressed
    if (millis() - lastButtonPress > 50) { 
      RGB_STATE=0; //this will disable the cyclergb function if button is pressed and will proceed to next func
    }
  }
}
}

void countdown(){
  display.clearDisplay();
  display.setCursor(0,0);  
  while (counter2!= 20) { //ensures the continuity of count
    if (counter3==0){ //displays first number in 0,0 coord
      counter2++;
      display.setCursor(0,counter3);
      display.print(counter2);
      display.display();
      counter3++; //y coordinate multiplier
    }
    else if(counter3==7){ // if last line was reached (counter3 being the multiplier for y coord)
      counter3=7;
      z=counter3*8;
      counter2++;
      display.setCursor(0,z);
      display.print(counter2);
      display.display();
      x=counter2; // x as copy of counter2
      c=counter3; // c as copy of counter 3
        while(c!=0){ // while c is not equal to 0 count backwards for the multiplier and number to be displayed
          if (x!=0){
          x--;
          c--;
          y=c*8;
          display.setCursor(0,y);
          display.print(x);
          display.display();
          }
        }
    }
    else{ // will start with 2 up to 8
      counter2++;
      z=counter3*8;
      display.setCursor(0,z);
      display.print(counter2);
      display.display();
      counter3++;
      x=counter2;
      c=counter3-1;
      while(c!=0){// reupdate of previous numbers printing upwards
        if (x!=0){
        x--;
        c--;
        y=c*8;
        display.setCursor(0,y);
        display.print(x);
        display.display();
        }
      }
    }
  display.clearDisplay();// clears the display in preparation for reprinting of previous numbers
  delay(250);
  }
counter2=0;// counter reset
counter3=0;
x=0;
y=0;
c=0;
z=0;
}

// calibration functions
void cal_linesensor(){
  //configure line sensors
  setupLineSensors();
  //calibrate line sensor
  display.clearDisplay();
  display.setCursor(0,0);
  display.print("FRONT L_SENSOR");
  display.display();
  calibrate_sensor(qtr_front);
  delay(2000);
  display.clearDisplay();
  display.setCursor(0,0);
  display.print("BACK L_SENSOR");
  display.display();
  calibrate_sensor(qtr_back);
  delay(2000);
  display.clearDisplay();
  display.setCursor(0,0);
  display.print("LEFT L_SENSOR");
  display.display();
  calibrate_sensor(qtr_left);
  delay(2000);
  display.clearDisplay();
  display.setCursor(0,0);
  display.print("RIGHT L_SENSOR");
  display.display();
  calibrate_sensor(qtr_right);
  delay(2000);
}
//functions needed for cal_linesensor
void calibrate_sensor(QTRSensors &lineSensor)
{
  Serial.println("Calibrating . . .");
  Serial.println("Slowly move the sensor across the electrical tape");
  // 2.5 ms RC read timeout (default) * 10 reads per calibrate() call
  // = ~25 ms per calibrate() call.
  // Call calibrate() 400 times to make calibration take about 10 seconds.
  for (uint16_t i = 0; i < 400; i++)
  {
    lineSensor.calibrate();
  }
  Serial.println();
  Serial.print("Min Values: ");
  display.setCursor(0,8);
  display.print("Min Values: ");
  display.display(); 
  // print the calibration minimum values measured when emitters were on
  for (uint8_t i = 0; i < SensorCount; i++)
  {
    Serial.print(lineSensor.calibrationOn.minimum[i]);
    Serial.print(' ');
    display.print(lineSensor.calibrationOn.minimum[i]);
    display.print(' ');
    display.display(); 
  }
  Serial.println();
  Serial.print("Max Values: ");
  display.setCursor(0,24);
  display.print("Max Values: ");
  display.display(); 
  // print the calibration maximum values measured when emitters were on
  for (uint8_t i = 0; i < SensorCount; i++)
  {
    Serial.print(lineSensor.calibrationOn.maximum[i]);
    Serial.print(' ');
    display.print(lineSensor.calibrationOn.maximum[i]);
    display.print(' ');
    display.display();
  }
  Serial.println();
  Serial.println("Done calibrating!");
  delay(1000);
  Serial.println();
}

void setupLineSensors()
{
  const uint16_t def_timeout = 1000;
  Serial.println("initializinf line sensors . . .");
  //front line sensor
  qtr_front.setTimeout(def_timeout);
  qtr_front.setTypeRC();
  qtr_front.setSensorPins((const uint8_t[]) {
    A0, A1, A2, A3
  }, SensorCount);
  qtr_front.setEmitterPin(33);
  //back line sensor
  qtr_back.setTimeout(def_timeout);
  qtr_back.setTypeRC();
  qtr_back.setSensorPins((const uint8_t[]) {
    A15, A14, A13, A12
  }, SensorCount);
  qtr_back.setEmitterPin(32);
  //left line sensor
  qtr_left.setTimeout(def_timeout);
  qtr_left.setTypeRC();
  qtr_left.setSensorPins((const uint8_t[]) {
    A7, A6, A5, A4
  }, SensorCount);
  qtr_left.setEmitterPin(30);
  //right line sensor
  qtr_right.setTimeout(def_timeout);
  qtr_right.setTypeRC();
  qtr_right.setSensorPins((const uint8_t[]) {
    A8, A9, A10, A11
  }, SensorCount);
  qtr_right.setEmitterPin(31);
}
// end of functions needed for cal_linsensor
void color_rec(){
  while(COL_SEN==1){
  dio.clear(); 
  dio.setPixelColor(0, dio.Color(255, 255, 255)); 
  dio.show(); 
  delay(DELAYVAL); 
  uint16_t r, g, b, c;
  while(!apds.colorDataReady()){
    delay(5);
  }
  apds.getColorData(&r, &g, &b, &c);
  if (r>=30||c<=16){
    display.clearDisplay();
    display.setCursor(0,0);
    display.println("COLOR RECOGNITION"); 
    display.setCursor(0,16);
    display.println("RED"); 
    display.display();
  }
  else if (b>=40||c<=3){
    display.clearDisplay();
    display.setCursor(0,0);
    display.println("COLOR RECOGNITION"); 
    display.setCursor(0,16);
    display.println("BLUE"); 
    display.display(); 
  }
  else{
    display.clearDisplay();
    display.setCursor(0,0);
    display.println("COLOR RECOGNITION"); 
    display.setCursor(0,16);
    display.println("NONE"); 
    display.display();  
  }
  int btnState = digitalRead(SW); // Read the button state
  if (btnState == LOW) { // If we detect LOW signal, button is pressed
    if (millis() - lastButtonPress > 50) {    
      COL_SEN=0; // disables the color_rec func if button is pressed
    }
  }
  }
}

void sensor_lifter(){
  display.clearDisplay();
  display.setCursor(0,0);
  display.println("COLOR SENSOR");
  display.print("WITH LIFTER"); 
  display.display();
  while(LIFTER_STATE==1){ //loop will execute while LIFTER_STATE is equal to 1
  dio.clear(); 
  dio.setPixelColor(0, dio.Color(255, 255, 255)); 
  dio.show(); 
  delay(DELAYVAL); 
  uint16_t r, g, b, c;
  while(!apds.colorDataReady()){
    delay(5);
  }
  apds.getColorData(&r, &g, &b, &c);
  if (r>=30||c<=16){
    display.clearDisplay();
    display.setCursor(0,0);
    display.println("COLOR SENSOR");
    display.print("WITH LIFTER");
    display.setCursor(0,24);
    display.println("RED"); 
    display.display();
    setServoAngle(100); 
  }
  else if (b>=40||c<=3){
    display.clearDisplay();
    display.setCursor(0,0);
    display.println("COLOR SENSOR");
    display.print("WITH LIFTER"); 
    display.setCursor(0,24);
    display.println("BLUE"); 
    display.display(); 
    setServoAngle(100);
  }
  else{
    display.clearDisplay();
    display.clearDisplay();
    display.setCursor(0,0);
    display.println("COLOR SENSOR");
    display.print("WITH LIFTER");
    display.setCursor(0,24);
    display.println("NONE"); 
    display.display();  
    setServoAngle(0);
  }
  int btnState = digitalRead(SW); // Read the button state
  if (btnState == LOW) { // If we detect LOW signal, button is pressed
    if (millis() - lastButtonPress > 50) {    
      LIFTER_STATE=0; // disables the sensor_lifter func if button is pressed
    }
  }
  }
}

void setServoAngle(int _angle) // function reference for servo angle 
{
  if (_angle < 0){
    _angle = 0;
  }
  else if (_angle > 270){
    _angle = 270;
  }
  int pulseWidth = map(_angle, 0, 270, COUNTER_WIDTH_MIN, COUNTER_WIDTH_MAX);
  Serial.println(pulseWidth);
  myservo.writeMicroseconds(pulseWidth);
}