#include "SPI.h"

#include <SoftwareSerial.h>

SoftwareSerial mySerial(6, 7); // RX, TX


#define PIN_SS        10
#define PIN_MISO      12
#define PIN_MOSI      11
#define PIN_SCK       13

#define PIN_MOUSECAM_RESET     9
#define PIN_MOUSECAM_CS        10

#define ADNS3080_PIXELS_X                 30
#define ADNS3080_PIXELS_Y                 30

#define ADNS3080_PRODUCT_ID            0x00
#define ADNS3080_REVISION_ID           0x01
#define ADNS3080_MOTION                0x02
#define ADNS3080_DELTA_X               0x03
#define ADNS3080_DELTA_Y               0x04
#define ADNS3080_SQUAL                 0x05
#define ADNS3080_PIXEL_SUM             0x06
#define ADNS3080_MAXIMUM_PIXEL         0x07
#define ADNS3080_CONFIGURATION_BITS    0x0a
#define ADNS3080_EXTENDED_CONFIG       0x0b
#define ADNS3080_DATA_OUT_LOWER        0x0c
#define ADNS3080_DATA_OUT_UPPER        0x0d
#define ADNS3080_SHUTTER_LOWER         0x0e
#define ADNS3080_SHUTTER_UPPER         0x0f
#define ADNS3080_FRAME_PERIOD_LOWER    0x10
#define ADNS3080_FRAME_PERIOD_UPPER    0x11
#define ADNS3080_MOTION_CLEAR          0x12
#define ADNS3080_FRAME_CAPTURE         0x13
#define ADNS3080_SROM_ENABLE           0x14
#define ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER      0x19
#define ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER      0x1a
#define ADNS3080_FRAME_PERIOD_MIN_BOUND_LOWER      0x1b
#define ADNS3080_FRAME_PERIOD_MIN_BOUND_UPPER      0x1c
#define ADNS3080_SHUTTER_MAX_BOUND_LOWER           0x1e
#define ADNS3080_SHUTTER_MAX_BOUND_UPPER           0x1e
#define ADNS3080_SROM_ID               0x1f
#define ADNS3080_OBSERVATION           0x3d
#define ADNS3080_INVERSE_PRODUCT_ID    0x3f
#define ADNS3080_PIXEL_BURST           0x40
#define ADNS3080_MOTION_BURST          0x50
#define ADNS3080_SROM_LOAD             0x60

#define ADNS3080_PRODUCT_ID_VAL        0x17

void mousecam_reset()
{
  digitalWrite(PIN_MOUSECAM_RESET,HIGH);
  delay(1); // reset pulse >10us
  digitalWrite(PIN_MOUSECAM_RESET,LOW);
  delay(35); // 35ms from reset to functional
}

int mousecam_init()
{
  pinMode(PIN_MOUSECAM_RESET,OUTPUT);
  pinMode(PIN_MOUSECAM_CS,OUTPUT);
  
  digitalWrite(PIN_MOUSECAM_CS,HIGH);
  
  mousecam_reset();
  
  int pid = mousecam_read_reg(ADNS3080_PRODUCT_ID);
  if(pid != ADNS3080_PRODUCT_ID_VAL)
    return -1;

  // turn on sensitive mode
  mousecam_write_reg(ADNS3080_CONFIGURATION_BITS, 0x19);

  return 0;
}

void mousecam_write_reg(int reg, int val)
{
  digitalWrite(PIN_MOUSECAM_CS, LOW);
  SPI.transfer(reg | 0x80);
  SPI.transfer(val);
  digitalWrite(PIN_MOUSECAM_CS,HIGH);
  delayMicroseconds(50);
}

int mousecam_read_reg(int reg)
{
  digitalWrite(PIN_MOUSECAM_CS, LOW);
  SPI.transfer(reg);
  delayMicroseconds(75);
  int ret = SPI.transfer(0xff);
  digitalWrite(PIN_MOUSECAM_CS,HIGH); 
  delayMicroseconds(1);
  return ret;
}

struct MD
{
 byte motion;
 char dx, dy;
 byte squal;
 word shutter;
 byte max_pix;
};

void mousecam_read_motion(struct MD *p)
{
  digitalWrite(PIN_MOUSECAM_CS, LOW);
  SPI.transfer(ADNS3080_MOTION_BURST);
  delayMicroseconds(75);
  p->motion =  SPI.transfer(0xff);
  p->dx =  SPI.transfer(0xff);
  p->dy =  SPI.transfer(0xff);
  p->squal =  SPI.transfer(0xff);
  p->shutter =  SPI.transfer(0xff)<<8;
  p->shutter |=  SPI.transfer(0xff);
  p->max_pix =  SPI.transfer(0xff);
  digitalWrite(PIN_MOUSECAM_CS,HIGH); 
  delayMicroseconds(5);
}

// pdata must point to an array of size ADNS3080_PIXELS_X x ADNS3080_PIXELS_Y
// you must call mousecam_reset() after this if you want to go back to normal operation
int mousecam_frame_capture(byte *pdata)
{
  mousecam_write_reg(ADNS3080_FRAME_CAPTURE,0x83);
  
  digitalWrite(PIN_MOUSECAM_CS, LOW);
  
  SPI.transfer(ADNS3080_PIXEL_BURST);
  delayMicroseconds(50);
  
  int pix;
  byte started = 0;
  int count;
  int timeout = 0;
  int ret = 0;
  for(count = 0; count < ADNS3080_PIXELS_X * ADNS3080_PIXELS_Y; )
  {
    pix = SPI.transfer(0xff);
    delayMicroseconds(10);
    if(started==0)
    {
      if(pix&0x40)
        started = 1;
      else
      {
        timeout++;
        if(timeout==100)
        {
          ret = -1;
          break;
        }
      }
    }
    if(started==1)
    {
      pdata[count++] = (pix & 0x3f)<<2; // scale to normal grayscale byte range
    }
  }

  digitalWrite(PIN_MOUSECAM_CS,HIGH); 
  delayMicroseconds(14);
  
  return ret;
}

void setup() 
{
  pinMode(PIN_SS,OUTPUT);
  pinMode(PIN_MISO,INPUT);
  pinMode(PIN_MOSI,OUTPUT);
  pinMode(PIN_SCK,OUTPUT);
  
  SPI.begin();
  SPI.setClockDivider(SPI_CLOCK_DIV32);
  SPI.setDataMode(SPI_MODE3);
  SPI.setBitOrder(MSBFIRST);
  
  Serial.begin(115200);

  if(mousecam_init()==-1)
  {
    Serial.println("Mouse cam failed to init");
    while(1);
  }
}

char asciiart(int k)
{
  static char foo[] = "WX86*3I>!;~:,`. ";
  return foo[k>>4];


  Serial.begin(9600);
}

byte frame[ADNS3080_PIXELS_X * ADNS3080_PIXELS_Y];

void loop() 
{
  //#if 1
  
  // if enabled this section grabs frames and outputs them as ascii art
  
  if(mousecam_frame_capture(frame)==0)
  {
    int i,j,k;
    Serial.print("S");
    Serial.println();
    for(i=0, k=0; i<ADNS3080_PIXELS_Y; i++) 
    {
      for(j=0; j<ADNS3080_PIXELS_X; j++, k++) 
      {
        Serial.print(frame[k]);//asciiart(frame[k]));
        Serial.print('-');
      }
      Serial.println();
    }
  }
  //Serial.print("E");
  //Serial.println();
  ////Serial.println();
  delay(100);
  
  
  //#else
  
  // if enabled this section produces a bar graph of the surface quality that can be used to focus the camera
  // also drawn is the average pixel value 0-63 and the shutter speed and the motion dx,dy.
  
  //int val = mousecam_read_reg(ADNS3080_PIXEL_SUM);
  //MD md;
  
  //mousecam_read_motion(&md);
  /*
  for(int i=0; i<md.squal/4; i++)
    Serial.print('*');
  Serial.print(' ');
  Serial.print((val*100)/351);
  Serial.print(' ');
  */
  //Serial.print(md.shutter); Serial.print(" (");
  //Serial.print("I");
  
  /*int X = (int)md.dx;
  int Y = (int)md.dy;
  if(X > -1) Serial.print("+");
  if(X < 0) Serial.print("-");
  if(abs(X) < 10) Serial.print("0");
  Serial.print(abs(X));
  Serial.print(',');
  if(Y > -1) Serial.print("+");
  if(Y < 0) Serial.print("-");
  if(abs(Y) < 10) Serial.print("0");
  Serial.println(abs(Y));
  /*
  Serial.print((int)md.dx); Serial.print(',');
  Serial.println((int)md.dy); //Serial.println(')');
  // Serial.println(md.max_pix);
  */
  //delay(20);
  //*/
  //#endif
}

import processing.javafx.*;

import processing.serial.*;

Serial myPort;
String myRawString = "";


int[][] image = new int[30][30];
int row = 0;
int col = 0;

int driftX = 0;
int driftY = 0;

void setup() {
  size(300, 330, FX2D);
  pixelDensity(2);
  myPort = new Serial(this, "COM4", 115200);//这里写自己arduino nano的串口号
}

void draw() {
  
  fill(255, 0, 0);
  rect(0, 0, 300, 30);

  fill(255, 255, 0);
  textSize(15);
  text("Ling & Thomas", 10, 20); 
  

  while (myPort.available() > 0) {
    myRawString = myPort.readStringUntil('\n');
    if (myRawString != null) {
      
      if(myRawString.charAt(0) == 'S' | myRawString.charAt(0) == 'E'){
        row = 0;
        col = 0;
      }
      if(myRawString.charAt(0) == 'I'){
        String strX = myRawString.substring(1, 4);
        String strY = myRawString.substring(5, 8);
      
        driftX = parseInt(strX)*-1;
        driftY = parseInt(strY);
      
        //println("X: "+ driftX);
        //println("Y: "+ driftY);
        
        fill(255, 255, 0);
        textSize(15);
        text(driftX, 210, 20);
        text(driftY, 280, 20);
      }
      else{
        int[] line = int(split(myRawString, "-"));
        if(line.length > 30 && row < 30){
          for (int i = 0; i < 30; i = i+1) {
            image[i][row] = line[i];
            
            stroke(image[i][row]);
            fill(image[i][row]);
            rect(i*10, row*10+30, 10, 10);
          }
          
          row++;
        }
      }

    }
  }

}