#!/usr/bin/env python3
# coding: utf-8

"""
Simuloidaan Outomerellä hyppivää väkkärää. Tämä simulaattori ei anna
mahdollisuutta koettaa ohjata väkkärää.

- Takaisinkytketty säätö pitää väkkärän pystyssä paikallaan
- Väkkärää voidaan ohjata optimointialgoritmin tuottamilla ohjauksilla

Koska optimointiin käytetään tarkalleen samaa malllia kuin simulointiin,
näemme animaation optimoinnin tuottamasta ratkaisusta.

Reaalimaailman ilmiöistä emme voi tehdä täsmälleen oikeaa mallia, mutta sitä
meidän ei tarvitse nyt murehtia.

Simulaattoriin voisi toki lisätä realismia, mutta se ei ole
tämän jutun pointti.
"""

from math import sqrt
import pygame
import json
# from feedback import initSt, liike
import par
import random
import initSim
from rk4 import rk4
from dxdt import fdxdt


Musta = (0, 0, 0)  # Mustassa ei ole mitään valoa
Valk = (255, 255, 255)  # valkoisessa on kaikenväristä valoa
Sin = (0, 0, 255)  # vain sinistä
Pun = (255, 0, 0)
Vihr = (0, 200, 50)
Kelt = (255, 245, 50)
Orange = (255, 200, 50)
Taivas = (60, 190, 255)
Pilvi_v = (150, 245, 255)
Meri = (0, 50, 150)
Hytti = (75, 125, 75)
Teksti_y = (10, 60, 0)
Teksti_a = (100, 150, 255)


# Tämän luokan olio kirjoittelee tekstit "maisemaan"
class ClGraph:
    def __init__(self):
        self.font_vakkara = pygame.font.SysFont('robotoslab', 50)
        self.TekstiLaatikot = []
        txt1 = "Loikkauta väkkärä paikasta A paikkaan B minimienergialla."
        txt2 = " Väkkärä liikkuu Newtonin mekaniikan lakien mukaan.  "
        self.tekstit_y = [
            (txt1, (0.02, 0.05)),
            (txt2, (0.02, 0.08)),
            ("Muotoile optimointitehtävä ja ratkaise se. ",
             (0.55, 0.08)),
            (" Tarkista lopputulos simuloimalla.",
             (0.55, 0.11)),
            ("hyppy: paina o", (0.25, 0.25))]

        txt1 = "Outomeren tiheys ja viskositeetti kasvavat"
        txt2 = " eksponentiaalisesti syvyyden funktiona"
        self.tekstit_a = [
            (txt1 + txt2, (0.02, 0.9)),
            ("Ohjaukset hyppyä varten lasketaan", (0.2, 0.75)),
            ("Pontryaginin maksimiperiaatteen avulla.", (0.2, 0.78)),
            ("Paikallaan väkkärän pitää pystyssä",
             (0.6, 0.75)),
            ("takaisinkytketty säätö",
             (0.6, 0.78)),
        ]
        self.tekstit()
        pygame.display.set_caption("Väkkärä Outomerellä")

    def tekstit(self):
        for ohje in self.tekstit_y:
            (teksti, (px, py)) = ohje
            rend_ohje = self.font_vakkara.render(
                teksti, True, Teksti_y)
            ix = int(px * Wxnaytto)
            iy = int(py * Wynaytto)
            self.TekstiLaatikot.append((rend_ohje, (ix, iy)))
        for ohje in self.tekstit_a:
            (teksti, (px, py)) = ohje
            rend_ohje = self.font_vakkara.render(teksti, True, Teksti_a)
            ix = int(px * Wxnaytto)
            iy = int(py * Wynaytto)
            self.TekstiLaatikot.append((rend_ohje, (ix, iy)))

    def blitTekstit(self):
        '''
        '''
        for laatikko in self.TekstiLaatikot:
            (teksti, paikka) = laatikko
            screen.blit(teksti, paikka)


# Varoitusvalo hytin katolle
class varoitus:
    def __init__(self):
        self.N = 0
        self.w1 = 0
        self.w2 = -1

    def blink(self):
        h0 = 85
        x0 = 50
        xa = pallot[0].x
        ya = pallot[0].y
        (x, y) = Kamera.xy_naytolla((xa, ya))
        self.N = self.N+1
        if self.N > 20:
            self.N = 0
            if self.w1 == 0:
                self.w1 = -1
                self.w2 = 0
            else:
                self.w1 = 0
                self.w2 = -1

        if Ohj.opti:
            pygame.draw.circle(screen, Pun, (x+x0, y-h0), 8, self.w1)
            pygame.draw.circle(screen, Pun, (x-x0, y-h0), 8, self.w2)
        else:
            pygame.draw.circle(screen, Sin, (x+x0, y-h0), 8, self.w1)
            pygame.draw.circle(screen, Sin, (x-x0, y-h0), 8, self.w2)


# Pelin ja simuloinnin ajoitus
class cl_Ajat:
    def __init__(self, Dt):
        self.Dt = Dt
        self.Nayttotaajuus = 24  # 1.0/self.Dt
        self.Dt0 = self.Dt


# Pelimaailman kuvaaminen näytölle peli-ikkunaan.
# Luokka Kamera periytyy toisesta ohjelmasta, jossa kuvakulmaa
# saattoi vaihtaa.
class Cl_Kamera:
    def __init__(self):
        self.Wx = 14.0  # 16.0
        self.Wx0 = -3.0  # -4.0
        self.Wy0 = -3.0
        self.Wy = kuvasuhde * self.Wx

# Lasketaan mitä pikseliä näytöllä vastaa pelimaailman piste.
# Oivallinen geometrian harjoitus miettiä, mihin tämä perustuu
    def xy_naytolla(self, xy):
        scale = Wxnaytto / self.Wx
        (x, y) = xy
        xpikseli = int((x - self.Wx0) * scale)
        ypikseli = int((-y + self.Wy + self.Wy0) * scale)
        return (xpikseli, ypikseli)

    def xy_skaalaus(self, w, h):
        scale = Wxnaytto / self.Wx
        xpikseli = int(w * scale)
        ypikseli = int(h * scale)
        return (xpikseli, ypikseli)

    def skaalaus(self, x):
        scale = Wxnaytto / self.Wx
        xpikseli = int(x * scale)
        return xpikseli


# kellukkeiden ja hytin tila
class Cl_pallo:
    def __init__(self, init_St):
        (x, y, vx, vy, vari) = init_St
        self.x = y
        self.y = y
        self.vx = vx
        self.vy = vy
        self.vari = vari


# Taivaan pilvet
class Pilvi:
    def __init__(self):
        xvasen = random.uniform(0.1, 0.9*Wxnaytto)
        ytop = random.uniform(0.05, 0.35*Wynaytto)
        self.w = random.uniform(0.04*Wxnaytto, 0.15*Wxnaytto)
        h = max(0.2*self.w,
                random.uniform(0.01*Wynaytto, 0.025*Wynaytto))
        self.dx = 0.0
        self.dy = 0.0
        self.coords = pygame.Rect(int(xvasen), int(ytop),
                                  int(self.w), int(h))

    def piirra(self, screen):
        self.dx = self.dx + random.uniform(-0.5, 1.5)
        self.dy = self.dy + random.uniform(-0.1, 0.1)
        if self.dx > 1.0 or self.dy > 1.0:
            self.coords = self.coords.move(int(self.dx), int(self.dy))
            self.dx = 0.0
            self.dy = 0.0
        pygame.draw.ellipse(screen, Pilvi_v, self.coords, 0)
        if self.coords.x + 0.2*self.w > Wxnaytto:
            self.coords.x = -0.8*self.w


# Käytetään tallennettua optimiohjausta
class Ohjaus:
    def __init__(self):
        with open('uopt.dat', 'r') as fil:
            self.uu_opt = json.load(fil)
        with open('xopt.dat', 'r') as fil:
            self.xs = json.load(fil)
        with open('yopt.dat', 'r') as fil:
            self.xy_opt = json.load(fil)
        self.maxotim = len(self.xs) - 1
        self.otim = -1
        self.opti = False
        self.max_F = self.F_max()

    def opt_uu(self):
        if (self.otim >= self.maxotim):
            self.opti = False
            Aika.Dt = Aika.Dt0
            return (self.uu_opt[self.maxotim], self.xy_opt[self.maxotim])
        else:
            itim = max(0, self.otim)
            Aika.Dt = self.xs[itim+1] - self.xs[itim]
            self.otim = self.otim + 1
            return (self.uu_opt[itim], self.xy_opt[itim])

    def F_max(self):
        F_max = -1000.0
        for FF in self.uu_opt:
            for F in FF:
                F_max = max(F_max, abs(F))
        return F_max


# Jalkojen välissä oleva "hydraulitunkki"
# a ja b, koska osat pitää piirtää tietyssä järjestyksessä,
# että ne jäävät oikealla tavalla toistensa taakse
# enkä jaksanut miettiä hienompaa ratkaisua

def connect_a(a, b, c, JalkaVari):
    (ax, ay) = a
    (bx, by) = b
    (cx, cy) = c

    x1 = int((2.0*ax+bx)/3.0)
    x2 = int((2.0*ax+cx)/3.0)
    y1 = int((2.0*ay+by)/3.0)
    y2 = int((2.0*ay+cy)/3.0)

    pygame.draw.line(screen, JalkaVari, (x1, y1), (x2, y2), 10)
    pygame.draw.circle(screen, Musta, (x1, y1), 12, 0)


def connect_b(a, b, c, JalkaVari):
    (ax, ay) = a
    (bx, by) = b
    (cx, cy) = c

    x2 = int((2.0*ax+cx)/3.0)
    y2 = int((2.0*ay+cy)/3.0)

    pygame.draw.circle(screen, Musta, (x2, y2), 12, 0)


# Jalan väri kertoo, kuinka suurella voimalla sitä työnnetään
# pidemmäksi tai vedetään kasaan.
def jalka_vari(F):
    R = 0
    G = 0
    B = 0
    F = 1.2*F
    if F < 0:
        R = min(255, int(-F/Ohj.max_F*255.0))
    else:
        B = min(255, int(F/Ohj.max_F*255.0))
    return (R, G, B)


def piirraVakkara(pallot, FF):
    [A, B, C] = pallot
    [Fab, Fac, Fbc] = FF
    axy = Kamera.xy_naytolla((A.x, A.y))
    bxy = Kamera.xy_naytolla((B.x, B.y))
    cxy = Kamera.xy_naytolla((C.x, C.y))

    JalkaVari = jalka_vari(Fab)
    pygame.draw.line(screen, JalkaVari, axy, bxy, 10)
    connect_a(axy, bxy, cxy, jalka_vari(Fbc))

    (bxi, byi) = bxy
    eh = 90
    ew = 0.7*eh
    pygame.draw.rect(
        screen, B.vari,
        ((bxi-int(ew/2), byi-int(eh/2)), (ew, eh)), 0, border_radius=30)

    (axi, ayi) = axy
    ayh = ayi - 35
    pygame.draw.rect(screen, Hytti,
                     ((axi-75, ayh-45), (160, 90)), 0, border_radius=30)
    pygame.draw.rect(screen, Taivas,
                     ((axi+45, ayh-15), (45, 20)), 0, border_radius=4)
    pygame.draw.circle(screen, Musta, (axi+65, ayh-3), 8, 0)

    JalkaVari = jalka_vari(Fac)
    pygame.draw.line(screen, JalkaVari, axy, cxy, 10)
    connect_b(axy, bxy, cxy, jalka_vari(Fbc))

    ew_m = 1.0*eh
    eh_m = 0.9*ew
    pygame.draw.ellipse(
        screen, Hytti, ((axi-int(ew_m/2), ayi-int(eh_m/2)), (ew_m, eh_m)), 0)

    (cxi, cyi) = cxy
    pygame.draw.rect(
        screen, B.vari,
        ((cxi-int(ew/2), cyi-int(eh/2)), (ew, eh)), 0, border_radius=30)


def maisema():
    w = Kamera.skaalaus(Kamera.Wx)
    h = Kamera.skaalaus(Kamera.Wy+Kamera.Wy0)
    dim = (w, h)
    pygame.draw.rect(screen, Taivas, ((0, 0), dim), 0)

    (p0x, p0y) = Kamera.xy_naytolla((Kamera.Wx0, 0.2))  # -initSim.h_0
    w = Kamera.skaalaus(Kamera.Wx)
    h = Kamera.skaalaus(5)
    dim = (w, h)
    pygame.draw.rect(screen, Meri, ((p0x, p0y), dim), 0)

    wl = int(Wxnaytto/100)
    for i in range(60):
        ix = int(1.9*wl*i)
        iy = p0y-wl/2.0
        pygame.draw.circle(screen, Taivas, (ix, iy), wl, 0)


def init_pallot(pallot):
    pallot[0].x = initSim.ax_0
    pallot[0].y = initSim.ay_0
    pallot[0].vx = initSim.avx_0
    pallot[0].vy = initSim.avy_0
    pallot[1].x = initSim.bx_0
    pallot[1].y = initSim.by_0
    pallot[1].vx = initSim.bvx_0
    pallot[1].vy = initSim.bvy_0
    pallot[2].x = initSim.cx_0
    pallot[2].y = initSim.cy_0
    pallot[2].vx = initSim.cvx_0
    pallot[2].vy = initSim.cvy_0


# Takaisinkytketty tilasäätäjä.
# Ohjausvoima on verannollinen kellukkeiden ja hytin poikkeamalle
# halutusta tilasta.
def feedbck(pallot):
    ha = par.s_0
    Fd = par.C_fb*(pallot[0].y - ha)
    Fx = 4*par.C_fb*(pallot[0].x-(pallot[1].x+pallot[2].x)/2.0)
    F1 = par.C_fb*(pallot[0].y - (ha + pallot[1].y)) + \
        Fd + Fx - par.m_1*par.g/2
    F2 = par.C_fb*(pallot[0].y - (ha + pallot[2].y)
                   ) + Fd - Fx - par.m_1*par.g/2
    F3 = 4*par.C_fb*(pallot[2].x - (pallot[1].x + par.l_0)) + par.m_1*par.g/4
    return [F1, F2, F3]


def dist(a, b):
    return sqrt((a.x - b.x)**2 + (a.y - b.y)**2)


def initSt():
    return [
        (initSim.ax_0, initSim.ay_0, initSim.avx_0, initSim.avy_0, Hytti),
        (initSim.bx_0, initSim.by_0, initSim.bvx_0, initSim.bvy_0, Vihr),
        (initSim.cx_0, initSim.cy_0, initSim.cvx_0, initSim.cvy_0, Vihr)
    ]


# Lastekaan paljonko kellukkeet ja hytti liikkuvat ajassa Dt
def liike(pallot, Dt, Ohj):
    # [va_x, va_y, vb_x, vb_y, vc_x, vc_y, a_x, a_y, b_x, b_y, c_x, c_y]
    yy = [pallot[0].vx, pallot[0].vy, pallot[1].vx, pallot[1].vy,
          pallot[2].vx, pallot[2].vy, pallot[0].x, pallot[0].y,
          pallot[1].x, pallot[1].y, pallot[2].x, pallot[2].y]

    # Käytetään optimointialgoritmin antamaa ohjausta, jos
    # 'opti' päällä, muuten käytetään takaisinkytkettyä säätöä
    if Ohj.opti:
        (uu, yy) = Ohj.opt_uu()
    else:
        uu = feedbck(pallot)
    # runge-kutta integrointialgoritmi laskee uudet paikat ja nopeudet
    yy = rk4(fdxdt, yy, uu, Dt)

    [pallot[0].vx, pallot[0].vy, pallot[1].vx, pallot[1].vy,
     pallot[2].vx, pallot[2].vy, pallot[0].x, pallot[0].y,
     pallot[1].x, pallot[1].y, pallot[2].x, pallot[2].y] = yy
    # palautetaan ohjaukset, että saadaan piirrettyä jalat
    # värillä, joka kuvaa käytettyä voimaa
    return uu

# ====================================================================
#
# Pääohjelma alkaa tästä
# ====================================================================


Ohj = Ohjaus()
Aika = cl_Ajat(par.Tf/Ohj.maxotim)

pygame.display.init()

# Selvitetään peli-ikkunan koko näytöllä pikseleinä
D_Info = pygame.display.Info()
Wxnaytto = D_Info.current_w
Wynaytto = D_Info.current_h
kuvasuhde = Wynaytto / Wxnaytto

screen = pygame.display.set_mode((Wxnaytto, Wynaytto))
pygame.display.set_caption("vakkara")
pygame.init()

esittelyt = ClGraph()

clock = pygame.time.Clock()

Kamera = Cl_Kamera()

pilvet = []
for i in range(30):
    pilvet.append(Pilvi())

iniSt = initSt()

pallot = [Cl_pallo(iniSt[i]) for i in range(3)]
init_pallot(pallot)

vilkku = varoitus()

loppu = False
Saato = False

while not loppu:
    screen.fill(Musta)  # pyyhitään peli-ikkuna tyhjäksi
    for event in pygame.event.get():
        # Lopetetaan peli sulkemalla ikkuna tai painamalla ESC
        if event.type == pygame.QUIT:
            loppu = True
        if event.type == pygame.KEYDOWN:
            if event.key == pygame.K_o:
                Saato = False
                Ohj.opti = True
            if event.key == pygame.K_ESCAPE:
                loppu = True

    FF = liike(pallot, Aika.Dt, Ohj)
    maisema()
    for pilvi in pilvet:
        pilvi.piirra(screen)
    piirraVakkara(pallot, FF)
    vilkku.blink()
    esittelyt.blitTekstit()
    pygame.display.flip()
    clock.tick(Aika.Nayttotaajuus)

pygame.quit()