//libraries:
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
#include <RPC.h>
//pins:
const int enablea = 24;
const int enableb = 25;
const int led_power = 26;
const int red = 4;
const int blue = 3;
const int green = 5;
const int motorrf = 10;
const int motorlf = 9;
const int motorrb = 6;
const int motorlb = 2;
//vars:
int greenv =0;
int redv = 100;
int bluev = 0;
//nrf:
const byte address[6] = "00001";
RF24 radio(7, 8);  //   CE, CSN
void setup() {
  // put your setup code here, to run once:
if (RPC.cpu_id() == CM7_CPUID){
pinMode(motorrf, OUTPUT);
pinMode(motorlf, OUTPUT);
pinMode(motorrb, OUTPUT);
pinMode(motorlb, OUTPUT);
pinMode(enablea, OUTPUT);
pinMode(enableb, OUTPUT);
Serial.begin(9600);
SPI1.begin();
radio.begin(&SPI1);
radio.openReadingPipe(0,  address);
radio.startListening();
delay(1000);
digitalWrite(enablea, HIGH);
digitalWrite(enableb, HIGH);
Serial.println("M7 boot");
}else if(RPC.cpu_id() == CM4_CPUID){
pinMode(red, OUTPUT);
pinMode(green, OUTPUT);
pinMode(blue, OUTPUT);
pinMode(led_power, OUTPUT);
digitalWrite(led_power, HIGH);
analogWrite(blue, bluev);
analogWrite(green, greenv);
analogWrite(red, redv);
Serial.begin(9600);
delay(1000);
Serial.println("M4 boot");
}


}


void loop() {
if (RPC.cpu_id() == CM7_CPUID) {
// --- M7: Robot control ---
radioread(); // check NRF commands
led(0, 255, 255, 0, 0);
// Example: move forward for testing

} else if(RPC.cpu_id() == CM4_CPUID) {
    // --- M4: RGB fade demo ---
led(255, 0, 255, 0, 0);
}
}
void led(int r, int g, int b, int fade, int delayTime){
if(fade == 1){
int stepR = (r > redv) ? 1 : -1;
int stepG = (g > greenv) ? 1 : -1;
int stepB = (b > bluev) ? 1 : -1;
while(redv != r || greenv != g || bluev != b){
if(redv != r) redv += stepR;
if(greenv != g) greenv += stepG;
if(bluev != b) bluev += stepB;
analogWrite(red, redv);
analogWrite(green, greenv);
analogWrite(blue, bluev);
delay(delayTime);
}
}else {
redv = r;
greenv = g;
bluev = b;
analogWrite(red, redv);
analogWrite(green, greenv);
analogWrite(blue, bluev);
delay(delayTime);
}
}

void right(){//right();
Serial.println("right");
digitalWrite(motorrb, HIGH);
digitalWrite(motorlf, HIGH);
digitalWrite(motorrf, LOW);
digitalWrite(motorlb, LOW);
}
void left(){//left();
Serial.println("left");
digitalWrite(motorrf, HIGH);
digitalWrite(motorlb, HIGH);
digitalWrite(motorrb, LOW);
digitalWrite(motorlf, LOW);
}
void forward(){//forward();
Serial.println("forward");
digitalWrite(motorrf, HIGH);
digitalWrite(motorlf, HIGH);
digitalWrite(motorrb, LOW);
digitalWrite(motorlb, LOW);
}
void backward(){//backward();
Serial.println("backward");
digitalWrite(motorrb, HIGH);
digitalWrite(motorlb, HIGH);
digitalWrite(motorrf, LOW);
digitalWrite(motorlf, LOW);
}
void leftf(int v){
Serial.println("left forward");
analogWrite(motorlf, v);
digitalWrite(motorrf, HIGH);
digitalWrite(motorrb, LOW);
digitalWrite(motorlb, LOW);
}
void rightf(int v){
Serial.println("right forward");
analogWrite(motorrf, v);
digitalWrite(motorlf, HIGH);
digitalWrite(motorrb, LOW);
digitalWrite(motorlb, LOW);
}
void leftb(int v){
Serial.println("left backward");
analogWrite(motorlb, v);
digitalWrite(motorrb, HIGH);;
digitalWrite(motorrf, LOW);
digitalWrite(motorlf, LOW);
}
void rightb(int v){
Serial.println("right backward");
digitalWrite(motorrf, LOW);
digitalWrite(motorlf, LOW);
analogWrite(motorrb, v);
digitalWrite(motorlb, HIGH);
}
void allstop(){
digitalWrite(motorrf, LOW);
digitalWrite(motorlf, LOW);
digitalWrite(motorrb, LOW);
digitalWrite(motorlb, LOW);
}
void radioread(){
if (radio.available()){
char text[32] = {0};
radio.read(&text,  sizeof(text));
Serial.println(text);
}
}
void robotmovecheck(){

}