#include <Servo.h>
Servo servox;
Servo servoy;
#define VERT_PIN A0
#define HORZ_PIN A1
const int originx=1450;
const int originy=1450;
int posx=originx;
int posy=originy;
double anglePrev=0;
double angle=0;
const int switch_pin=2;

void setup() {

Serial.begin(9600);
randomSeed(analogRead(0));
servox.attach(3);
servoy.attach(5);
pinMode(VERT_PIN, INPUT);
pinMode(HORZ_PIN, INPUT);
servox.writeMicroseconds(posx);
servoy.writeMicroseconds(posy);
pinMode(switch_pin, INPUT_PULLUP);
delay(1000);
}


void move(int tposx, int tposy, int changerate, int delayT){
char targetpos[70];
sprintf(targetpos, "TargetX: %03u - TargetY: %03u - CurrentX: %03u - CurrentY: %03u", tposx, tposy, posx, posy);
Serial.println(targetpos);
//const int changerate=10;
//const int delayT=2;
int changex;
int changey;
if(tposx>posx){changex=changerate;}
else if (tposx<posx){changex = -changerate;}
else if (tposx==posx){changex=0;}
if(tposy>=posy){changey=changerate;}
else if (tposy<posy){changey = -changerate;}
else if (tposy==posy){changey=0;}
char posstring[70];
for(int x=posx, y=posy; (x!=tposx+changex || y!=tposy+changey); x=x+changex, y=y+changey){ 
servox.writeMicroseconds(x);
servoy.writeMicroseconds(y); 
if(changex>0){if(x+changex>tposx){changex=tposx-x;}}
if(changex<0){if(x+changex<tposx){changex=tposx-x;}}
if(changey>0){if(y+changey>tposy){changey=tposy-y;}}
if(changey<0){if(y+changey<tposy){changey=tposy-y;}}
if(x==tposx){changex=0;}
if(y==tposy){changey=0;}
sprintf(posstring, "PosX: %03u - PosY: %03u - DeltaX: %+03i - DeltaY: %+03i" , x, y, changex, changey);
Serial.println(posstring);
delay(delayT);
} 
}


double mapf(double val, double in_min, double in_max, double out_min, double out_max) {
    return (val - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

double calculateRadius(double xcoord, double ycoord) {
    if (xcoord == 0 && ycoord == 0) {
        return 0.0f; // Define radius at the origin as 0
    } else {
        return sqrt(pow(xcoord, 2) + pow(ycoord, 2)); // Calculate radius
    }
}

double calculateAngle(double xcoord, double ycoord) {
    if (xcoord == 0 && ycoord == 0) return 0.0; // At the origin
    double angle = (xcoord != 0) ? atan(ycoord / xcoord) * 57.2957795131 : (ycoord > 0 ? 90.0 : 270.0); // Calculate angle or handle vertical case
    angle += (xcoord > 0) ? (ycoord < 0 ? 360 : 0) : (xcoord < 0 ? 180 : 0); // Adjust for quadrants
    if (ycoord == 0) angle = (xcoord > 0) ? 0.0 : 180.0; // Handle horizontal cases
    return (angle < 0) ? angle + 360 : (angle >= 360) ? angle - 360 : angle; // Normalize angle
}



void loop() {

if (digitalRead(switch_pin) == LOW){
  Serial.println("Free control mode ON");}

while(digitalRead(switch_pin) == LOW){
int joyx = map(analogRead(HORZ_PIN), 0, 1023, 2400, 500);
int joyy = map(analogRead(VERT_PIN),0, 1023, 500, 2400);
//char status[90];
//sprintf(status, "PosX: %03u - PosY: %03u - JoyX: %03u - JoyY: %03u", posx, posy, joyx, joyy);
//
//servox.writeMicroseconds(joyx);
//servoy.writeMicroseconds(joyy);
move(joyx,joyy,100,1);
posx=joyx;
posy=joyy;

delay(5);
}

if (digitalRead(switch_pin) == HIGH){Serial.println("Auto mode ON");}

while(digitalRead(switch_pin) == HIGH){


int tposx=random(500,2400);
int tposy=random(500,2400);

double xcoord=mapf(tposx, 500, 2400, -10, 10);
double ycoord=mapf(tposy, 500, 2400, -10, 10);
double radius=calculateRadius(xcoord,ycoord);

if (radius<8){continue;} //find a new target if radius is less than 8.

angle=calculateAngle(xcoord,ycoord);

double lowerb=anglePrev-240;
double upperb=anglePrev-180;

if (lowerb<0){lowerb+=360;}
if (upperb<0){upperb+=360;}

if (upperb>lowerb){
  if(angle<lowerb || angle>upperb){continue;} 
}
if (upperb<lowerb){
  if(angle<lowerb && angle>upperb){continue;} 
}//find a new target if angle is too close, within 120 degrees of opposite side is desired.


char xcoordStr[10];
char ycoordStr[10];
char radiusStr[10];
char angleStr[10];
char anglePrevStr[10];
dtostrf(xcoord, 6, 2, xcoordStr);
dtostrf(ycoord, 6, 2, ycoordStr);
dtostrf(radius, 6, 2, radiusStr);
dtostrf(angle, 6, 2, angleStr);
dtostrf(anglePrev, 6, 2, anglePrevStr);
char status[300]; 
sprintf(status, "TPosX: %03u - TPosY: %03u - XCoord: %s - YCoord: %s - Radius: %s - Angle: %s - Prev. Angle: %s", tposx, tposy, xcoordStr, ycoordStr, radiusStr, angleStr, anglePrevStr);
Serial.println(status);


move(tposx,tposy,10,2);
posx=tposx;
posy=tposy;
anglePrev=angle;
delay(100);
}
Serial.println("resetting coordinates to origin");
delay(1000);
move(originx,originy,10,2);
posx=originx;
posy=originy;
angle=0;
anglePrev=angle;
}