//  Nilheim Mechatronics Simplified Eye Mechanism Code
//  Make sure you have the Adafruit servo driver library installed >>>>> https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library
//  X-axis joystick pin: A1
//  Y-axis joystick pin: A0
//  Trim potentiometer pin: A2
//  Button pin: 2

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

#define SERVOMIN  140 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX  520 // this is the 'maximum' pulse length count (out of 4096)
#define CE_PIN 10
#define CSN_PIN 9

#include <avr/sleep.h>
#include <Adafruit_NeoPixel.h>
#define NUM_LEDS 1
Adafruit_NeoPixel pixels = Adafruit_NeoPixel(NUM_LEDS, 6, NEO_GRB + NEO_KHZ800);
Adafruit_NeoPixel fuck = Adafruit_NeoPixel(NUM_LEDS, 7, NEO_GRB + NEO_KHZ800);

int delayval = 500; // delay for half a second


uint8_t servonum = 0;
RF24 radio(CE_PIN, CSN_PIN);
const uint64_t pipe = 0x1;  
int controllerData[10];

int xval;
int yval;

int lexpulse;
int rexpulse;

int leypulse;
int reypulse;

int uplidpulse;
int lolidpulse;

int trimval;

const int analogInPin = A0;
int sensorValue = 0;
int outputValue = 0;
int switchval = 0;

int green;
int red;
int blue;

void setup() {
  Serial.begin(9600);
  Serial.println("8 channel Servo test!");
  pinMode(analogInPin, INPUT);
  pinMode(2, INPUT);
  Serial.println("Nrf24L01 Receiver Starting");
  radio.begin();
  radio.openReadingPipe(1,pipe);
  radio.startListening();
  pwm.begin();
  
  pwm.setPWMFreq(60);  // Analog servos run at ~60 Hz updates

pixels.begin(); // This initializes the NeoPixel library. 
pixels.show(); 
fuck.begin(); // This initializes the NeoPixel library. 
fuck.show(); 


  delay(10);
}

// you can use this function if you'd like to set the pulse length in seconds
// e.g. setServoPulse(0, 0.001) is a ~1 millisecond pulse width. its not precise!
void setServoPulse(uint8_t n, double pulse) {
  double pulselength;
  
  pulselength = 1000000;   // 1,000,000 us per second
  pulselength /= 60;   // 60 Hz
  Serial.print(pulselength); Serial.println(" us per period"); 
  pulselength /= 4096;  // 12 bits of resolution
  Serial.print(pulselength); Serial.println(" us per bit"); 
  pulse *= 1000000;  // convert to us
  pulse /= pulselength;
  Serial.println(pulse);
 // pwm.setPWM(n, 0, pulse);
}

void loop() {

      radio.read( &controllerData, sizeof(controllerData));
    while (controllerData[0] == 0 || controllerData[0] > 2024) {
        radio.read( &controllerData, sizeof(controllerData));
    }
      Serial.print(controllerData[0]);
      Serial.print("[0] ");
      Serial.print(controllerData[1]);
      Serial.print("[1] ");
      Serial.print(controllerData[2]);
      Serial.print("[2] ");
      Serial.print(controllerData[3]);
      Serial.print("[3] ");
      //Serial.print(controllerData[4]);
      delay(5);

  xval = controllerData[1];
    lexpulse = map(xval, 1023,1, 200, 500);
    rexpulse = lexpulse;

    switchval = controllerData[3];
    
    
  yval = controllerData[0];
    leypulse = map(yval, 1023,1, 500, 200);
    reypulse = map(yval, 1023,1, 200, 500);
  



  // trimval = controllerData[2];
  //   trimval=map(trimval, 320, 580, -40, 40);
  //     uplidpulse = map(yval, 0, 1023, 280, 420);
  //       uplidpulse += (trimval-40);
  //         uplidpulse = constrain(uplidpulse, 280, 400);
  //     lolidpulse = map(yval, 0, 1023, 410, 280);
  //       lolidpulse += (trimval/2);
  //         lolidpulse = constrain(lolidpulse, 280, 400);      
    

    
      pwm.setPWM(0, 0, lexpulse);
      pwm.setPWM(1, 0, leypulse);
      pwm.setPWM(2, 0, rexpulse);
      pwm.setPWM(3, 0, reypulse); 

      if (switchval == HIGH) {
      pwm.setPWM(4, 0, 240);
      pwm.setPWM(5, 0, 240);
      }
      else if (switchval == LOW) {
      pwm.setPWM(4, 0, uplidpulse);
      pwm.setPWM(5, 0, lolidpulse);
      }
      
      


          Serial.println(lexpulse);
   
  delay(5);


if (controllerData[3]==0){
  green=controllerData[2]/4;
}
else{
  red=controllerData[2]/4;
}
pixels.setPixelColor(0, pixels.Color(green,red,255 ));
fuck.setPixelColor(0, pixels.Color(green,red,255 ));
pixels.show();
fuck.show();

}