#include <Servo.h>
#include "serial.h"
// Arduino Code for 2012 competition 

#define CLAW 3
#define ARM 5
#define LEFTDOOR 7
#define RIGHTDOOR 6
#define LEFTMOTOR 10
#define RIGHTMOTOR 11

#define BUTTONPIN A1
#define IRPIN A3
#define LEDPIN 13

#define CLAWOPEN 80
#define CLAWCLOSED 167
#define CLAWSTOP 127

#define ARMDOWN 99      
#define ARMDOWNINIT 50
#define ARMUP 155
#define ARMSTOP 127

#define LEFTOPEN 0
#define LEFTCLOSED 90
#define RIGHTOPEN 255
#define RIGHTCLOSED 120

#define IRTHRESH 20
#define BUTTONTHRESH 10

int foo = 1;

void arduinoInit();
void set_servo_position(int s, unsigned char position);
int get_sw1();  
void led_on();
void led_off();
int analog(int num);


void openClaw();
void closeClaw();
void liftCans();
void grabFirst();
void lowerClaw();
void raiseClaw();
void openWings();
void closeWings();
void openClawAll();
void setSpeed(char motor, int speed);


int can = 0;
int button = 0;





//Servos
Servo servos[15];

int main()

{
  arduinoInit();
  int i;
  int x = 0;
  char y = 0;
  can = 0;
  button = 0;
  openWings();
  set_servo_position(ARM, ARMSTOP);
  set_servo_position(CLAW, CLAWSTOP);
  delay(1000);
  lowerClaw();
  delay(50);
  raiseClaw();
  openClawAll();        

  while(true)   //read serial if its there and check button and switch
  {     
    if(get_sw1() == 1 || button == 1)
    {
      button = 1;       
      led_on();         
    }
    else
    {
      led_off();
    }
    if(analog(IRPIN) <= IRTHRESH)       //TODO read analog
    {
      can = 1;
    }
    if(Serial.available() > 0)
    {
       execute_function();
    }
  }
}



void arduinoInit()

{       
  init();
  servos[LEFTMOTOR].attach(LEFTMOTOR, 1000, 2000);
  servos[RIGHTMOTOR].attach(RIGHTMOTOR, 1000, 2000);
  servos[CLAW].attach(CLAW, 1000, 2000);
  servos[ARM].attach(ARM, 1000, 2000);
  servos[LEFTDOOR].attach(LEFTDOOR, 1000, 2000);
  servos[RIGHTDOOR].attach(RIGHTDOOR, 1000, 2000);

  pinMode(LEDPIN, OUTPUT);      //set LED pin to output

  Serial.begin(BAUD);
}



void set_servo_position(int s, unsigned char position)

{
  int towrite = position*(12.0/17.0);
  printf("Writing %d to motor %d\n", towrite, position);
  Serial.print("Writing ");
  Serial.print((int)position);
  Serial.print("to motor ");
  Serial.println(s);
  servos[s].write(towrite);
}



int get_sw1()

{
  int sum =  analogRead(BUTTONPIN);
  
  if(sum > 0)
    return 0;
    
  button = 1;
  return 1;
}

void led_on()
{
  digitalWrite(LEDPIN, HIGH);
}


void led_off()
{
  digitalWrite(LEDPIN, LOW);
}


int analog(int num)
{
  int x = 0;
  int sum = 0;

  for(x = 0; x < 100; x++)
  {
    sum += analogRead(num);
  }
  return sum; 
}


void setSpeed(char motor, int speed)
{ //175 to 255 and 100 to 0

  int x;

  if(speed >= 0)
    set_servo_position(motor, x = 100-speed);
    
  if(speed <  0)
    set_servo_position(motor, x = (175 - speed*.8));
}





//Closes claw //DONE

void closeClaw()
{
  set_servo_position(CLAW, CLAWCLOSED);
  delay(400); //TODO FIND THIS VALUE
  set_servo_position(CLAW, CLAWSTOP);
}

//DONE

void openClaw()
{
  set_servo_position(CLAW, CLAWOPEN);
  delay(700); //TODO FIND THIS VALUE
  set_servo_position(CLAW, CLAWSTOP);
}

void openClawAll()
{
  set_servo_position(CLAW, CLAWOPEN);
  delay(700); //TODO FIND THIS VALUE
  set_servo_position(CLAW, CLAWSTOP);
}

void lowerClaw()
{
  int x = 0;
  set_servo_position(ARM, ARMDOWNINIT);
  delay(300);
  set_servo_position(ARM, ARMDOWN);
  delay(700);
  set_servo_position(ARM, ARMSTOP);
}



void raiseClaw()
{
  set_servo_position(ARM, ARMUP);
  delay(800); //TODO find value to raise
  set_servo_position(ARM, ARMSTOP);
}



//lower arm, grab cans, lift arm
//For lifting every can but first
void liftCans()  
{
  set_servo_position(ARM, 84);
  delay(300);
  set_servo_position(ARM, ARMSTOP);
  openClaw();
  lowerClaw();
  closeClaw();
  raiseClaw();
  can = 0;
  Serial.write(LIFT_CANS);
}



//GRAB_FIRST

void grabFirst()
{
  closeWings();
  set_servo_position(ARM, 84);
  delay(300);
  set_servo_position(ARM, ARMSTOP);
  openClaw();
  lowerClaw();
  closeClaw();
  raiseClaw();
  can = 0;
  Serial.write(GRAB_FIRST);
}

//SCORE CANS

void scoreCans()
{
  set_servo_position(ARM, 84);
  delay(300);
  set_servo_position(ARM, ARMSTOP);

  lowerClaw();

  openClawAll();

  openWings();

  raiseClaw();
  Serial.write(SCORE);
}



char isCan()    
{
  //if(digital(0) == 0 || can == 1)
  if(analog(IRPIN) <= IRTHRESH|| can == 1)
  {
    can = 0;
    return CAN;
  }     
  else
      return NOCAN;
}

int isButton()  
{
  if(get_sw1() || button == 1)
  {
    button = 0;
   return BUTTON;
    delay(300);
  }     
  else
      return NOBUTTON;
}


void openWings()
{
  set_servo_position(LEFTDOOR, LEFTOPEN);
  set_servo_position(RIGHTDOOR, RIGHTOPEN);
}



void closeWings()
{
  set_servo_position(LEFTDOOR, LEFTCLOSED);
  set_servo_position(RIGHTDOOR, RIGHTCLOSED);   
}


void reverse()
{
  setSpeed(0, -100);
  setSpeed(1, -100);
}