int pos = 90;
int pas = 1;

/* Servomoteur */
#include <Servo.h>

Servo myservo;  // create servo object to control a servo
// twelve servo objects can be created on most boards

/* SONAR*/
int trig = 7;
int echo = 9;
long lecture_echo;
long cm;

float theta;
float x;
float y;
int c;
int l;
int num_case;

//Define Colors we need
#define BLACK   0x00
#define RED     0xE0

//Define the SPI Pin Numbers
#define SLAVESELECT 10//ss
#define DATAOUT     11//MOSI
#define DATAIN      12//MISO 
#define SPICLOCK    13//sck

const int num_matrix = 1; 
char color_buffer[64*num_matrix]; 

char blank[]={
  BLACK, BLACK, BLACK, BLACK, BLACK, BLACK, BLACK, BLACK,
  BLACK, BLACK, BLACK, BLACK, BLACK, BLACK, BLACK, BLACK,
  BLACK, BLACK, BLACK, BLACK, BLACK, BLACK, BLACK, BLACK,
  BLACK, BLACK, BLACK, BLACK, BLACK, BLACK, BLACK, BLACK,
  BLACK, BLACK, BLACK, BLACK, BLACK, BLACK, BLACK, BLACK,
  BLACK, BLACK, BLACK, BLACK, BLACK, BLACK, BLACK, BLACK,
  BLACK, BLACK, BLACK, BLACK, BLACK, BLACK, BLACK, BLACK,
  BLACK, BLACK, BLACK, BLACK, BLACK, BLACK, BLACK, BLACK
};

void set_matrix(){
  digitalWrite(SLAVESELECT, LOW); //Activate the RGB Matrix
  spi_transfer(0x25); //command character % == 0x25
  spi_transfer(0x02); //set LED matrix to position in daisy chain, must be between 0x01-0x08
  digitalWrite(SLAVESELECT, HIGH); //Deactivate the RGB Matrix
}

int p = 0; //dot position
int xMAX = 8;//max number column in 1 subset
int yMAX = 8;//max number row in 1 subset

void clear_buffer(){
  //start clearing array moving down rows
  for(int y=0; y<yMAX; y++){
    //clear array from the highest subset in daisy chained array
    for(int i=num_matrix; i>0; i--){
      //clear array from right to left
      for(int x=0; x<xMAX; x++){
        //current position that is being cleared
        p = (yMAX*y) + (x + (i-1)*(xMAX*yMAX));
        //clear by saving what is in the blank sub-array to your final array
        color_buffer[p] = blank[x];
      }
    }
  }
  matrix_write();//write array of colors to matrix
  delay(10);//allow some time for the final matrix to be seen  
}

void setup() {
  // SERVO
  myservo.attach(3);

  // SONAR
  pinMode(trig, OUTPUT);
  digitalWrite(trig, LOW);
  pinMode(echo, INPUT);

    //----------SPI BUS SETUP----------
  SPCR = (1<<SPE)|(1<<MSTR)|(1<<SPR1);  //Enable SPI HW, Master Mode, divide clock by 16

  //Set the pin modes for the RGB matrix
  pinMode(DATAOUT, OUTPUT);
  pinMode(DATAIN, INPUT);
  pinMode(SPICLOCK,OUTPUT);
  pinMode(SLAVESELECT,OUTPUT);

  //Make sure the RGB matrix is deactivated
  digitalWrite(SLAVESELECT,HIGH); 
  //----------END SPI BUS SETUP----------

  //use this to set a LED Matrix to position in daisy chain
  //set_matrix();

  //make sure buffer is cleared before writing to matrices
  clear_buffer();
  
  Serial.begin(9600);


}

void loop() {
  // put your main code here, to run repeatedly:
pos += pas;
if (pos>= 180){
  pas = -1;
}
if (pos <=0){
  pas = 1;
}
  
myservo.write(pos);
Serial.print("Position_degres : ");
Serial.println(pos);


// **** Capteur Ultrason ****
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  lecture_echo = pulseIn(echo, HIGH);
  cm = lecture_echo / 58;  
//** envoie sur la liaison serie la distance
  Serial.print("Distance_cm : ");
  Serial.println(cm);


/*
//  *** Gerer si on detecte un objet et allumer LED en fonction pos et cm **
  if (cm <3){
    theta = (pos/180)*pi;
    x = pos*cos(theta);
    y = pos*sin(theta);
    c = x+4;
    y = y;
    num_case = c + 8*l + 31;
    color_buffer[num_case]=RED;
  }
 */ 

  //** si on detecte une transmission
  delay(500); // pause de 500ms
  clear_buffer();


}