//--------------------------------
//
// Telecranduino
//
// Nico - June-2011
//
//--------------------------------



//#define NUNCHUK
#ifdef NUNCHUK
#include <Wire.h>
#include "nunchuck_funcs.h"
#endif

//#define DBG

//---------------- Buttons
int buttonPin[] = {10,11};

//---------------- Motors
int dirPin[] = {2,4};
int stepperPin[sizeof(dirPin)/sizeof(int)] = {3,5};
int saved_dir[sizeof(dirPin)/sizeof(int)];
int backlash[sizeof(dirPin)/sizeof(int)] = {14,14}; // in steps
boolean orientation[sizeof(dirPin)/sizeof(int)] = {false,true}; // X, Y axis
int position[sizeof(dirPin)/sizeof(int)] = {0,0};
int res = 1; //step resolution

void init_motors()
{
  int i;
  for(i=0; i<sizeof(dirPin)/sizeof(int);i++)
    {
    pinMode(dirPin[i], OUTPUT);
    pinMode(stepperPin[i], OUTPUT);
    saved_dir[i] = LOW; // initial position
    digitalWrite(dirPin[i],saved_dir[i]);
    }
  delay(100);
}

#define MICROSEC 800

void move(int mot, int stepS)
{
  position[mot]+=stepS; // update global position

  if (!orientation[mot]) 
    stepS = -stepS;
  int steps = stepS;
  boolean dir = (steps < 0) ? HIGH : LOW;

  steps = abs(steps);
  if (dir != saved_dir[mot]) // add backlash if direction has changed
    {
    digitalWrite(dirPin[mot],dir);
    saved_dir[mot]=dir;
    delayMicroseconds(MICROSEC);
    steps+=backlash[mot];
    }

  for(int i=0;i<steps;i++) // actual steps
    {
    digitalWrite(stepperPin[mot], LOW);
    delayMicroseconds(MICROSEC);
    digitalWrite(stepperPin[mot], HIGH);
    delayMicroseconds(MICROSEC);
    }

}

//---------------- end Motors

int sign(int val)
{
  return (val < 0) ? -1 : 1;
}

void trace(int X1, int Y1, int X2, int Y2)
{
#ifdef DBG
  char buf[64];
  sprintf(buf, "trace x1 %d x2 %d y1 %d y2 %d", X1, X2, Y1, Y2); Serial.println(buf);
#endif

  int nbx = (X2-X1)/res;
  int nby = (Y2-Y1)/res;

  int num, den, mot0, mot1;
  if (abs(nbx) >= abs(nby))
    {
    num = nbx;
    den = nby;
    mot0 = 0; // x
    mot1 = 1; // y
    }
  else
    {
    num = nby;
    den = nbx;
    mot0 = 1;
    mot1 = 0;
    }
  
  if (!den)
    {
    move(mot0, num * res);
    return;
    }
    
  int len = abs(num / den);
  if (!len)
    {
    move(mot1, den * res);
    return;
    }
  int i;
  for(i=0; i<=abs(den); i++)
    {
    move(mot0, len * res * sign(num));
    move(mot1, res * sign(den));
    }
}

void trace(int X, int Y) // trace a segment
{
  trace(0,0,X,Y);
}

void moveTo(int X, int Y) // move to a coordinate
{
  trace(position[0], position[1], X, Y);
}

void calibrate_backlash(int minb, int maxb, int stepb, int unitb)
{
  int save[2] = { backlash[0], backlash[1] };
  int i;
  for(i=minb; i<maxb; i+=stepb)
    {
    // set backlash
    backlash[0]=i;
    backlash[1]=i;

    // backlash test pattern
    move(0,-unitb);
    move(0,unitb);
    move(1,unitb);
    move(1,-unitb);
    move(0,unitb);
    move(0,-unitb);
    move(1,-unitb);
    move(1,unitb);

    // move away
    backlash[0] = save[0];
    backlash[1] = save[1];    
    trace(1.5*unitb,1.5*unitb);
    }
}


void setup() {
  init_motors(); // Motors

#ifdef NUNCHUK
  nunchuck_setpowerpins();
  nunchuck_init(); // send the initilization handshake
#endif

  Serial.begin(9600);
}


int loop_cnt = 0;

void checker(int a, int b, int unit)
{
  int i, j;
  for (i=0;i<a;i++)
    {
    for (j=0;j<b;j++)
      {
      trace(unit, 0);
      trace(0, unit );
      trace(0, -unit );
      }
    trace(-b*unit, 0);
    trace(0, unit);
    }
  trace(b*unit, 0);
  trace(-b*unit, 0);
  trace(0, -a*unit);
}

void
maison(int taille)
{
    trace(2*taille,0);
    trace(0,2*taille);
    trace(-taille,taille);
    trace(-taille,-taille);
    trace(0,-2*taille);
    trace(2*taille,0);
}

void loop(){

#ifndef NUNCHUK
  
  int button1 = digitalRead(buttonPin[0]);
  int button2 = digitalRead(buttonPin[1]);

  if (button1)
    {

  int mode = 0; // change mode at compile time

  if (mode == 0) // backlash calibration
    {
    calibrate_backlash(10, 30, 1, 40);
    }
  if (mode == 1) // 2D math function experiment
    {
    double x, y;
    for(x=0;x<35;x+=0.1)
      {
      if (x==0) 
        y = 4 + 8;
      else
        y = 4 + 8*sin(0.5*x)/(0.5*x);
      moveTo(x*50,y*100);
      }
    }
  else if (mode == 2) // 3D math function experiment
    {
    double x, y, z;
    double minx = 0, maxx = 0;
    double miny = 0, maxy = 0;
    for(y=-5;y<5;y+=0.5)
      for(x=-5;x<5;x+=0.5)
        {
        double X = sqrt(0.5*(x*x+y*y));
        z = sin(X)/X;
        //z = sin(x);
        double theta = 0.3 * 3.14159;
        double xx = x + y * cos(theta);
        double yy = z + y * sin(theta);
        if (xx < minx) minx = xx;
        if (xx > maxx) maxx = xx;
        if (yy < miny) miny = yy;
        if (yy > maxy) maxy = yy;
      }
    for(y=-5;y<5;y+=0.5)
      {
      int index;
      for(index = 0; index < 2; index++)
        {
        for(x=(!index?-5:5);(!index?x<5:x>-5);x+=(!index?0.1:-0.1))
          {
          double X = sqrt(0.5*(x*x+y*y));
          z = sin(X)/X;
          //z = sin(x);
          double theta = 0.3 * 3.14159;
          double xx = x + y * cos(theta);
          double yy = z + y * sin(theta);
          int xxx = map(xx*1000, minx*1000, maxx*1000, 0, 1000);
          int yyy = map(yy*1000, miny*1000, maxy*1000, 0, 1000);
          moveTo(xxx,yyy);
          }
        }
      }  
    moveTo(0,0);  // move back to 0,0
    }
  else if (mode == 3) // checker board
    {
    checker(3,4,200);
    }
  else if (mode == 4) // little houses
    {
    int taille;
    for(taille=1; taille < 60; taille += 2)
      maison(taille);
    }

  moveTo(position[0],0); // move back to 0,0
  moveTo(0,0);  // move back to 0,0

  move(0, 0); // get the motor back into the initial position (take into account backlash)
  move(1, 0); // get the motor back into the initial position (take into account backlash)

  delay(1000); // debounce
  return;
    }


#else NUNCHUK
    backlash[0]=0; // no backlash correction for nunchuck mode
    backlash[1]=0;

    if( loop_cnt > 10 ) { // every 100 msecs get new data
        loop_cnt = 0;
        nunchuck_get_data();

        byte joyx = nunchuck_joyx();
        byte joyy = nunchuck_joyy(); 
        byte zbut = nunchuck_zbutton();
        byte cbut = nunchuck_cbutton(); 

        if (zbut || cbut)
          {
          int nun_minx = 27;
          int nun_zerox = 124;
          int nun_maxx = 220;
          int nun_miny = 25;
          int nun_zeroy = 137;
          int nun_maxy = 215;

        int val1 = 0, val2 = 0;

        if (joyy > nun_zeroy)
          val1 = map (joyy, nun_zeroy, nun_maxy, 0, 1);
        else
          val1 = -map (joyy, nun_zeroy, nun_miny, 0, 0);

        if (joyx > nun_zeroy)
          val2 = map (joyx, nun_zerox, nun_maxx, 0, 1);
        else
          val2 = -map (joyx, nun_zerox, nun_minx, 0, 1);

      val1 = -val1;

      char buf[63];


      int nres = zbut ? res*5 : res*1;
      trace(val1*nres, val2*nres, 1);  

#ifdef NUNCHUCK_CALIBRATION
      Serial.print("\tjoyy: "); Serial.print((byte)joyy,DEC);
      Serial.print("\tjoyx: "); Serial.print((byte)joyx,DEC);
      sprintf(buf, " val1: %d, val2: %d", val1, val2); Serial.print(buf);
      sprintf(buf, " X: %d, X: %d", position[0], position[1]); Serial.print(buf);
      Serial.print("\n");  
#endif
      }
    }

    loop_cnt++;
    delay(1);
#endif

}