#include <Servo.h>

#include <WallVis.h>

WallVis node(
  "new001",     // Our ID
  "WallVisNet", //Wifi Access Point
  "wallvisAP",  //WiFi Password
  "192.168.4.1",//IP address of Node RED server
  1883          //Port for Node RED server
  );


// Set up the servo we will use
Servo s1 = Servo();
Servo s2 = Servo();
Servo s3 = Servo();
Servo s4 = Servo();


void setup()
{
  // Attach the servo to the right pin on the board
  s1.attach(D1);
  s2.attach(D2);
  s3.attach(D3);
  s4.attach(D4);

  s1.write(0);
  s2.write(0);
  s3.write(0);
  s4.write(0);

  //Get the serial port ready
  Serial.begin(115200);

  //Add in a behaviour that just goes to a certain angle, with the default name 'goto'
  node.add(new ServoGoto(s1,"goto1") );
  node.add(new ServoGoto(s2,"goto2") );
  node.add(new ServoGoto(s3,"goto3") );
  node.add(new ServoGoto(s4,"goto4") );

  //Add in a behaviour that rotates from 0 to 180 and back again (e.g. dropping a ball trustball style)
  node.add(new RotateReturn(s1,"drop1",1,500, 3, 175) );
  node.add(new RotateReturn(s2,"drop2",1,500, 3, 175) );
  node.add(new RotateReturn(s3,"drop3",1,500, 3, 175) );
  node.add(new RotateReturn(s4,"drop4",1,500, 3, 175) );
 
  //Initialise the whole infrastructure
  node.set_wifi(false);
  node.init();
}


void loop()
{
  //Should be all we have to do
  node.vis_loop();
}