#include <Stepper.h>
#include <WiFi.h>
#include <ThingSpeak.h>

char ssid[] = "Wokwi-GUEST";
char pass[] = "";
WiFiClient client;
unsigned long myChannelNumber = 2069367;
const char * myWriteAPIKey = "K109889A4DO58V4L";

// ADC2 cannot work between esp_wifi_start() and esp_wifi_stop()... only ADC1 can 
//LOWER BODY HORIZONDAL 
int LBMotorHoriPin = 33, LBMotorHoriValue; // 33 is ADC1
int LBMotorStepPin = 14;
int LBMotorDirectionPin = 27;
int LBMotorSteps=10, LBMotorDegree=0, LBMotorMaxDegree=30;//30 is max
Stepper LBMotor(LBMotorSteps, LBMotorStepPin, LBMotorDirectionPin);

//UPPER BODY VERTICAL 
int UBMotorVertPin = 32, UBMotorVertValue; // 32 is ADC1
int UBMotorStepPin = 13;
int UBMotorDirectionPin = 12;
int UBMotorSteps=5, UBMotorDegree=0, UBMotorMaxDegree=10;//15 is max
Stepper UBMotor(UBMotorSteps, UBMotorStepPin, UBMotorDirectionPin);

int baseMotorHoriPin = 34, baseMotorHoriValue; // 34 is ADC1
int baseMotorStepPin = 2;
int baseMotoDirectionPin = 4;
int baseMotorSteps=15, baseMotorDegree=0, baseMotorMaxDegree=90;
Stepper baseMotor(baseMotorSteps, baseMotorStepPin, baseMotoDirectionPin);


void setup() {  
  Serial.begin(115200); 

  WiFi.mode(WIFI_STA);
  ThingSpeak.begin(client);

  LBMotor.setSpeed(100);
  UBMotor.setSpeed(100);
  baseMotor.setSpeed(100);
  
  pinMode(LBMotorHoriPin, INPUT);
  pinMode(LBMotorStepPin, OUTPUT);
  pinMode(LBMotorDirectionPin, OUTPUT);

  pinMode(UBMotorVertPin, INPUT);
  pinMode(UBMotorStepPin, OUTPUT);
  pinMode(UBMotorDirectionPin, OUTPUT);

  pinMode(baseMotorHoriPin, INPUT);
  pinMode(baseMotorStepPin, OUTPUT);
  pinMode(baseMotoDirectionPin, OUTPUT);

}

void loop() {
/*
vertical -> 0 down 4095 up
horizondal -> 0 right 4095 left
*/

 connectToCloud();

 LBMotorHoriValue = map(analogRead(LBMotorHoriPin), 0, 4095, 1, -1);// right=1, left=-1, idle =0
 LBMotorDegree = rotateMotor(LBMotor,LBMotorHoriValue, LBMotorSteps, LBMotorDegree, LBMotorMaxDegree);
 Serial.print(LBMotorDegree);
 Serial.print(",");

 UBMotorVertValue = map(analogRead(UBMotorVertPin), 0, 4095, -1, 1);// top=-1, down=1, idle =0
 UBMotorDegree =rotateMotor(UBMotor,UBMotorVertValue, UBMotorSteps, UBMotorDegree,UBMotorMaxDegree);
 Serial.print(UBMotorDegree);
 Serial.print(",");


 baseMotorHoriValue  = map(analogRead(baseMotorHoriPin), 0, 4095, 1, -1);// top=-1, down=1, idle =0
 baseMotorDegree = rotateMotor(baseMotor, baseMotorHoriValue, baseMotorSteps, baseMotorDegree, baseMotorMaxDegree);
 Serial.print(baseMotorDegree);
 Serial.println(".");

 writeData();

}

int rotateMotor(Stepper motor, int pinValue, int steps, int degree, int maxDegree){
  if(pinValue == 1) {
    motor.step(steps); 
    if(degree < maxDegree) 
      degree += steps;
  }
  else if(pinValue == -1) {
    motor.step(-steps);
    if(degree > 0) 
      degree -= steps;  
  }
  return degree;
}

void connectToCloud(){
  if(WiFi.status() != WL_CONNECTED) {
  Serial.print("Attempting to connect");
  while(WiFi.status() != WL_CONNECTED) {
    WiFi.begin(ssid, pass);
    for(int i=0;i<5;i++) {
      Serial.print(".");
      delay(1000);
    }
  }
  Serial.println("\nConnected.");
  }
}
int statusCode;
void writeData(){
  ThingSpeak.setField(1, baseMotorDegree);
  ThingSpeak.setField(2, LBMotorDegree);
  ThingSpeak.setField(3, UBMotorDegree);
  statusCode = ThingSpeak.writeFields(myChannelNumber,myWriteAPIKey);
  if(statusCode == 200) //successful writing code
    Serial.println("Channel update successful.");
  else
    Serial.println("Problem Writing data. HTTP error code :" +  String(statusCode));
  delay(1000); // data to be uploaded every 15secs
}