#include "arduino.h"
#include <Wire_slave.h>
#include <EEPROM.h>
#include <Servo.h>
#include <HardwareTimer.h>
#include <STM32ADC.h>

STM32ADC myADC(ADC1);
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;
Servo servo5;
Servo servo6;
Servo servo7;
Servo servo8;

uint8_t dataone = 0xff;
uint8_t datas[3] = {0xff};
uint16_t datapinacd[10];
int adcpin[10] = {PA0, PA1, PA2, PA3, PA4, PA5, PA6, PA7, PB0, PB1};
int servopin[8] = {PA8, PA9, PA10, PA11, PB6, PB7, PB8, PB9};
uint16_t servodata;
void setuppin()
{
  for (int i = 0; i < 10; i++) {

    pinMode(adcpin[i], INPUT_ANALOG);
  }
}
void i2c_slave_write_handler(int numBytes)
{
  digitalWrite(PC13, !digitalRead(PC13));
  if (numBytes <= 1) {
    for (int i = 0; i < numBytes; i++)
      dataone =  Wire1.read();
  } else {
    for (int i = 0; i < numBytes; i++)
      datas[i] =  Wire1.read();
  }
  switch (datas[0]) {
    case 0x30:
      servodata = word(datas[1], datas[2]);
      servo1.writeMicroseconds(servodata);
      break;
    case 0x31:
      servodata = word(datas[1], datas[2]);
      servo2.writeMicroseconds(servodata);
      break;
    case 0x32:
      servodata = word(datas[1], datas[2]);
      servo3.writeMicroseconds(servodata);
      break;
    case 0x33:
      servodata = word(datas[1], datas[2]);
      servo4.writeMicroseconds(servodata);
      break;
    case 0x34:
      servodata = word(datas[1], datas[2]);
      servo5.writeMicroseconds(servodata);
      break;
    case 0x35:
      servodata = word(datas[1], datas[2]);
      servo6.writeMicroseconds(servodata);
      break;
    case 0x36:
      servodata = word(datas[1], datas[2]);
      servo7.writeMicroseconds(servodata);
      break;
    case 0x37:
      servodata = word(datas[1], datas[2]);
      servo8.writeMicroseconds(servodata);
      break;
    default:
      ;
      break;
  }

}

void i2c_slave_read_handler()
{
  digitalWrite(PC13, !digitalRead(PC13));
  uint8_t val[2];
  switch (dataone) {
    case 0x10:
      val[0] = datapinacd[0] >> 8;
      val[1] = datapinacd[0];
      Wire1.write(val, 2);
      break;
    case 0x11:
      val[0] = datapinacd[1] >> 8;
      val[1] = datapinacd[1];
      Wire1.write(val, 2);
      break;
    case 0x12:
      val[0] = datapinacd[2] >> 8;
      val[1] = datapinacd[2];
      Wire1.write(val, 2);
      break;
    case 0x13:
      val[0] = datapinacd[3] >> 8;
      val[1] = datapinacd[3];
      Wire1.write(val, 2);
      break;
    case 0x14:
      val[0] = datapinacd[4] >> 8;
      val[1] = datapinacd[4];
      Wire1.write(val, 2);
      break;
    case 0x15:
      val[0] = datapinacd[5] >> 8;
      val[1] = datapinacd[5];
      Wire1.write(val, 2);
      break;
    case 0x16:
      val[0] = datapinacd[6] >> 8;
      val[1] = datapinacd[6];
      Wire1.write(val, 2);
      break;
    case 0x17:
      val[0] = datapinacd[7] >> 8;
      val[1] = datapinacd[7];
      Wire1.write(val, 2);
      break;
    case 0x18:
      val[0] = datapinacd[8] >> 8;
      val[1] = datapinacd[8];
      Wire1.write(val, 2);
      break;
    case 0x19:
      val[0] = datapinacd[9] >> 8;
      val[1] = datapinacd[9];
      Wire1.write(val, 2);
      break;
    default:
      val[0] = 0xFF;
      val[1] = 0xFF;
      Wire1.write(val, 2);
      break;
  }

}

void read_and_write()
{
  for (int i = 0; i < 10; i++) {
    datapinacd[i] = analogRead(adcpin[i]);
  }
}

void setup()
{
  Serial.begin(115200);
  pinMode(PC13, OUTPUT);
  setuppin();

  Wire1.begin(0x43);
  Wire1.onReceive(i2c_slave_write_handler);
  Wire1.onRequest(i2c_slave_read_handler);

  delay(1000);

  myADC.setSampleRate(ADC_SMPR_1_5);
  servo1.attach(servopin[0]);
  servo2.attach(servopin[1]);
  servo3.attach(servopin[2]);
  servo4.attach(servopin[3]);
  servo5.attach(servopin[4]);
  servo6.attach(servopin[5]);
  servo7.attach(servopin[6]);
  servo8.attach(servopin[7]);
  delay(1000);
}
uint64_t previousMillis = 0;
void loop()
{
  if (millis() - previousMillis > 1) {
    previousMillis = millis();
    read_and_write();
  }
}