//sketch_3D_MyJet_MPU6050_V3
import processing.serial.*;
Serial myPort;        // The serial port
float My=0;
float Mz=0;
float Mx=0;
float Py=0;
float Pz=0;
float Px=0;
 void setup ()
 {
   size(1000, 800, P3D);
   println(Serial.list());
   myPort = new Serial(this, Serial.list()[1], 38400);
   myPort.bufferUntil('\n');   
   myPort.write("B"); 
 }
 
 void draw ()
 {
   background(0, 256, 256);
   lights();
   noStroke();
   fill(240, 240, 230);
   translate(width / 2, height / 2);
   translate(Px,Pz,Py);
   rotateX(Mx);
   rotateY(Mz);
   rotateZ(My);
  
   OB_A2();//Z軸回転体（胴体）
   OB_A3();//Z軸回転体(胴体先端）
   OB2();//主翼
   OB3();//水平尾翼
   OB4();//垂直尾翼
   OB_A4();//Z軸回転体(エンジン）
 }
 
void serialEvent(Serial myPort)
{
  float  r=0.2,Mxn,Myn;
  String myString = myPort.readStringUntil('\n');
  myString = trim(myString);
  int sensors[] = int(split(myString, ','));
  for (int sensorNum = 0; sensorNum < sensors.length; sensorNum++) 
  {
     print(sensors[sensorNum] + "\t");
  }
  println();
  Mxn=1.0*atan2(sensors[1],sensors[2]);//重力方向
  Myn=1.0*atan2(sensors[0],sensors[2]);//重力方向
    
  if(abs(Mxn) < 3.2 && abs(Myn) < 3.2)
  {
    if(sensors[6]==1)
    {
      Mx=Mx*(1-r)+r*Mxn;//重力方向
      My=My*(1-r)+r*Myn;//重力方向
      Mz=sensors[5]/100000.0;
    }
    else
    {
      Px=Px*(1-r)+400*r*Mxn;//重力方向
      Pz=Py*(1-r)+1000*r*Myn;//重力方向
      Py=sensors[5]/400.0;
    }   
  }
  myPort.write("A"); 
}
 
 void OB_A4()//Z軸回転体(エンジン）
{
  int sides=16;//分割数
  int Sn=6;//面数
  float Pz[]={0,-1,-50,-60,-70,-70};//z座標
  float R[]={15,20,20,15,15,1};//回転物半径
 
  int i,j;
  float angleIncrement = TWO_PI/sides;
  pushMatrix();
  translate(60, 36.3,-145);
  for(j=0;j<Sn-1;j++)
  {
    float angle = 0;
    beginShape(QUAD_STRIP);
    for (i = 0; i < sides + 1; ++i)
    {
      vertex(R[j]*cos(angle), R[j]*sin(angle), Pz[j]);
      vertex(R[j+1]*cos(angle), R[j+1]*sin(angle), Pz[j+1]);
      angle += angleIncrement;
    }
    endShape(CLOSE);
  }
  popMatrix();
  
  pushMatrix();
  translate(-60, 36.3,-145);
  for(j=0;j<Sn-1;j++)
  {
    float angle = 0;
    beginShape(QUAD_STRIP);
    for (i = 0; i < sides + 1; ++i)
    {
      vertex(R[j]*cos(angle), R[j]*sin(angle), Pz[j]);
      vertex(R[j+1]*cos(angle), R[j+1]*sin(angle), Pz[j+1]);
      angle += angleIncrement;
    }
    endShape(CLOSE);
  }
  popMatrix();
}

void OB4()//垂直尾翼
{
  float Px[]={0,0,3,-3,0,0,2,-2};//ポイントx
  float Py[]={0,0,0,0,-80,-80,-80,-80};//ポイントy
  float Pz[]={25,-25,0,0,0,-25,-15,-15};//ポイントz
  int Sn=10;//面数
  int S1[]={0,0,1,1,0,0,1,1,4,5};//面ポイント1
  int S2[]={2,4,2,5,3,4,3,5,6,6};//面ポイント2
  int S3[]={6,6,5,6,7,7,7,7,7,7};//面ポイント3
  int i;
  
  pushMatrix();
  translate(0, 0,-395);
  for(i=0;i<Sn;i++)
  {
    beginShape();
    vertex(Px[S1[i]], Py[S1[i]],Pz[S1[i]]);
    vertex(Px[S2[i]], Py[S2[i]],Pz[S2[i]]);
    vertex(Px[S3[i]], Py[S3[i]],Pz[S3[i]]);
    endShape(CLOSE);
  }
  popMatrix();
}

void OB3()//水平尾翼
{
  float Px[]={0,0,0,100,100,100};//ポイントx
  float Py[]={3,3,-3,2,2,-2};//ポイントy
  float Pz[]={15,-15,0,-5,-15,-10};//ポイントz
  int Sn=7;//面数
  int S1[]={0,0,0,0,1,1,3};//面ポイント1
  int S2[]={1,3,2,3,2,4,4};//面ポイント2
  int S3[]={4,4,5,5,5,5,5};//面ポイント3
  int i;
  
  pushMatrix();
  translate(0, 0,-415);
  for(i=0;i<Sn;i++)
  {
    beginShape();
    vertex(Px[S1[i]], Py[S1[i]],Pz[S1[i]]);
    vertex(Px[S2[i]], Py[S2[i]],Pz[S2[i]]);
    vertex(Px[S3[i]], Py[S3[i]],Pz[S3[i]]);
    endShape(CLOSE);
  }
  for(i=0;i<Sn;i++)
  {
    beginShape();
    vertex(-Px[S1[i]], Py[S1[i]],Pz[S1[i]]);
    vertex(-Px[S2[i]], Py[S2[i]],Pz[S2[i]]);
    vertex(-Px[S3[i]], Py[S3[i]],Pz[S3[i]]);
    endShape(CLOSE);
  }
  popMatrix();
}

void OB2()//主翼
{
  float Px[]={10,10,10,220,220,220};//ポイントx
  float Py[]={20,20,10,15,15,10};//ポイントy
  float Pz[]={40,-40,0,10,-10,0};//ポイントz
  int Sn=7;//面数
  int S1[]={0,0,0,0,1,1,3};//面ポイント1
  int S2[]={1,3,2,3,2,4,4};//面ポイント2
  int S3[]={4,4,5,5,5,5,5};//面ポイント3
  int i;
  
  pushMatrix();
  translate(0, 0,-200);
  for(i=0;i<Sn;i++)
  {
    beginShape();
    vertex(Px[S1[i]], Py[S1[i]],Pz[S1[i]]);
    vertex(Px[S2[i]], Py[S2[i]],Pz[S2[i]]);
    vertex(Px[S3[i]], Py[S3[i]],Pz[S3[i]]);
    endShape(CLOSE);
  }
  for(i=0;i<Sn;i++)
  {
    beginShape();
    vertex(-Px[S1[i]], Py[S1[i]],Pz[S1[i]]);
    vertex(-Px[S2[i]], Py[S2[i]],Pz[S2[i]]);
    vertex(-Px[S3[i]], Py[S3[i]],Pz[S3[i]]);
    endShape(CLOSE);
  }
  popMatrix();
}

void OB_A3()//Z軸回転体(胴体先端）
{
  int sides=16;//分割数
  int Sn=8;//面数
  float Pz[]={0,-3.2,-11.7,-21.8,-31.9,-45.3,-70.2,-129.4};//z座標
  float R[]={0.1,3.7,7.4,9.6,11.8,13.8,17.4,17.7};//回転物半径
  
  int i,j;
  float angleIncrement = TWO_PI/sides;
  pushMatrix();
  translate(0, 8.5);
  for(j=0;j<Sn-1;j++)
  {
    float angle = 0;
    beginShape(QUAD_STRIP);
    for (i = 0; i < sides + 1; ++i)
    {
      vertex(R[j]*cos(angle), R[j]*sin(angle), Pz[j]);
      vertex(R[j+1]*cos(angle), R[j+1]*sin(angle), Pz[j+1]);
      angle += angleIncrement;
    }
    endShape(CLOSE);
  }
  popMatrix();
}

void OB_A2()//Z軸回転体（胴体）
{
  int sides=16;//分割数
  int Sn=13;//面数
  float Pz[]={-40,-46.9,-68.4,-88.2,-108.8,-129.4,-319.4,-346.2,-373,-391.2,-409.4,-433,-440};//z座標
  float R[]={0.1,7.8,15.4,19.5,22.3,25,25,22.3,18.7,16,13.3,8.3,0.1};//回転物半径
 
  int i,j;
  float angleIncrement = TWO_PI/sides;
  for(j=0;j<Sn-1;j++)
  {
    float angle = 0;
    beginShape(QUAD_STRIP);
    for (i = 0; i < sides + 1; ++i)
    {
      fill(240, 240, 230);
      if(j==1){fill(20, 20, 40);}
      if(j==5 && i==0){fill(20, 20, 40);}
      if(j==5 && i==8){fill(20, 20, 40);}
      if(j==5 && i==16){fill(20, 20, 40);}
      vertex(R[j]*cos(angle), R[j]*sin(angle), Pz[j]);
      vertex(R[j+1]*cos(angle), R[j+1]*sin(angle), Pz[j+1]);
      angle += angleIncrement;
    }
    endShape(CLOSE);
  }
}