/* =================================================================
 * NEURO-ROBO STUDIO — Sim-to-Real Autonomous Firmware
 * Generated from 2026 MaleCNS Fruit Fly Connectome Policy
 * Target: Arduino Uno / ESP32 + L298N Motor Driver + Dual IR/US Sensors
 * Model Repository: https://huggingface.co/hwihwalab/neuro-robo-connectome
 * Interactive Studio: https://huggingface.co/spaces/hwihwalab/neuro-robo-studio
 * ================================================================= */

#include <Arduino.h>

// Pin Configuration (L298N Dual H-Bridge Motor Driver)
const int ENA = 5;  // PWM Left Speed Enable (0-255)
const int ENB = 6;  // PWM Right Speed Enable (0-255)
const int IN1 = 7;  // Left Motor Forward Direction
const int IN2 = 8;  // Left Motor Backward Direction
const int IN3 = 9;  // Right Motor Forward Direction
const int IN4 = 10; // Right Motor Backward Direction

// Sensory Input Pins (Dual Analog Odor / Infrared Scent Receivers)
const int SENSOR_LEFT  = A0; // Left Antenna Scent / Obstacle Receptor (ORN_L)
const int SENSOR_RIGHT = A1; // Right Antenna Scent / Obstacle Receptor (ORN_R)

// 2026 MaleCNS Connectome Policy Constants (Tuned in Neuro-Robo Studio)
const float FORWARD_BASE = 160.0f; // Nominal forward driving speed
const float TURN_GAIN    = 1.25f;  // Descending (DN) turning gain coefficient

void setup() {
  Serial.begin(115200);
  
  // Initialize Motor Control Pins
  pinMode(ENA, OUTPUT);
  pinMode(ENB, OUTPUT);
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  
  Serial.println("[MaleCNS-2026] Neuromorphic Physical AI Firmware Loaded.");
}

void loop() {
  // 1. Read Simulated Biological Sensory Input (Antenna ORNs)
  int rawL = analogRead(SENSOR_LEFT);
  int rawR = analogRead(SENSOR_RIGHT);
  
  float smellL = rawL / 1023.0f;
  float smellR = rawR / 1023.0f;
  
  // 2. LIF Contrast Decoding (from 2026 MaleCNS ALPN -> DN Neural Circuit)
  float odor = smellL + smellR;
  float turn = 0.0f;
  float forward = 0.0f;
  
  if (odor > 0.02f) {
    // Bilateral contrast calculation (Antennal Lobe Projection Neurons)
    float contrast = (smellL - smellR) / max(0.02f, odor);
    turn = constrain(contrast * 5.0f * TURN_GAIN, -1.0f, 1.0f);
    forward = FORWARD_BASE * min(1.0f, odor * 1.5f);
  } else {
    // Searching pattern when scent concentration is below threshold
    forward = 80.0f;
    turn = 0.2f;
  }
  
  // 3. Differential Drive Output to Motor Neurons (MN)
  int pwmLeft  = constrain((int)(forward - turn * 80.0f), 0, 255);
  int pwmRight = constrain((int)(forward + turn * 80.0f), 0, 255);
  
  digitalWrite(IN1, HIGH); digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH); digitalWrite(IN4, LOW);
  analogWrite(ENA, pwmLeft);
  analogWrite(ENB, pwmRight);
  
  // Real-time Telemetry Stream
  Serial.print("ORN_L:"); Serial.print(smellL, 3);
  Serial.print(" | ORN_R:"); Serial.print(smellR, 3);
  Serial.print(" | Turn:"); Serial.print(turn, 2);
  Serial.print(" | PWM_L:"); Serial.print(pwmLeft);
  Serial.print(" | PWM_R:"); Serial.println(pwmRight);
  
  delay(20); // 50Hz Real-Time Biological Control Loop
}