import processing.serial.*;
import cc.arduino.*;
Arduino arduino;
int countstart = 0;
int servo1Pin = 9; // Control pin for servo motor
int servo2Pin = 10; // Control pin for servo motor
int servo3Pin = 11; // Control pin for servo motor
int armposn;// = 0;
int digitposn;// = 0;
//int penposn;//= 0;
int penstate = 0;

int xcent = 300;
int ycent = 300;
//int targetX= 400;
//int targetY= 200;
int line1 = 130;
int line2 = 130;
int minimumlength = 40;
color black = color(0);
PImage img;
PImage edgeImg ;

void setup(){
 float[][] kernel = { { -1, -1, -1 },
                     { -1,  9, -1 },
                     { -1, -1, -1 } };
  
size (600, 600);
background(255);

img = loadImage("james.JPG"); // Load the original image

img.loadPixels();
edgeImg = createImage(img.width, img.height, RGB);

// Loop through every pixel in the image.
for (int y = 1; y < img.height-1; y++) { // Skip top and bottom edges
  for (int x = 1; x < img.width-1; x++) { // Skip left and right edges
    float sum = 0; // Kernel sum for this pixel
    for (int ky = -1; ky <= 1; ky++) {
      for (int kx = -1; kx <= 1; kx++) {
        // Calculate the adjacent pixel for this kernel point
        int pos = (y + ky)*img.width + (x + kx);
        // Image is grayscale, red/green/blue are identical
        float val = red(img.pixels[pos]);
        // Multiply adjacent pixels based on the kernel values
        sum += kernel[ky+1][kx+1] * val;
      }
    }
    // For this pixel in the new image, set the gray value
    // based on the sum from the kernel
    edgeImg.pixels[y*img.width + x] = color(sum);
  }
}
// State that there are changes to edgeImg.pixels[]
edgeImg.updatePixels();



arduino = new Arduino(this, Arduino.list()[0]);
arduino.pinMode(servo1Pin, Arduino.OUTPUT);
arduino.pinMode(servo2Pin, Arduino.OUTPUT);
arduino.pinMode(servo3Pin, Arduino.OUTPUT);

arduino.analogWrite(servo1Pin, 70);
arduino.analogWrite(servo2Pin, 170); 
arduino.analogWrite(servo3Pin, 50); // the servo moves to the horizontal location of the mouse

// note - we are setting a digital pin to output
background(255);

}

void draw()
{
if (countstart == 1){
makepic();
arduino.analogWrite(servo1Pin, 70);
arduino.analogWrite(servo2Pin, 170); 
arduino.analogWrite(servo3Pin, 50);
}
countstart = countstart +1;
}

//void draw(){
void makepic(){
  background(255);
    //fill(50);
  stroke(0);
 //rect(350+30,320-10,130,130);
  image(img, 0, 0, 130, 130); // Displays the image from point (0,0) 

image(edgeImg, 130, 0, 130, 130); // Draw the new image
filter(POSTERIZE,4);
filter(THRESHOLD);
colorMode(HSB);


for (int xcycle = 130; xcycle < 259; xcycle = xcycle+2)
{
  for (int ycycle = 0; ycycle < 129; ycycle++)
    { 
      if (get(xcycle,ycycle)<-2)
        {
        set(xcycle+350-130+30,ycycle+320-10,black);
        markpaper(xcycle+350-130+30,ycycle+320-10,140);
        //delay(200);
        }
      else
        {
         set(xcycle,ycycle,0);
         markpaper(xcycle+350-130+30,ycycle+320-10,170);
         //delay(50);  
      }
    }
  for (int ycycle = 129; ycycle > 0; ycycle--)
    {
      if (get(xcycle+1,ycycle)<-2)
        {
        set(xcycle+350-130+1+30,ycycle+320-10,black);
        markpaper(xcycle+350-130+1+30,ycycle+320-10,140);
        //delay(200);
        }
      else
        {
        set(xcycle+1,ycycle,0);
        markpaper(xcycle+350-130+1+30,ycycle+320-10,170);
        //delay(50);
        }
    }
}

}

void markpaper(int targetX, int targetY, int penposn){
  //print("xposn" + targetX);
  //println(" yposn" + targetY);
  float angle1 =  atan2((targetX - xcent), (targetY - ycent));
  float sectorlength = sqrt(sq(targetX - xcent)+sq(targetY - ycent));

//if it's out of reach, shorten the length in the same direction  
if (sectorlength > (line1+line2))
  {
      sectorlength = line1+line2;
      targetX = int(xcent + sin(angle1)*sectorlength);
      targetY = int(ycent + cos(angle1)*sectorlength);
   }

if (sectorlength < minimumlength)
  {
      sectorlength = minimumlength;
      targetX = int(xcent + sin(angle1)*sectorlength);
      targetY = int(ycent + cos(angle1)*sectorlength);
  }
   
float internangle = acos((sq(sectorlength)-sq(line2)-sq(line1))/(2*line1*line2));

float sendangle1st = (angle1+(internangle/2));
if (degrees(sendangle1st) < 0 )
  {
    sendangle1st = radians(0);
  }
if (degrees(sendangle1st) > 180 )
  {
    sendangle1st = radians(180);
  }
  
int line1X = int(xcent+ sin(sendangle1st)*line1);
int line1Y = int(ycent+ cos(sendangle1st)*line1);
int line2X = line1X + int(sin(((angle1+(internangle/2) - internangle)))*line2);
int line2Y = line1Y + int(cos(((angle1+(internangle/2) - internangle)))*line2);
stroke(0,9);
line(xcent, ycent, line1X, line1Y);
line(line1X, line1Y, line2X-1, line2Y-1);
   // if (penposn == 150)
   // {
   //   set(line2X,line2Y,black);
     // println("TEXT");
   // }
       //   set(mouseX,mouseY,black);

//if it's too close set the minimum threshold in the same direction

  
int sendangle1 = round(degrees(sendangle1st));
int sendangle2 = round(degrees((radians(180) - internangle)-radians(35)));
//println("Send1>"+ sendangle1 + " Send2 >" + sendangle2);
digitposn = sendangle2;
armposn = sendangle1;

arduino.analogWrite(servo1Pin, digitposn);
arduino.analogWrite(servo2Pin, penposn); 
arduino.analogWrite(servo3Pin, armposn); 
if (penstate != penposn)
{
  delay(300);
}
else
{
delay(1);
}// the servo moves to the horizontal location of the mouse
penstate = penposn;
}