/*
 * FabMotorController Firmware v0.1
 * A Firmware to control through I2C the FabMotorController, 
 * a hardware based on the ATTINY1614 and the L298N driver.
 * 
 * Author: Harley Lara
 * Create: 01 May 2021
 * License: (CC BY-SA 4.0) Attribution-ShareAlike 4.0 International
 * 
 * This work may be reproduced, modified, distributed,
 * performed, and displayed for any purpose, but must
 * acknowledge this project. Copyright is retained and
 * must be preserved. The work is provided as is; no
 * warranty is provided, and users accept all liability.
 * 
 */

#include <Wire.h>

#define CONTROLLER_ADDR 0x04

/******* CONTROL PINs *******/
#define IN1 9   // OUTPUT 1 INPUT 1
#define IN2 8   // OUTPUT 1 INPUT 2
#define IN3 10  // OUTPUT 2 INPUT 1
#define IN4 2   // OUTPUT 2 INPUT 2
#define EN1 0   // 0 ENABLE 1 (PWM - SPEED CONTROL)
#define EN2 1   // 1 ENABLE 2 (PWM - SPEED CONTROL)

#define RESPONSE_SIZE 12
String response = "executing...";

void setup() {
  Wire.begin(CONTROLLER_ADDR);
  //Wire.onRequest(requestEvent); // TODO
  Wire.onReceive(receiveEvent);
  Serial.begin(115200);
  Serial.println("[INFO]: FAB MOTOR CONTROLLER - Starting ...");
  delay(20);

  // PINs Mode
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  pinMode(EN1, OUTPUT);
  pinMode(EN2, OUTPUT);
}

unsigned char output;
int speed;
int direction;

void receiveEvent(){

  /******* Read data from Master *******/
  while (Wire.available()){
    output = Wire.read();
    speed =  Wire.read();
    direction = Wire.read();
  }

  /******* OUTPUT 1 *******/
  if (output == 1){
    Serial.println("[DEGUB]: OUTPUT 1");
    // Forward direction
    if (direction == 1){
      // stop output
      if (speed == 0){
        //Serial.println("[DEGUB]: OUTPUT 1 STOP FREE");
        digitalWrite(IN1, LOW);
        digitalWrite(IN2, LOW);
      }
      // moving output
      else{
        //Serial.println("[DEGUB]: OUTPUT 1 FORWARD");
        analogWrite(EN1, speed);
        digitalWrite(IN1, HIGH);
        digitalWrite(IN2, LOW);
      }
    }
    // backward direction
    else if (direction == 0){
      //Serial.println("[DEGUB]: OUTPUT 1 STOP FREE");
      if (speed == 0){
        digitalWrite(IN1, LOW);
        digitalWrite(IN2, LOW);
      }
      else{
        //Serial.println("[DEGUB]: OUTPUT 1 BACKWARD");
        analogWrite(EN1, speed);
        digitalWrite(IN1, LOW);
        digitalWrite(IN2, HIGH); 
      }
    }
  }
  /******* OUTPUT 2 *******/
  else if (output == 2){
    Serial.println("[DEGUB]: OUTPUT 2");
    // Forward direction
    if (direction == 1){
      // stop output
      //Serial.println("[DEGUB]: OUTPUT 2 STOP FREE");
      if (speed == 0){
        digitalWrite(IN3, LOW);
        digitalWrite(IN4, LOW);
      }
      // moving output
      //Serial.println("[DEGUB]: OUTPUT 2 FORWARD");
      else{
        analogWrite(EN2, speed);
        digitalWrite(IN3, HIGH);
        digitalWrite(IN4, LOW);
      }
    }
    // backward direction
    else if (direction == 0){
      if (speed == 0){
        //Serial.println("[DEGUB]: OUTPUT 2 STOP FREE");
        digitalWrite(IN3, LOW);
        digitalWrite(IN4, LOW);
      }
      else{
        //Serial.println("[DEGUB]: OUTPUT 2 BACKWARD");
        analogWrite(EN2, speed);
        digitalWrite(IN3, LOW);
        digitalWrite(IN4, HIGH); 
      }
    }
  }
  
  /*
  // For degubbing
  Serial.println("[DEGUB]: Data received ...");
  Serial.print("[DEGUB]: Ouput=");
  Serial.println(output);
  Serial.print("[DEGUB]: Speed= ");
  Serial.println(speed);
  Serial.print("[DEGUB]: Direction=");
  Serial.println(direction);
  */
  
}

void loop() {
  delay(50);
}